Renjian Hao / Mbed 2 deprecated Fish_2014Fall

Renjian Hao

Dependencies:   L3G4200D L3GD20 LSM303DLHC LSM303DLM PwmIn Servo mbed

Fork of Fish_2014Fall by Zhan Tu

main.cpp@5:a5f0395d2fa4, 2015-08-13 (annotated)

Committer:
RenjianHao
Date:
Thu Aug 13 18:37:15 2015 +0000
Revision:
5:a5f0395d2fa4
Parent:
4:c93e1ecd3359 
Renjian Hao2015/8/13

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tzxl10000 0:8f37781c0054 1 #include "mbed.h"
tzxl10000 0:8f37781c0054 2 #include "L3GD20.h"
tzxl10000 0:8f37781c0054 3 #include "LSM303DLHC.h"
tzxl10000 0:8f37781c0054 4 #include "Servo.h"
tzxl10000 0:8f37781c0054 5 #include "math.h"
tzxl10000 0:8f37781c0054 6 #include "PwmIn.h"
tzxl10000 0:8f37781c0054 7
tzxl10000 0:8f37781c0054 8 #include "iSerial.h"
tzxl10000 0:8f37781c0054 9 #include "iostream"
tzxl10000 0:8f37781c0054 10 #include "stdio.h"
tzxl10000 0:8f37781c0054 11 #include "string.h"
tzxl10000 0:8f37781c0054 12
tzxl10000 0:8f37781c0054 13 #include <string>
tzxl10000 0:8f37781c0054 14 #include <sstream>
tzxl10000 0:8f37781c0054 15 #define TS 10 // sample time [ms]
tzxl10000 0:8f37781c0054 16 #define SIMULATION_TIME 10 // simulation time [s]
tzxl10000 0:8f37781c0054 17
RenjianHao 5:a5f0395d2fa4 18 #define Left 1
RenjianHao 5:a5f0395d2fa4 19 #define Right 0
RenjianHao 5:a5f0395d2fa4 20 #define Lost 555
tzxl10000 0:8f37781c0054 21
tzxl10000 0:8f37781c0054 22 //serial
tzxl10000 0:8f37781c0054 23 Serial BB(p13, p14); // tx, rx
tzxl10000 0:8f37781c0054 24 Serial pc(USBTX,USBRX);
tzxl10000 0:8f37781c0054 25
tzxl10000 0:8f37781c0054 26 //period_pwm setup as 20ms
tzxl10000 0:8f37781c0054 27 //speed: 0 is off, and 1 is full speed；for servo 0-0.2 duty circle
tzxl10000 0:8f37781c0054 28 //direction: 0 clockwise, 1 counter-clockwise
tzxl10000 0:8f37781c0054 29
tzxl10000 0:8f37781c0054 30 float period_pwm_ac; // Actuator current
tzxl10000 0:8f37781c0054 31 float period_pwm;
tzxl10000 0:8f37781c0054 32
tzxl10000 0:8f37781c0054 33 bool directionA;
tzxl10000 0:8f37781c0054 34 bool directionB;
tzxl10000 0:8f37781c0054 35 bool directionC;
RenjianHao 2:d5dc0db74d84 36 bool directionE;
RenjianHao 5:a5f0395d2fa4 37 bool Target;
RenjianHao 5:a5f0395d2fa4 38 bool direction;
tzxl10000 0:8f37781c0054 39
tzxl10000 0:8f37781c0054 40 float speedA;
tzxl10000 0:8f37781c0054 41 float speedB;
tzxl10000 0:8f37781c0054 42 float speedC;
RenjianHao 4:c93e1ecd3359 43 float Tail_increasement=0.0;
tzxl10000 0:8f37781c0054 44
RenjianHao 1:9a7e97e643bc 45 float AngleLeft=0.08;
RenjianHao 2:d5dc0db74d84 46 float AngleRight=0.04;
RenjianHao 2:d5dc0db74d84 47 float AngleTail=0.08;
RenjianHao 2:d5dc0db74d84 48 float Tailbias=0.0;
RenjianHao 2:d5dc0db74d84 49 float AngleTail_stand_bias=0.0;
RenjianHao 5:a5f0395d2fa4 50 float dirE=1.0;
RenjianHao 5:a5f0395d2fa4 51 float bias=0.0;
RenjianHao 1:9a7e97e643bc 52
RenjianHao 5:a5f0395d2fa4 53 long int FA=1000000,FB=1000000,FE=40,CountA=0,CountB=0,CountE=0,CountT=0;
RenjianHao 1:9a7e97e643bc 54
tzxl10000 0:8f37781c0054 55 int flag;
tzxl10000 0:8f37781c0054 56 int x,y; // Image coordinate
tzxl10000 0:8f37781c0054 57
tzxl10000 0:8f37781c0054 58 //funtion declaration
tzxl10000 0:8f37781c0054 59 void move(int motor, float speed, int direction);
tzxl10000 0:8f37781c0054 60 PwmOut PWMA(p25);//LEFT FIN
tzxl10000 0:8f37781c0054 61 PwmOut PWMB(p23);//RIGHT FIN
tzxl10000 0:8f37781c0054 62 PwmOut PWMC(p24);//Left servo
tzxl10000 0:8f37781c0054 63 PwmOut PWMD(p26);//Right servo
tzxl10000 0:8f37781c0054 64 PwmOut PWME(p21);//Tail
tzxl10000 0:8f37781c0054 65
tzxl10000 0:8f37781c0054 66 DigitalOut STBY (p30);
tzxl10000 0:8f37781c0054 67 //DigitalOut STBY2 (p29);
tzxl10000 0:8f37781c0054 68 DigitalOut AIN1 (p8);
tzxl10000 0:8f37781c0054 69 DigitalOut AIN2 (p11); // Fin1 left
tzxl10000 0:8f37781c0054 70 DigitalOut BIN1 (p7);
tzxl10000 0:8f37781c0054 71 DigitalOut BIN2 (p6); // Fin2 right
tzxl10000 0:8f37781c0054 72 DigitalOut myLed (LED1);
tzxl10000 0:8f37781c0054 73 DigitalOut myLed1(LED2);
tzxl10000 0:8f37781c0054 74
tzxl10000 0:8f37781c0054 75
tzxl10000 0:8f37781c0054 76
tzxl10000 0:8f37781c0054 77
tzxl10000 0:8f37781c0054 78 void moveA()
tzxl10000 0:8f37781c0054 79 {
tzxl10000 0:8f37781c0054 80 STBY=1; //disable standby
tzxl10000 0:8f37781c0054 81 int inPin1=1;
tzxl10000 0:8f37781c0054 82 int inPin2=0;
tzxl10000 0:8f37781c0054 83 if(directionA==0) {
tzxl10000 0:8f37781c0054 84 inPin1 = 0;
tzxl10000 0:8f37781c0054 85 inPin2 = 1;
tzxl10000 0:8f37781c0054 86 }
tzxl10000 0:8f37781c0054 87 AIN1=inPin1;
tzxl10000 0:8f37781c0054 88 AIN2=inPin2;
RenjianHao 4:c93e1ecd3359 89 //directionA=!directionA;
tzxl10000 0:8f37781c0054 90
tzxl10000 0:8f37781c0054 91 //pc.printf("dirA = %d\n\r",directionA);
tzxl10000 0:8f37781c0054 92
tzxl10000 0:8f37781c0054 93
RenjianHao 1:9a7e97e643bc 94 if(directionA)
RenjianHao 1:9a7e97e643bc 95 {
RenjianHao 1:9a7e97e643bc 96 //speedA=0.4;
RenjianHao 5:a5f0395d2fa4 97 PWMA.pulsewidth(period_pwm*(speedA*1));
RenjianHao 4:c93e1ecd3359 98 //PWMA.pulsewidth(period_pwm*(0.6));
RenjianHao 1:9a7e97e643bc 99 }
RenjianHao 1:9a7e97e643bc 100 else
RenjianHao 1:9a7e97e643bc 101 {
RenjianHao 1:9a7e97e643bc 102 //speedA=0.2;
RenjianHao 5:a5f0395d2fa4 103 PWMA.pulsewidth(period_pwm*speedA*1);
RenjianHao 1:9a7e97e643bc 104 }
RenjianHao 1:9a7e97e643bc 105 //PWMA.pulsewidth(period_pwm*speedA);
tzxl10000 0:8f37781c0054 106
tzxl10000 0:8f37781c0054 107 }
tzxl10000 0:8f37781c0054 108
tzxl10000 0:8f37781c0054 109 void moveB()
tzxl10000 0:8f37781c0054 110 {
tzxl10000 0:8f37781c0054 111 STBY=1; //disable standby
tzxl10000 0:8f37781c0054 112 int inPin1=0;
tzxl10000 0:8f37781c0054 113 int inPin2=1;
tzxl10000 0:8f37781c0054 114 if(directionB==0) {
tzxl10000 0:8f37781c0054 115 inPin1 = 1;
tzxl10000 0:8f37781c0054 116 inPin2 = 0;
tzxl10000 0:8f37781c0054 117 }
tzxl10000 0:8f37781c0054 118 //pc.printf("dirB = %d\n\r",directionB);
tzxl10000 0:8f37781c0054 119 BIN1=inPin1;
tzxl10000 0:8f37781c0054 120 BIN2=inPin2;
RenjianHao 4:c93e1ecd3359 121 //directionB=!directionB;
tzxl10000 0:8f37781c0054 122
RenjianHao 1:9a7e97e643bc 123 if(!directionB)
RenjianHao 1:9a7e97e643bc 124 {
RenjianHao 1:9a7e97e643bc 125 //speedB=0.4;
RenjianHao 5:a5f0395d2fa4 126 PWMB.pulsewidth(period_pwm*(speedB*1));
RenjianHao 4:c93e1ecd3359 127 //PWMB.pulsewidth(period_pwm*(0.6));
RenjianHao 1:9a7e97e643bc 128 }
RenjianHao 1:9a7e97e643bc 129 else
RenjianHao 1:9a7e97e643bc 130 {
RenjianHao 1:9a7e97e643bc 131 //speedB=0.2;
RenjianHao 5:a5f0395d2fa4 132 PWMB.pulsewidth(period_pwm*speedB*1);//1.5 7-28
RenjianHao 1:9a7e97e643bc 133 }
RenjianHao 1:9a7e97e643bc 134 //PWMB.pulsewidth(period_pwm*speedB);
tzxl10000 0:8f37781c0054 135
tzxl10000 0:8f37781c0054 136 }
tzxl10000 0:8f37781c0054 137
RenjianHao 1:9a7e97e643bc 138
RenjianHao 1:9a7e97e643bc 139 void moveC()
RenjianHao 1:9a7e97e643bc 140 {
RenjianHao 1:9a7e97e643bc 141 STBY=1; //disable standby
RenjianHao 1:9a7e97e643bc 142 PWMC.pulsewidth(period_pwm*AngleLeft);//0.08 mid 0.04 back
RenjianHao 1:9a7e97e643bc 143
RenjianHao 1:9a7e97e643bc 144 }
RenjianHao 1:9a7e97e643bc 145 void moveD()
RenjianHao 1:9a7e97e643bc 146 {
RenjianHao 1:9a7e97e643bc 147 STBY=1; //disable standby
RenjianHao 1:9a7e97e643bc 148 PWMD.pulsewidth(period_pwm*AngleRight);//0.08 mid 0.04 forward
RenjianHao 1:9a7e97e643bc 149
RenjianHao 1:9a7e97e643bc 150 }
RenjianHao 3:caa0d4fd1d1d 151
RenjianHao 2:d5dc0db74d84 152 void moveE()
RenjianHao 2:d5dc0db74d84 153 {
RenjianHao 4:c93e1ecd3359 154 /*
RenjianHao 2:d5dc0db74d84 155 if(directionE)
RenjianHao 2:d5dc0db74d84 156 {
RenjianHao 4:c93e1ecd3359 157 AngleTail=0.082+Tailbias;
RenjianHao 2:d5dc0db74d84 158 directionE=!directionE;
RenjianHao 2:d5dc0db74d84 159 }
RenjianHao 2:d5dc0db74d84 160 else
RenjianHao 2:d5dc0db74d84 161 {
RenjianHao 4:c93e1ecd3359 162 AngleTail=0.068+Tailbias;
RenjianHao 2:d5dc0db74d84 163 directionE=!directionE;
RenjianHao 2:d5dc0db74d84 164 }
RenjianHao 4:c93e1ecd3359 165 */
RenjianHao 4:c93e1ecd3359 166 /*
RenjianHao 4:c93e1ecd3359 167 if(Tailbias==0)
RenjianHao 4:c93e1ecd3359 168 {
RenjianHao 4:c93e1ecd3359 169 AngleTail=0.075;
RenjianHao 4:c93e1ecd3359 170 }
RenjianHao 4:c93e1ecd3359 171 */
RenjianHao 4:c93e1ecd3359 172 AngleTail=0.075+Tailbias;
RenjianHao 2:d5dc0db74d84 173 //AngleTail=0.12;
RenjianHao 4:c93e1ecd3359 174 //AngleTail=0.075;
RenjianHao 2:d5dc0db74d84 175 STBY=1; //disable standby
RenjianHao 2:d5dc0db74d84 176 PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
RenjianHao 2:d5dc0db74d84 177 }
RenjianHao 3:caa0d4fd1d1d 178
RenjianHao 2:d5dc0db74d84 179
RenjianHao 3:caa0d4fd1d1d 180 /*
RenjianHao 2:d5dc0db74d84 181 void moveE_stand()
RenjianHao 2:d5dc0db74d84 182 {
RenjianHao 2:d5dc0db74d84 183
RenjianHao 2:d5dc0db74d84 184 AngleTail=0.075+AngleTail_stand_bias;
RenjianHao 2:d5dc0db74d84 185
RenjianHao 2:d5dc0db74d84 186 //AngleTail=0.12;
RenjianHao 2:d5dc0db74d84 187 //AngleTail=0.08;
RenjianHao 2:d5dc0db74d84 188 STBY=1; //disable standby
RenjianHao 2:d5dc0db74d84 189 PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
RenjianHao 2:d5dc0db74d84 190 }
RenjianHao 3:caa0d4fd1d1d 191 */
tzxl10000 0:8f37781c0054 192 void stop()
tzxl10000 0:8f37781c0054 193 {
tzxl10000 0:8f37781c0054 194 //enable standby
tzxl10000 0:8f37781c0054 195 STBY=0;;
tzxl10000 0:8f37781c0054 196 }
tzxl10000 0:8f37781c0054 197
tzxl10000 0:8f37781c0054 198 int main()
tzxl10000 0:8f37781c0054 199 {
tzxl10000 0:8f37781c0054 200 //serial to BBB setup
tzxl10000 0:8f37781c0054 201 char str[9];
tzxl10000 0:8f37781c0054 202 char *token;
tzxl10000 0:8f37781c0054 203
tzxl10000 0:8f37781c0054 204 pc.baud(9600);
tzxl10000 0:8f37781c0054 205 BB.baud(9600);
tzxl10000 0:8f37781c0054 206
tzxl10000 0:8f37781c0054 207 //Actuator & servo setup
tzxl10000 0:8f37781c0054 208
RenjianHao 5:a5f0395d2fa4 209 period_pwm_ac=0.010; //500hz/5khz
RenjianHao 5:a5f0395d2fa4 210 period_pwm=0.020; //10ms
tzxl10000 0:8f37781c0054 211 PWMA.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 212 PWMB.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 213 PWMC.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 214 PWMD.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 215 PWME.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 216
tzxl10000 0:8f37781c0054 217 //initial direction & current
tzxl10000 0:8f37781c0054 218 directionA=1;
tzxl10000 0:8f37781c0054 219 directionB=0;
RenjianHao 2:d5dc0db74d84 220 directionE=1;
tzxl10000 0:8f37781c0054 221 flag=1;
tzxl10000 0:8f37781c0054 222 speedA=0.2;
tzxl10000 0:8f37781c0054 223 speedB=0.2; //Actuator speed control
tzxl10000 0:8f37781c0054 224
tzxl10000 0:8f37781c0054 225 while(1)
tzxl10000 0:8f37781c0054 226 {
tzxl10000 0:8f37781c0054 227
RenjianHao 1:9a7e97e643bc 228
RenjianHao 1:9a7e97e643bc 229 // moveA();
RenjianHao 1:9a7e97e643bc 230 // moveB();
RenjianHao 1:9a7e97e643bc 231 // wait(0.2);
RenjianHao 1:9a7e97e643bc 232 // pc.printf(" x=%d y=%d", x,y);
RenjianHao 1:9a7e97e643bc 233 // pc.printf(str);
RenjianHao 1:9a7e97e643bc 234
tzxl10000 0:8f37781c0054 235 //Talk to BBB
RenjianHao 5:a5f0395d2fa4 236
tzxl10000 0:8f37781c0054 237 int i;
tzxl10000 0:8f37781c0054 238 if(BB.readable()>0)
tzxl10000 0:8f37781c0054 239 {
tzxl10000 0:8f37781c0054 240 for(i=0;i<9;i++)
tzxl10000 0:8f37781c0054 241 str[i] = BB.getc();
tzxl10000 0:8f37781c0054 242
tzxl10000 0:8f37781c0054 243 if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
tzxl10000 0:8f37781c0054 244 {
tzxl10000 0:8f37781c0054 245 token = strtok(str, ",");
tzxl10000 0:8f37781c0054 246 x = atoi(token); //100-420(width_320);No target:555
tzxl10000 0:8f37781c0054 247
tzxl10000 0:8f37781c0054 248 token = strtok(NULL, ",");
tzxl10000 0:8f37781c0054 249 y = atoi(token); //100-340(height_240); No target:555
RenjianHao 5:a5f0395d2fa4 250 //pc.printf("%d",y);
RenjianHao 4:c93e1ecd3359 251 //pc.printf("\n");
tzxl10000 0:8f37781c0054 252 //pc.printf(str);
tzxl10000 0:8f37781c0054 253 }
tzxl10000 0:8f37781c0054 254 }
RenjianHao 5:a5f0395d2fa4 255
RenjianHao 4:c93e1ecd3359 256 /*
RenjianHao 4:c93e1ecd3359 257 if(y<300&&y>100)
RenjianHao 4:c93e1ecd3359 258 {
RenjianHao 4:c93e1ecd3359 259 Tailbias=(y-200)*0.0006;
RenjianHao 4:c93e1ecd3359 260 }
RenjianHao 4:c93e1ecd3359 261
RenjianHao 4:c93e1ecd3359 262 else
RenjianHao 4:c93e1ecd3359 263 {
RenjianHao 4:c93e1ecd3359 264 Tailbias=0;
RenjianHao 4:c93e1ecd3359 265 }
RenjianHao 4:c93e1ecd3359 266 */
RenjianHao 5:a5f0395d2fa4 267 FA=5;//5;
RenjianHao 5:a5f0395d2fa4 268 FB=5;//5;
RenjianHao 5:a5f0395d2fa4 269
RenjianHao 5:a5f0395d2fa4 270 //speedA=0.0;
RenjianHao 5:a5f0395d2fa4 271 //speedB=0.95;
RenjianHao 5:a5f0395d2fa4 272 /*
RenjianHao 5:a5f0395d2fa4 273 //Tailbias=0;
RenjianHao 5:a5f0395d2fa4 274 if(CountT<=50)
RenjianHao 5:a5f0395d2fa4 275 {
RenjianHao 5:a5f0395d2fa4 276 //Tailbias=0;//(200-160)*0.0006;
RenjianHao 5:a5f0395d2fa4 277 CountT++;
RenjianHao 5:a5f0395d2fa4 278 }
RenjianHao 5:a5f0395d2fa4 279 else if(CountT>50&&CountT<=100)
RenjianHao 5:a5f0395d2fa4 280 {
RenjianHao 5:a5f0395d2fa4 281 Tail_increasement=speedA*0.0012;
RenjianHao 5:a5f0395d2fa4 282 CountT++;
RenjianHao 5:a5f0395d2fa4 283 }
RenjianHao 5:a5f0395d2fa4 284 else
RenjianHao 5:a5f0395d2fa4 285 {
RenjianHao 5:a5f0395d2fa4 286 CountT=0;
RenjianHao 5:a5f0395d2fa4 287 }
RenjianHao 5:a5f0395d2fa4 288
RenjianHao 5:a5f0395d2fa4 289 Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
RenjianHao 5:a5f0395d2fa4 290 if(Tailbias>0.03)
RenjianHao 5:a5f0395d2fa4 291 {
RenjianHao 5:a5f0395d2fa4 292 Tailbias=0.03;
RenjianHao 5:a5f0395d2fa4 293 }
RenjianHao 5:a5f0395d2fa4 294 if(Tailbias<-0.03)
RenjianHao 5:a5f0395d2fa4 295 {
RenjianHao 5:a5f0395d2fa4 296 Tailbias=-0.03;
RenjianHao 5:a5f0395d2fa4 297 }
RenjianHao 5:a5f0395d2fa4 298 */
RenjianHao 4:c93e1ecd3359 299 /*
RenjianHao 4:c93e1ecd3359 300 if(CountT>300&&CountT<=600)
RenjianHao 4:c93e1ecd3359 301 {
RenjianHao 4:c93e1ecd3359 302 speedA=0.6;
RenjianHao 4:c93e1ecd3359 303 Tailbias=0;//(160-200)*0.0006;
RenjianHao 4:c93e1ecd3359 304 speedB=0.0;
RenjianHao 4:c93e1ecd3359 305 CountT++;
RenjianHao 4:c93e1ecd3359 306 }
RenjianHao 4:c93e1ecd3359 307 else if(CountT<=300)
RenjianHao 4:c93e1ecd3359 308 {
RenjianHao 4:c93e1ecd3359 309 speedB=0.6;
RenjianHao 4:c93e1ecd3359 310 Tailbias=0;//(200-160)*0.0006;
RenjianHao 4:c93e1ecd3359 311 speedA=0.0;
RenjianHao 4:c93e1ecd3359 312 CountT++;
RenjianHao 4:c93e1ecd3359 313 }
RenjianHao 4:c93e1ecd3359 314 else
RenjianHao 4:c93e1ecd3359 315 {
RenjianHao 4:c93e1ecd3359 316 CountT=0;
RenjianHao 4:c93e1ecd3359 317 }
RenjianHao 4:c93e1ecd3359 318 */
RenjianHao 5:a5f0395d2fa4 319 /*Tracking demo*/
RenjianHao 5:a5f0395d2fa4 320
RenjianHao 4:c93e1ecd3359 321 if(y<350&&y>200)
RenjianHao 4:c93e1ecd3359 322 {
RenjianHao 4:c93e1ecd3359 323
RenjianHao 5:a5f0395d2fa4 324 speedA=(y-200)*0.005+0.1;
RenjianHao 5:a5f0395d2fa4 325 //Tail_increasement=speedA*0.0006;
RenjianHao 5:a5f0395d2fa4 326 //Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
RenjianHao 4:c93e1ecd3359 327 speedB=0.0;
RenjianHao 5:a5f0395d2fa4 328 Target=1;
RenjianHao 5:a5f0395d2fa4 329 bias=-0.01;//(200-y)*0.0001;
RenjianHao 5:a5f0395d2fa4 330 direction=Left;
RenjianHao 4:c93e1ecd3359 331 }
RenjianHao 4:c93e1ecd3359 332 else if (y>50&&y<=200)
RenjianHao 4:c93e1ecd3359 333 {
RenjianHao 5:a5f0395d2fa4 334 speedB=(200-y)*0.005+0.1;
RenjianHao 5:a5f0395d2fa4 335 //Tail_increasement=speedB*0.0006;
RenjianHao 5:a5f0395d2fa4 336 //Tailbias=Tailbias+Tail_increasement;//0.0;//(y-200)*0.0006;
RenjianHao 4:c93e1ecd3359 337 speedA=0.0;
RenjianHao 5:a5f0395d2fa4 338 Target=1;
RenjianHao 5:a5f0395d2fa4 339 bias=0.01;//(200-y)*0.0001;
RenjianHao 5:a5f0395d2fa4 340 direction=Right;
RenjianHao 4:c93e1ecd3359 341 }
RenjianHao 4:c93e1ecd3359 342 else
RenjianHao 4:c93e1ecd3359 343 {
RenjianHao 4:c93e1ecd3359 344 speedA=0.0;
RenjianHao 4:c93e1ecd3359 345 speedB=0.0;
RenjianHao 4:c93e1ecd3359 346 //Tailbias=0.0;
RenjianHao 5:a5f0395d2fa4 347 Target=0;
RenjianHao 5:a5f0395d2fa4 348 direction=Lost;
RenjianHao 4:c93e1ecd3359 349 }
RenjianHao 5:a5f0395d2fa4 350 bias=0;
RenjianHao 5:a5f0395d2fa4 351 if(Target)
RenjianHao 5:a5f0395d2fa4 352 {
RenjianHao 5:a5f0395d2fa4 353 if(Tailbias>=(0.008+bias))
RenjianHao 5:a5f0395d2fa4 354 {
RenjianHao 5:a5f0395d2fa4 355 dirE=-1.0;
RenjianHao 5:a5f0395d2fa4 356 }
RenjianHao 5:a5f0395d2fa4 357 else if(Tailbias<=(-0.008+bias))
RenjianHao 5:a5f0395d2fa4 358 {
RenjianHao 5:a5f0395d2fa4 359 dirE=1.0;
RenjianHao 5:a5f0395d2fa4 360 }
RenjianHao 5:a5f0395d2fa4 361 Tailbias=Tailbias+0.0016*dirE;
RenjianHao 5:a5f0395d2fa4 362 }
RenjianHao 5:a5f0395d2fa4 363
RenjianHao 5:a5f0395d2fa4 364 /*
RenjianHao 5:a5f0395d2fa4 365 if(Tailbias>0.03)
RenjianHao 4:c93e1ecd3359 366 {
RenjianHao 5:a5f0395d2fa4 367 Tailbias=0.03;
RenjianHao 4:c93e1ecd3359 368 }
RenjianHao 5:a5f0395d2fa4 369 if(Tailbias<-0.03)
RenjianHao 5:a5f0395d2fa4 370 {
RenjianHao 5:a5f0395d2fa4 371 Tailbias=-0.03;
RenjianHao 5:a5f0395d2fa4 372 }
RenjianHao 5:a5f0395d2fa4 373 */
RenjianHao 4:c93e1ecd3359 374
RenjianHao 4:c93e1ecd3359 375 /*
RenjianHao 1:9a7e97e643bc 376 if(y>210&&y<400)
RenjianHao 1:9a7e97e643bc 377 {
RenjianHao 1:9a7e97e643bc 378 //FA=2000000;
RenjianHao 1:9a7e97e643bc 379 //FB=200000;
RenjianHao 1:9a7e97e643bc 380 FA=20;//FA=40;
RenjianHao 4:c93e1ecd3359 381 FB=5;
RenjianHao 4:c93e1ecd3359 382 speedB=0.6;
RenjianHao 4:c93e1ecd3359 383 speedA=0.02;
RenjianHao 4:c93e1ecd3359 384 //Tailbias=(y-210)*0.0003;
RenjianHao 4:c93e1ecd3359 385 //Tailbias=0.03;
RenjianHao 4:c93e1ecd3359 386 //AngleTail_stand_bias=0.03;
RenjianHao 3:caa0d4fd1d1d 387 //moveE_stand();
RenjianHao 1:9a7e97e643bc 388 }
RenjianHao 1:9a7e97e643bc 389
RenjianHao 1:9a7e97e643bc 390 else if(y<190&&y>100)
RenjianHao 1:9a7e97e643bc 391 {
RenjianHao 1:9a7e97e643bc 392 FB=20;//FB=40;
RenjianHao 4:c93e1ecd3359 393 FA=5;
RenjianHao 4:c93e1ecd3359 394 speedA=0.6;
RenjianHao 1:9a7e97e643bc 395 speedB=0.02;
RenjianHao 4:c93e1ecd3359 396 //Tailbias=-0.03;
RenjianHao 4:c93e1ecd3359 397 //AngleTail_stand_bias=-0.03;
RenjianHao 3:caa0d4fd1d1d 398 //moveE_stand();
RenjianHao 1:9a7e97e643bc 399 }
RenjianHao 1:9a7e97e643bc 400 else
RenjianHao 1:9a7e97e643bc 401 {
RenjianHao 1:9a7e97e643bc 402 FA=20;
RenjianHao 1:9a7e97e643bc 403 FB=20;
RenjianHao 1:9a7e97e643bc 404 directionB=!directionA;
RenjianHao 1:9a7e97e643bc 405 CountB=CountA;
RenjianHao 4:c93e1ecd3359 406 speedA=0.02;
RenjianHao 4:c93e1ecd3359 407 speedB=0.02;
RenjianHao 2:d5dc0db74d84 408 Tailbias=0;
RenjianHao 2:d5dc0db74d84 409 AngleTail_stand_bias=0.0;
RenjianHao 3:caa0d4fd1d1d 410 //moveE_stand();
RenjianHao 1:9a7e97e643bc 411 }
RenjianHao 4:c93e1ecd3359 412 */
RenjianHao 4:c93e1ecd3359 413 /*
RenjianHao 4:c93e1ecd3359 414 FA=20;//FA=40;
RenjianHao 4:c93e1ecd3359 415 FB=5;
RenjianHao 4:c93e1ecd3359 416 speedB=0.6;
RenjianHao 4:c93e1ecd3359 417 speedA=0.01;
RenjianHao 4:c93e1ecd3359 418 //Tailbias=0.03;
RenjianHao 4:c93e1ecd3359 419 //AngleTail_stand_bias=0.03;
RenjianHao 4:c93e1ecd3359 420 Tailbias=0;
RenjianHao 4:c93e1ecd3359 421 AngleTail_stand_bias=0;
RenjianHao 4:c93e1ecd3359 422 */
RenjianHao 1:9a7e97e643bc 423 if(CountA>=FA)
RenjianHao 1:9a7e97e643bc 424 {
RenjianHao 4:c93e1ecd3359 425 //moveA();
RenjianHao 1:9a7e97e643bc 426 CountA=0;
RenjianHao 4:c93e1ecd3359 427 directionA=!directionA;
RenjianHao 4:c93e1ecd3359 428 /*
RenjianHao 4:c93e1ecd3359 429 if(directionA==1)
RenjianHao 4:c93e1ecd3359 430 {
RenjianHao 4:c93e1ecd3359 431 CountA=10;
RenjianHao 4:c93e1ecd3359 432 }
RenjianHao 4:c93e1ecd3359 433 else
RenjianHao 4:c93e1ecd3359 434 {
RenjianHao 4:c93e1ecd3359 435 CountA=0;
RenjianHao 4:c93e1ecd3359 436 }
RenjianHao 4:c93e1ecd3359 437 */
RenjianHao 1:9a7e97e643bc 438 }
RenjianHao 1:9a7e97e643bc 439 else
RenjianHao 1:9a7e97e643bc 440 {
RenjianHao 1:9a7e97e643bc 441 CountA++;
RenjianHao 1:9a7e97e643bc 442 }
RenjianHao 1:9a7e97e643bc 443 if(CountB>=FB)
RenjianHao 1:9a7e97e643bc 444 {
RenjianHao 4:c93e1ecd3359 445 //moveB();
RenjianHao 1:9a7e97e643bc 446 CountB=0;
RenjianHao 4:c93e1ecd3359 447 directionB=!directionB;
RenjianHao 4:c93e1ecd3359 448 /*
RenjianHao 4:c93e1ecd3359 449 if(directionB==0)
RenjianHao 4:c93e1ecd3359 450 {
RenjianHao 4:c93e1ecd3359 451 CountB=10;
RenjianHao 4:c93e1ecd3359 452 }
RenjianHao 4:c93e1ecd3359 453 else
RenjianHao 4:c93e1ecd3359 454 {
RenjianHao 4:c93e1ecd3359 455 CountB=0;
RenjianHao 4:c93e1ecd3359 456 }
RenjianHao 4:c93e1ecd3359 457 */
RenjianHao 1:9a7e97e643bc 458 }
RenjianHao 1:9a7e97e643bc 459 else
RenjianHao 1:9a7e97e643bc 460 {
RenjianHao 1:9a7e97e643bc 461 CountB++;
RenjianHao 1:9a7e97e643bc 462 }
RenjianHao 1:9a7e97e643bc 463
RenjianHao 4:c93e1ecd3359 464 /*
RenjianHao 2:d5dc0db74d84 465 if(CountE>=FE)
RenjianHao 2:d5dc0db74d84 466 {
RenjianHao 2:d5dc0db74d84 467 moveE();
RenjianHao 2:d5dc0db74d84 468 CountE=0;
RenjianHao 2:d5dc0db74d84 469 }
RenjianHao 2:d5dc0db74d84 470 else
RenjianHao 2:d5dc0db74d84 471 {
RenjianHao 2:d5dc0db74d84 472 CountE++;
RenjianHao 2:d5dc0db74d84 473 }
RenjianHao 5:a5f0395d2fa4 474 */
RenjianHao 3:caa0d4fd1d1d 475 //6/29/2015 Tail
RenjianHao 1:9a7e97e643bc 476 if(x<300&&x>100)
RenjianHao 1:9a7e97e643bc 477 {
RenjianHao 1:9a7e97e643bc 478 //AngleLeft=-0.0005*x+0.18;
RenjianHao 1:9a7e97e643bc 479 //AngleRight=0.0005*x-0.12;
RenjianHao 1:9a7e97e643bc 480 AngleRight=-0.0005*x+0.18;
RenjianHao 1:9a7e97e643bc 481 AngleLeft=0.0005*x-0.02;
RenjianHao 1:9a7e97e643bc 482 }
RenjianHao 1:9a7e97e643bc 483
RenjianHao 1:9a7e97e643bc 484
RenjianHao 1:9a7e97e643bc 485 /*
RenjianHao 1:9a7e97e643bc 486 if(x<300&&x>100)
RenjianHao 1:9a7e97e643bc 487 {
RenjianHao 1:9a7e97e643bc 488 if(x<220&&x>210)
RenjianHao 1:9a7e97e643bc 489 {
RenjianHao 1:9a7e97e643bc 490 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 491 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 492 }
RenjianHao 1:9a7e97e643bc 493 else if(x>=220)
RenjianHao 1:9a7e97e643bc 494 {
RenjianHao 1:9a7e97e643bc 495 AngleLeft=0.04;
RenjianHao 1:9a7e97e643bc 496 AngleRight=0.12;
RenjianHao 1:9a7e97e643bc 497 }
RenjianHao 1:9a7e97e643bc 498 else if(x<=210)
RenjianHao 1:9a7e97e643bc 499 {
RenjianHao 1:9a7e97e643bc 500 AngleLeft=0.12;
RenjianHao 1:9a7e97e643bc 501 AngleRight=0.04;
RenjianHao 1:9a7e97e643bc 502 }
RenjianHao 1:9a7e97e643bc 503 }
RenjianHao 1:9a7e97e643bc 504 else
RenjianHao 1:9a7e97e643bc 505 {
RenjianHao 1:9a7e97e643bc 506 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 507 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 508 }
RenjianHao 1:9a7e97e643bc 509 */
RenjianHao 1:9a7e97e643bc 510 /*
RenjianHao 1:9a7e97e643bc 511 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 512 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 513 */
RenjianHao 1:9a7e97e643bc 514 //moveC();
RenjianHao 1:9a7e97e643bc 515 //moveD();
RenjianHao 4:c93e1ecd3359 516
RenjianHao 4:c93e1ecd3359 517 moveA();
RenjianHao 5:a5f0395d2fa4 518 //
RenjianHao 5:a5f0395d2fa4 519
RenjianHao 5:a5f0395d2fa4 520 //directionB=0;
RenjianHao 4:c93e1ecd3359 521 moveB();
RenjianHao 4:c93e1ecd3359 522
RenjianHao 5:a5f0395d2fa4 523 //moveE();
RenjianHao 4:c93e1ecd3359 524
RenjianHao 1:9a7e97e643bc 525 wait(0.03);
RenjianHao 1:9a7e97e643bc 526
tzxl10000 0:8f37781c0054 527 //Serial test
RenjianHao 1:9a7e97e643bc 528 /*
tzxl10000 0:8f37781c0054 529 if(x>210)
tzxl10000 0:8f37781c0054 530 moveA();
tzxl10000 0:8f37781c0054 531 else if (x<210)
tzxl10000 0:8f37781c0054 532 moveB();
tzxl10000 0:8f37781c0054 533 else if (x==555)
tzxl10000 0:8f37781c0054 534 {
tzxl10000 0:8f37781c0054 535 moveA();
tzxl10000 0:8f37781c0054 536 moveB();
tzxl10000 0:8f37781c0054 537 wait(0.2);
tzxl10000 0:8f37781c0054 538 }
RenjianHao 1:9a7e97e643bc 539 else
RenjianHao 1:9a7e97e643bc 540 {
RenjianHao 1:9a7e97e643bc 541 moveA();
RenjianHao 1:9a7e97e643bc 542 moveB();
RenjianHao 1:9a7e97e643bc 543 wait(0.2);
RenjianHao 1:9a7e97e643bc 544 }
RenjianHao 1:9a7e97e643bc 545 */
tzxl10000 0:8f37781c0054 546
tzxl10000 0:8f37781c0054 547
tzxl10000 0:8f37781c0054 548 //Actuator test
tzxl10000 0:8f37781c0054 549 // moveA();
tzxl10000 0:8f37781c0054 550 // moveB();
tzxl10000 0:8f37781c0054 551 // wait(0.2);
tzxl10000 0:8f37781c0054 552
tzxl10000 0:8f37781c0054 553 //Servo test
tzxl10000 0:8f37781c0054 554
tzxl10000 0:8f37781c0054 555 // PWMC.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 556 // PWMD.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 557 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 558 // wait(0.05);
tzxl10000 0:8f37781c0054 559 // PWMC.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 560 // PWMD.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 561 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 562 //
tzxl10000 0:8f37781c0054 563
tzxl10000 0:8f37781c0054 564
tzxl10000 0:8f37781c0054 565
tzxl10000 0:8f37781c0054 566
tzxl10000 0:8f37781c0054 567 }
tzxl10000 0:8f37781c0054 568
tzxl10000 0:8f37781c0054 569 }