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@4:c93e1ecd3359, 2015-07-15 (annotated)

Committer:
RenjianHao
Date:
Wed Jul 15 20:13:09 2015 +0000
Revision:
4:c93e1ecd3359
Parent:
3:caa0d4fd1d1d
Child:
5:a5f0395d2fa4
Renjian Hao 7/15/2015

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