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@3:caa0d4fd1d1d, 2015-07-01 (annotated)

Committer:
RenjianHao
Date:
Wed Jul 01 18:45:22 2015 +0000
Revision:
3:caa0d4fd1d1d
Parent:
2:d5dc0db74d84
Child:
4:c93e1ecd3359
Renjian Hao 7/1/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;
tzxl10000 0:8f37781c0054 39
RenjianHao 1:9a7e97e643bc 40 float AngleLeft=0.08;
RenjianHao 2:d5dc0db74d84 41 float AngleRight=0.04;
RenjianHao 2:d5dc0db74d84 42 float AngleTail=0.08;
RenjianHao 2:d5dc0db74d84 43 float Tailbias=0.0;
RenjianHao 2:d5dc0db74d84 44 float AngleTail_stand_bias=0.0;
RenjianHao 1:9a7e97e643bc 45
RenjianHao 3:caa0d4fd1d1d 46 long int FA=1000000,FB=1000000,FE=10,CountA=0,CountB=0,CountE=0;;
RenjianHao 1:9a7e97e643bc 47
tzxl10000 0:8f37781c0054 48 int flag;
tzxl10000 0:8f37781c0054 49 int x,y; // Image coordinate
tzxl10000 0:8f37781c0054 50
tzxl10000 0:8f37781c0054 51 //funtion declaration
tzxl10000 0:8f37781c0054 52 void move(int motor, float speed, int direction);
tzxl10000 0:8f37781c0054 53 PwmOut PWMA(p25);//LEFT FIN
tzxl10000 0:8f37781c0054 54 PwmOut PWMB(p23);//RIGHT FIN
tzxl10000 0:8f37781c0054 55 PwmOut PWMC(p24);//Left servo
tzxl10000 0:8f37781c0054 56 PwmOut PWMD(p26);//Right servo
tzxl10000 0:8f37781c0054 57 PwmOut PWME(p21);//Tail
tzxl10000 0:8f37781c0054 58
tzxl10000 0:8f37781c0054 59 DigitalOut STBY (p30);
tzxl10000 0:8f37781c0054 60 //DigitalOut STBY2 (p29);
tzxl10000 0:8f37781c0054 61 DigitalOut AIN1 (p8);
tzxl10000 0:8f37781c0054 62 DigitalOut AIN2 (p11); // Fin1 left
tzxl10000 0:8f37781c0054 63 DigitalOut BIN1 (p7);
tzxl10000 0:8f37781c0054 64 DigitalOut BIN2 (p6); // Fin2 right
tzxl10000 0:8f37781c0054 65 DigitalOut myLed (LED1);
tzxl10000 0:8f37781c0054 66 DigitalOut myLed1(LED2);
tzxl10000 0:8f37781c0054 67
tzxl10000 0:8f37781c0054 68
tzxl10000 0:8f37781c0054 69
tzxl10000 0:8f37781c0054 70
tzxl10000 0:8f37781c0054 71 void moveA()
tzxl10000 0:8f37781c0054 72 {
tzxl10000 0:8f37781c0054 73 STBY=1; //disable standby
tzxl10000 0:8f37781c0054 74 int inPin1=1;
tzxl10000 0:8f37781c0054 75 int inPin2=0;
tzxl10000 0:8f37781c0054 76 if(directionA==0) {
tzxl10000 0:8f37781c0054 77 inPin1 = 0;
tzxl10000 0:8f37781c0054 78 inPin2 = 1;
tzxl10000 0:8f37781c0054 79 }
tzxl10000 0:8f37781c0054 80 AIN1=inPin1;
tzxl10000 0:8f37781c0054 81 AIN2=inPin2;
tzxl10000 0:8f37781c0054 82 directionA=!directionA;
tzxl10000 0:8f37781c0054 83
tzxl10000 0:8f37781c0054 84 //pc.printf("dirA = %d\n\r",directionA);
tzxl10000 0:8f37781c0054 85
tzxl10000 0:8f37781c0054 86
RenjianHao 1:9a7e97e643bc 87 if(directionA)
RenjianHao 1:9a7e97e643bc 88 {
RenjianHao 1:9a7e97e643bc 89 //speedA=0.4;
RenjianHao 1:9a7e97e643bc 90 PWMA.pulsewidth(period_pwm*(speedA*1.5));
RenjianHao 1:9a7e97e643bc 91 }
RenjianHao 1:9a7e97e643bc 92 else
RenjianHao 1:9a7e97e643bc 93 {
RenjianHao 1:9a7e97e643bc 94 //speedA=0.2;
RenjianHao 1:9a7e97e643bc 95 PWMA.pulsewidth(period_pwm*speedA);
RenjianHao 1:9a7e97e643bc 96 }
RenjianHao 1:9a7e97e643bc 97 //PWMA.pulsewidth(period_pwm*speedA);
tzxl10000 0:8f37781c0054 98
tzxl10000 0:8f37781c0054 99 }
tzxl10000 0:8f37781c0054 100
tzxl10000 0:8f37781c0054 101 void moveB()
tzxl10000 0:8f37781c0054 102 {
tzxl10000 0:8f37781c0054 103 STBY=1; //disable standby
tzxl10000 0:8f37781c0054 104 int inPin1=0;
tzxl10000 0:8f37781c0054 105 int inPin2=1;
tzxl10000 0:8f37781c0054 106 if(directionB==0) {
tzxl10000 0:8f37781c0054 107 inPin1 = 1;
tzxl10000 0:8f37781c0054 108 inPin2 = 0;
tzxl10000 0:8f37781c0054 109 }
tzxl10000 0:8f37781c0054 110 //pc.printf("dirB = %d\n\r",directionB);
tzxl10000 0:8f37781c0054 111 BIN1=inPin1;
tzxl10000 0:8f37781c0054 112 BIN2=inPin2;
tzxl10000 0:8f37781c0054 113 directionB=!directionB;
tzxl10000 0:8f37781c0054 114
RenjianHao 1:9a7e97e643bc 115 if(!directionB)
RenjianHao 1:9a7e97e643bc 116 {
RenjianHao 1:9a7e97e643bc 117 //speedB=0.4;
RenjianHao 1:9a7e97e643bc 118 PWMB.pulsewidth(period_pwm*(speedB*1.5));
RenjianHao 1:9a7e97e643bc 119 }
RenjianHao 1:9a7e97e643bc 120 else
RenjianHao 1:9a7e97e643bc 121 {
RenjianHao 1:9a7e97e643bc 122 //speedB=0.2;
RenjianHao 1:9a7e97e643bc 123 PWMB.pulsewidth(period_pwm*speedB);
RenjianHao 1:9a7e97e643bc 124 }
RenjianHao 1:9a7e97e643bc 125 //PWMB.pulsewidth(period_pwm*speedB);
tzxl10000 0:8f37781c0054 126
tzxl10000 0:8f37781c0054 127 }
tzxl10000 0:8f37781c0054 128
RenjianHao 1:9a7e97e643bc 129
RenjianHao 1:9a7e97e643bc 130 void moveC()
RenjianHao 1:9a7e97e643bc 131 {
RenjianHao 1:9a7e97e643bc 132 STBY=1; //disable standby
RenjianHao 1:9a7e97e643bc 133 PWMC.pulsewidth(period_pwm*AngleLeft);//0.08 mid 0.04 back
RenjianHao 1:9a7e97e643bc 134
RenjianHao 1:9a7e97e643bc 135 }
RenjianHao 1:9a7e97e643bc 136 void moveD()
RenjianHao 1:9a7e97e643bc 137 {
RenjianHao 1:9a7e97e643bc 138 STBY=1; //disable standby
RenjianHao 1:9a7e97e643bc 139 PWMD.pulsewidth(period_pwm*AngleRight);//0.08 mid 0.04 forward
RenjianHao 1:9a7e97e643bc 140
RenjianHao 1:9a7e97e643bc 141 }
RenjianHao 3:caa0d4fd1d1d 142
RenjianHao 2:d5dc0db74d84 143 void moveE()
RenjianHao 2:d5dc0db74d84 144 {
RenjianHao 2:d5dc0db74d84 145 if(directionE)
RenjianHao 2:d5dc0db74d84 146 {
RenjianHao 3:caa0d4fd1d1d 147 AngleTail=0.08+Tailbias;
RenjianHao 2:d5dc0db74d84 148 directionE=!directionE;
RenjianHao 2:d5dc0db74d84 149 }
RenjianHao 2:d5dc0db74d84 150 else
RenjianHao 2:d5dc0db74d84 151 {
RenjianHao 3:caa0d4fd1d1d 152 AngleTail=0.07+Tailbias;
RenjianHao 2:d5dc0db74d84 153 directionE=!directionE;
RenjianHao 2:d5dc0db74d84 154 }
RenjianHao 2:d5dc0db74d84 155
RenjianHao 2:d5dc0db74d84 156 //AngleTail=0.12;
RenjianHao 2:d5dc0db74d84 157 //AngleTail=0.08;
RenjianHao 2:d5dc0db74d84 158 STBY=1; //disable standby
RenjianHao 2:d5dc0db74d84 159 PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
RenjianHao 2:d5dc0db74d84 160 }
RenjianHao 3:caa0d4fd1d1d 161
RenjianHao 2:d5dc0db74d84 162
RenjianHao 3:caa0d4fd1d1d 163 /*
RenjianHao 2:d5dc0db74d84 164 void moveE_stand()
RenjianHao 2:d5dc0db74d84 165 {
RenjianHao 2:d5dc0db74d84 166
RenjianHao 2:d5dc0db74d84 167 AngleTail=0.075+AngleTail_stand_bias;
RenjianHao 2:d5dc0db74d84 168
RenjianHao 2:d5dc0db74d84 169 //AngleTail=0.12;
RenjianHao 2:d5dc0db74d84 170 //AngleTail=0.08;
RenjianHao 2:d5dc0db74d84 171 STBY=1; //disable standby
RenjianHao 2:d5dc0db74d84 172 PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
RenjianHao 2:d5dc0db74d84 173 }
RenjianHao 3:caa0d4fd1d1d 174 */
tzxl10000 0:8f37781c0054 175 void stop()
tzxl10000 0:8f37781c0054 176 {
tzxl10000 0:8f37781c0054 177 //enable standby
tzxl10000 0:8f37781c0054 178 STBY=0;;
tzxl10000 0:8f37781c0054 179 }
tzxl10000 0:8f37781c0054 180
tzxl10000 0:8f37781c0054 181 int main()
tzxl10000 0:8f37781c0054 182 {
tzxl10000 0:8f37781c0054 183 //serial to BBB setup
tzxl10000 0:8f37781c0054 184 char str[9];
tzxl10000 0:8f37781c0054 185 char *token;
tzxl10000 0:8f37781c0054 186
tzxl10000 0:8f37781c0054 187 pc.baud(9600);
tzxl10000 0:8f37781c0054 188 BB.baud(9600);
tzxl10000 0:8f37781c0054 189
tzxl10000 0:8f37781c0054 190 //Actuator & servo setup
tzxl10000 0:8f37781c0054 191
tzxl10000 0:8f37781c0054 192 period_pwm_ac=0.0020; //500hz
tzxl10000 0:8f37781c0054 193 period_pwm=0.020; //20ms
tzxl10000 0:8f37781c0054 194 PWMA.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 195 PWMB.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 196 PWMC.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 197 PWMD.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 198 PWME.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 199
tzxl10000 0:8f37781c0054 200 //initial direction & current
tzxl10000 0:8f37781c0054 201 directionA=1;
tzxl10000 0:8f37781c0054 202 directionB=0;
RenjianHao 2:d5dc0db74d84 203 directionE=1;
tzxl10000 0:8f37781c0054 204 flag=1;
tzxl10000 0:8f37781c0054 205 speedA=0.2;
tzxl10000 0:8f37781c0054 206 speedB=0.2; //Actuator speed control
tzxl10000 0:8f37781c0054 207
tzxl10000 0:8f37781c0054 208 while(1)
tzxl10000 0:8f37781c0054 209 {
tzxl10000 0:8f37781c0054 210
RenjianHao 1:9a7e97e643bc 211
RenjianHao 1:9a7e97e643bc 212 // moveA();
RenjianHao 1:9a7e97e643bc 213 // moveB();
RenjianHao 1:9a7e97e643bc 214 // wait(0.2);
RenjianHao 1:9a7e97e643bc 215 // pc.printf(" x=%d y=%d", x,y);
RenjianHao 1:9a7e97e643bc 216 // pc.printf(str);
RenjianHao 1:9a7e97e643bc 217
tzxl10000 0:8f37781c0054 218 //Talk to BBB
RenjianHao 1:9a7e97e643bc 219
tzxl10000 0:8f37781c0054 220 int i;
tzxl10000 0:8f37781c0054 221 if(BB.readable()>0)
tzxl10000 0:8f37781c0054 222 {
tzxl10000 0:8f37781c0054 223 for(i=0;i<9;i++)
tzxl10000 0:8f37781c0054 224 str[i] = BB.getc();
tzxl10000 0:8f37781c0054 225
tzxl10000 0:8f37781c0054 226 if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
tzxl10000 0:8f37781c0054 227 {
tzxl10000 0:8f37781c0054 228 token = strtok(str, ",");
tzxl10000 0:8f37781c0054 229 x = atoi(token); //100-420(width_320);No target:555
tzxl10000 0:8f37781c0054 230
tzxl10000 0:8f37781c0054 231 token = strtok(NULL, ",");
tzxl10000 0:8f37781c0054 232 y = atoi(token); //100-340(height_240); No target:555
RenjianHao 1:9a7e97e643bc 233 //pc.printf(" x=%d y=%d", x,y);
RenjianHao 1:9a7e97e643bc 234 //pc.printf("\n");
tzxl10000 0:8f37781c0054 235 //pc.printf(str);
tzxl10000 0:8f37781c0054 236 }
tzxl10000 0:8f37781c0054 237 }
tzxl10000 0:8f37781c0054 238
RenjianHao 1:9a7e97e643bc 239
RenjianHao 1:9a7e97e643bc 240 if(y>210&&y<400)
RenjianHao 1:9a7e97e643bc 241 {
RenjianHao 1:9a7e97e643bc 242 //FA=2000000;
RenjianHao 1:9a7e97e643bc 243 //FB=200000;
RenjianHao 1:9a7e97e643bc 244 FA=20;//FA=40;
RenjianHao 1:9a7e97e643bc 245 FB=20;
RenjianHao 1:9a7e97e643bc 246 speedB=0.3;
RenjianHao 1:9a7e97e643bc 247 speedA=0.02;
RenjianHao 2:d5dc0db74d84 248 Tailbias=0.015;
RenjianHao 2:d5dc0db74d84 249 AngleTail_stand_bias=0.015;
RenjianHao 3:caa0d4fd1d1d 250 //moveE_stand();
RenjianHao 1:9a7e97e643bc 251 }
RenjianHao 1:9a7e97e643bc 252
RenjianHao 1:9a7e97e643bc 253 else if(y<190&&y>100)
RenjianHao 1:9a7e97e643bc 254 {
RenjianHao 1:9a7e97e643bc 255 FB=20;//FB=40;
RenjianHao 1:9a7e97e643bc 256 FA=20;
RenjianHao 1:9a7e97e643bc 257 speedA=0.3;
RenjianHao 1:9a7e97e643bc 258 speedB=0.02;
RenjianHao 2:d5dc0db74d84 259 Tailbias=-0.015;
RenjianHao 2:d5dc0db74d84 260 AngleTail_stand_bias=-0.015;
RenjianHao 3:caa0d4fd1d1d 261 //moveE_stand();
RenjianHao 1:9a7e97e643bc 262 }
RenjianHao 1:9a7e97e643bc 263 else
RenjianHao 1:9a7e97e643bc 264 {
RenjianHao 1:9a7e97e643bc 265 FA=20;
RenjianHao 1:9a7e97e643bc 266 FB=20;
RenjianHao 1:9a7e97e643bc 267 directionB=!directionA;
RenjianHao 1:9a7e97e643bc 268 CountB=CountA;
RenjianHao 1:9a7e97e643bc 269 speedA=0.2;
RenjianHao 1:9a7e97e643bc 270 speedB=0.2;
RenjianHao 2:d5dc0db74d84 271 Tailbias=0;
RenjianHao 2:d5dc0db74d84 272 AngleTail_stand_bias=0.0;
RenjianHao 3:caa0d4fd1d1d 273 //moveE_stand();
RenjianHao 1:9a7e97e643bc 274 }
RenjianHao 1:9a7e97e643bc 275 if(CountA>=FA)
RenjianHao 1:9a7e97e643bc 276 {
RenjianHao 1:9a7e97e643bc 277 moveA();
RenjianHao 1:9a7e97e643bc 278 CountA=0;
RenjianHao 1:9a7e97e643bc 279 }
RenjianHao 1:9a7e97e643bc 280 else
RenjianHao 1:9a7e97e643bc 281 {
RenjianHao 1:9a7e97e643bc 282 CountA++;
RenjianHao 1:9a7e97e643bc 283 }
RenjianHao 1:9a7e97e643bc 284 if(CountB>=FB)
RenjianHao 1:9a7e97e643bc 285 {
RenjianHao 1:9a7e97e643bc 286 moveB();
RenjianHao 2:d5dc0db74d84 287 //moveE();
RenjianHao 1:9a7e97e643bc 288 CountB=0;
RenjianHao 1:9a7e97e643bc 289 }
RenjianHao 1:9a7e97e643bc 290 else
RenjianHao 1:9a7e97e643bc 291 {
RenjianHao 1:9a7e97e643bc 292 CountB++;
RenjianHao 1:9a7e97e643bc 293 }
RenjianHao 1:9a7e97e643bc 294
RenjianHao 3:caa0d4fd1d1d 295
RenjianHao 2:d5dc0db74d84 296 if(CountE>=FE)
RenjianHao 2:d5dc0db74d84 297 {
RenjianHao 2:d5dc0db74d84 298 moveE();
RenjianHao 2:d5dc0db74d84 299 CountE=0;
RenjianHao 2:d5dc0db74d84 300 }
RenjianHao 2:d5dc0db74d84 301 else
RenjianHao 2:d5dc0db74d84 302 {
RenjianHao 2:d5dc0db74d84 303 CountE++;
RenjianHao 2:d5dc0db74d84 304 }
RenjianHao 2:d5dc0db74d84 305
RenjianHao 3:caa0d4fd1d1d 306 //6/29/2015 Tail
RenjianHao 1:9a7e97e643bc 307 if(x<300&&x>100)
RenjianHao 1:9a7e97e643bc 308 {
RenjianHao 1:9a7e97e643bc 309 //AngleLeft=-0.0005*x+0.18;
RenjianHao 1:9a7e97e643bc 310 //AngleRight=0.0005*x-0.12;
RenjianHao 1:9a7e97e643bc 311 AngleRight=-0.0005*x+0.18;
RenjianHao 1:9a7e97e643bc 312 AngleLeft=0.0005*x-0.02;
RenjianHao 1:9a7e97e643bc 313 }
RenjianHao 1:9a7e97e643bc 314
RenjianHao 1:9a7e97e643bc 315
RenjianHao 1:9a7e97e643bc 316 /*
RenjianHao 1:9a7e97e643bc 317 if(x<300&&x>100)
RenjianHao 1:9a7e97e643bc 318 {
RenjianHao 1:9a7e97e643bc 319 if(x<220&&x>210)
RenjianHao 1:9a7e97e643bc 320 {
RenjianHao 1:9a7e97e643bc 321 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 322 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 323 }
RenjianHao 1:9a7e97e643bc 324 else if(x>=220)
RenjianHao 1:9a7e97e643bc 325 {
RenjianHao 1:9a7e97e643bc 326 AngleLeft=0.04;
RenjianHao 1:9a7e97e643bc 327 AngleRight=0.12;
RenjianHao 1:9a7e97e643bc 328 }
RenjianHao 1:9a7e97e643bc 329 else if(x<=210)
RenjianHao 1:9a7e97e643bc 330 {
RenjianHao 1:9a7e97e643bc 331 AngleLeft=0.12;
RenjianHao 1:9a7e97e643bc 332 AngleRight=0.04;
RenjianHao 1:9a7e97e643bc 333 }
RenjianHao 1:9a7e97e643bc 334 }
RenjianHao 1:9a7e97e643bc 335 else
RenjianHao 1:9a7e97e643bc 336 {
RenjianHao 1:9a7e97e643bc 337 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 338 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 339 }
RenjianHao 1:9a7e97e643bc 340 */
RenjianHao 1:9a7e97e643bc 341 /*
RenjianHao 1:9a7e97e643bc 342 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 343 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 344 */
RenjianHao 1:9a7e97e643bc 345 //moveC();
RenjianHao 1:9a7e97e643bc 346 //moveD();
RenjianHao 1:9a7e97e643bc 347 wait(0.03);
RenjianHao 1:9a7e97e643bc 348
tzxl10000 0:8f37781c0054 349 //Serial test
RenjianHao 1:9a7e97e643bc 350 /*
tzxl10000 0:8f37781c0054 351 if(x>210)
tzxl10000 0:8f37781c0054 352 moveA();
tzxl10000 0:8f37781c0054 353 else if (x<210)
tzxl10000 0:8f37781c0054 354 moveB();
tzxl10000 0:8f37781c0054 355 else if (x==555)
tzxl10000 0:8f37781c0054 356 {
tzxl10000 0:8f37781c0054 357 moveA();
tzxl10000 0:8f37781c0054 358 moveB();
tzxl10000 0:8f37781c0054 359 wait(0.2);
tzxl10000 0:8f37781c0054 360 }
RenjianHao 1:9a7e97e643bc 361 else
RenjianHao 1:9a7e97e643bc 362 {
RenjianHao 1:9a7e97e643bc 363 moveA();
RenjianHao 1:9a7e97e643bc 364 moveB();
RenjianHao 1:9a7e97e643bc 365 wait(0.2);
RenjianHao 1:9a7e97e643bc 366 }
RenjianHao 1:9a7e97e643bc 367 */
tzxl10000 0:8f37781c0054 368
tzxl10000 0:8f37781c0054 369
tzxl10000 0:8f37781c0054 370 //Actuator test
tzxl10000 0:8f37781c0054 371 // moveA();
tzxl10000 0:8f37781c0054 372 // moveB();
tzxl10000 0:8f37781c0054 373 // wait(0.2);
tzxl10000 0:8f37781c0054 374
tzxl10000 0:8f37781c0054 375 //Servo test
tzxl10000 0:8f37781c0054 376
tzxl10000 0:8f37781c0054 377 // PWMC.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 378 // PWMD.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 379 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 380 // wait(0.05);
tzxl10000 0:8f37781c0054 381 // PWMC.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 382 // PWMD.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 383 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 384 //
tzxl10000 0:8f37781c0054 385
tzxl10000 0:8f37781c0054 386
tzxl10000 0:8f37781c0054 387
tzxl10000 0:8f37781c0054 388
tzxl10000 0:8f37781c0054 389 }
tzxl10000 0:8f37781c0054 390
tzxl10000 0:8f37781c0054 391 }