Renjian Hao
Dependencies: L3G4200D L3GD20 LSM303DLHC LSM303DLM PwmIn Servo mbed
Fork of Fish_2014Fall by
main.cpp
- Committer:
- RenjianHao
- Date:
- 2015-08-13
- Revision:
- 5:a5f0395d2fa4
- Parent:
- 4:c93e1ecd3359
File content as of revision 5:a5f0395d2fa4:
#include "mbed.h" #include "L3GD20.h" #include "LSM303DLHC.h" #include "Servo.h" #include "math.h" #include "PwmIn.h" #include "iSerial.h" #include "iostream" #include "stdio.h" #include "string.h" #include <string> #include <sstream> #define TS 10 // sample time [ms] #define SIMULATION_TIME 10 // simulation time [s] #define Left 1 #define Right 0 #define Lost 555 //serial Serial BB(p13, p14); // tx, rx Serial pc(USBTX,USBRX); //period_pwm setup as 20ms //speed: 0 is off, and 1 is full speed;for servo 0-0.2 duty circle //direction: 0 clockwise, 1 counter-clockwise float period_pwm_ac; // Actuator current float period_pwm; bool directionA; bool directionB; bool directionC; bool directionE; bool Target; bool direction; float speedA; float speedB; float speedC; float Tail_increasement=0.0; float AngleLeft=0.08; float AngleRight=0.04; float AngleTail=0.08; float Tailbias=0.0; float AngleTail_stand_bias=0.0; float dirE=1.0; float bias=0.0; long int FA=1000000,FB=1000000,FE=40,CountA=0,CountB=0,CountE=0,CountT=0; int flag; int x,y; // Image coordinate //funtion declaration void move(int motor, float speed, int direction); PwmOut PWMA(p25);//LEFT FIN PwmOut PWMB(p23);//RIGHT FIN PwmOut PWMC(p24);//Left servo PwmOut PWMD(p26);//Right servo PwmOut PWME(p21);//Tail DigitalOut STBY (p30); //DigitalOut STBY2 (p29); DigitalOut AIN1 (p8); DigitalOut AIN2 (p11); // Fin1 left DigitalOut BIN1 (p7); DigitalOut BIN2 (p6); // Fin2 right DigitalOut myLed (LED1); DigitalOut myLed1(LED2); void moveA() { STBY=1; //disable standby int inPin1=1; int inPin2=0; if(directionA==0) { inPin1 = 0; inPin2 = 1; } AIN1=inPin1; AIN2=inPin2; //directionA=!directionA; //pc.printf("dirA = %d\n\r",directionA); if(directionA) { //speedA=0.4; PWMA.pulsewidth(period_pwm*(speedA*1)); //PWMA.pulsewidth(period_pwm*(0.6)); } else { //speedA=0.2; PWMA.pulsewidth(period_pwm*speedA*1); } //PWMA.pulsewidth(period_pwm*speedA); } void moveB() { STBY=1; //disable standby int inPin1=0; int inPin2=1; if(directionB==0) { inPin1 = 1; inPin2 = 0; } //pc.printf("dirB = %d\n\r",directionB); BIN1=inPin1; BIN2=inPin2; //directionB=!directionB; if(!directionB) { //speedB=0.4; PWMB.pulsewidth(period_pwm*(speedB*1)); //PWMB.pulsewidth(period_pwm*(0.6)); } else { //speedB=0.2; PWMB.pulsewidth(period_pwm*speedB*1);//1.5 7-28 } //PWMB.pulsewidth(period_pwm*speedB); } void moveC() { STBY=1; //disable standby PWMC.pulsewidth(period_pwm*AngleLeft);//0.08 mid 0.04 back } void moveD() { STBY=1; //disable standby PWMD.pulsewidth(period_pwm*AngleRight);//0.08 mid 0.04 forward } void moveE() { /* if(directionE) { AngleTail=0.082+Tailbias; directionE=!directionE; } else { AngleTail=0.068+Tailbias; directionE=!directionE; } */ /* if(Tailbias==0) { AngleTail=0.075; } */ AngleTail=0.075+Tailbias; //AngleTail=0.12; //AngleTail=0.075; STBY=1; //disable standby PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward } /* void moveE_stand() { AngleTail=0.075+AngleTail_stand_bias; //AngleTail=0.12; //AngleTail=0.08; STBY=1; //disable standby PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward } */ void stop() { //enable standby STBY=0;; } int main() { //serial to BBB setup char str[9]; char *token; pc.baud(9600); BB.baud(9600); //Actuator & servo setup period_pwm_ac=0.010; //500hz/5khz period_pwm=0.020; //10ms PWMA.period(period_pwm_ac); PWMB.period(period_pwm_ac); PWMC.period(period_pwm); // servo requires a 20ms period PWMD.period(period_pwm); // servo requires a 20ms period PWME.period(period_pwm); // servo requires a 20ms period //initial direction & current directionA=1; directionB=0; directionE=1; flag=1; speedA=0.2; speedB=0.2; //Actuator speed control while(1) { // moveA(); // moveB(); // wait(0.2); // pc.printf(" x=%d y=%d", x,y); // pc.printf(str); //Talk to BBB int i; if(BB.readable()>0) { for(i=0;i<9;i++) str[i] = BB.getc(); if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C)) { token = strtok(str, ","); x = atoi(token); //100-420(width_320);No target:555 token = strtok(NULL, ","); y = atoi(token); //100-340(height_240); No target:555 //pc.printf("%d",y); //pc.printf("\n"); //pc.printf(str); } } /* if(y<300&&y>100) { Tailbias=(y-200)*0.0006; } else { Tailbias=0; } */ FA=5;//5; FB=5;//5; //speedA=0.0; //speedB=0.95; /* //Tailbias=0; if(CountT<=50) { //Tailbias=0;//(200-160)*0.0006; CountT++; } else if(CountT>50&&CountT<=100) { Tail_increasement=speedA*0.0012; CountT++; } else { CountT=0; } Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006; if(Tailbias>0.03) { Tailbias=0.03; } if(Tailbias<-0.03) { Tailbias=-0.03; } */ /* if(CountT>300&&CountT<=600) { speedA=0.6; Tailbias=0;//(160-200)*0.0006; speedB=0.0; CountT++; } else if(CountT<=300) { speedB=0.6; Tailbias=0;//(200-160)*0.0006; speedA=0.0; CountT++; } else { CountT=0; } */ /*Tracking demo*/ if(y<350&&y>200) { speedA=(y-200)*0.005+0.1; //Tail_increasement=speedA*0.0006; //Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006; speedB=0.0; Target=1; bias=-0.01;//(200-y)*0.0001; direction=Left; } else if (y>50&&y<=200) { speedB=(200-y)*0.005+0.1; //Tail_increasement=speedB*0.0006; //Tailbias=Tailbias+Tail_increasement;//0.0;//(y-200)*0.0006; speedA=0.0; Target=1; bias=0.01;//(200-y)*0.0001; direction=Right; } else { speedA=0.0; speedB=0.0; //Tailbias=0.0; Target=0; direction=Lost; } bias=0; if(Target) { if(Tailbias>=(0.008+bias)) { dirE=-1.0; } else if(Tailbias<=(-0.008+bias)) { dirE=1.0; } Tailbias=Tailbias+0.0016*dirE; } /* if(Tailbias>0.03) { Tailbias=0.03; } if(Tailbias<-0.03) { Tailbias=-0.03; } */ /* if(y>210&&y<400) { //FA=2000000; //FB=200000; FA=20;//FA=40; FB=5; speedB=0.6; speedA=0.02; //Tailbias=(y-210)*0.0003; //Tailbias=0.03; //AngleTail_stand_bias=0.03; //moveE_stand(); } else if(y<190&&y>100) { FB=20;//FB=40; FA=5; speedA=0.6; speedB=0.02; //Tailbias=-0.03; //AngleTail_stand_bias=-0.03; //moveE_stand(); } else { FA=20; FB=20; directionB=!directionA; CountB=CountA; speedA=0.02; speedB=0.02; Tailbias=0; AngleTail_stand_bias=0.0; //moveE_stand(); } */ /* FA=20;//FA=40; FB=5; speedB=0.6; speedA=0.01; //Tailbias=0.03; //AngleTail_stand_bias=0.03; Tailbias=0; AngleTail_stand_bias=0; */ if(CountA>=FA) { //moveA(); CountA=0; directionA=!directionA; /* if(directionA==1) { CountA=10; } else { CountA=0; } */ } else { CountA++; } if(CountB>=FB) { //moveB(); CountB=0; directionB=!directionB; /* if(directionB==0) { CountB=10; } else { CountB=0; } */ } else { CountB++; } /* if(CountE>=FE) { moveE(); CountE=0; } else { CountE++; } */ //6/29/2015 Tail if(x<300&&x>100) { //AngleLeft=-0.0005*x+0.18; //AngleRight=0.0005*x-0.12; AngleRight=-0.0005*x+0.18; AngleLeft=0.0005*x-0.02; } /* if(x<300&&x>100) { if(x<220&&x>210) { AngleLeft=0.08; AngleRight=0.08; } else if(x>=220) { AngleLeft=0.04; AngleRight=0.12; } else if(x<=210) { AngleLeft=0.12; AngleRight=0.04; } } else { AngleLeft=0.08; AngleRight=0.08; } */ /* AngleLeft=0.08; AngleRight=0.08; */ //moveC(); //moveD(); moveA(); // //directionB=0; moveB(); //moveE(); wait(0.03); //Serial test /* if(x>210) moveA(); else if (x<210) moveB(); else if (x==555) { moveA(); moveB(); wait(0.2); } else { moveA(); moveB(); wait(0.2); } */ //Actuator test // moveA(); // moveB(); // wait(0.2); //Servo test // PWMC.pulsewidth(period_pwm*0.08); // PWMD.pulsewidth(period_pwm*0.08); // PWME.pulsewidth(period_pwm*0.08); // wait(0.05); // PWMC.pulsewidth(period_pwm*0.05); // PWMD.pulsewidth(period_pwm*0.05); // PWME.pulsewidth(period_pwm*0.08); // } }