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@2:d5dc0db74d84, 2015-06-29 (annotated)

Committer:
RenjianHao
Date:
Mon Jun 29 17:30:28 2015 +0000
Revision:
2:d5dc0db74d84
Parent:
1:9a7e97e643bc
Child:
3:caa0d4fd1d1d
6-29-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 2:d5dc0db74d84 46 long int FA=1000000,FB=1000000,FE=40,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 2:d5dc0db74d84 142 /*
RenjianHao 2:d5dc0db74d84 143 void moveE()
RenjianHao 2:d5dc0db74d84 144 {
RenjianHao 2:d5dc0db74d84 145 if(directionE)
RenjianHao 2:d5dc0db74d84 146 {
RenjianHao 2:d5dc0db74d84 147 AngleTail=0.09+Tailbias;
RenjianHao 2:d5dc0db74d84 148 directionE=!directionE;
RenjianHao 2:d5dc0db74d84 149 }
RenjianHao 2:d5dc0db74d84 150 else
RenjianHao 2:d5dc0db74d84 151 {
RenjianHao 2:d5dc0db74d84 152 AngleTail=0.06+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 2:d5dc0db74d84 161 */
RenjianHao 2:d5dc0db74d84 162
RenjianHao 2:d5dc0db74d84 163 void moveE_stand()
RenjianHao 2:d5dc0db74d84 164 {
RenjianHao 2:d5dc0db74d84 165
RenjianHao 2:d5dc0db74d84 166 AngleTail=0.075+AngleTail_stand_bias;
RenjianHao 2:d5dc0db74d84 167
RenjianHao 2:d5dc0db74d84 168 //AngleTail=0.12;
RenjianHao 2:d5dc0db74d84 169 //AngleTail=0.08;
RenjianHao 2:d5dc0db74d84 170 STBY=1; //disable standby
RenjianHao 2:d5dc0db74d84 171 PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
RenjianHao 2:d5dc0db74d84 172 }
tzxl10000 0:8f37781c0054 173 void stop()
tzxl10000 0:8f37781c0054 174 {
tzxl10000 0:8f37781c0054 175 //enable standby
tzxl10000 0:8f37781c0054 176 STBY=0;;
tzxl10000 0:8f37781c0054 177 }
tzxl10000 0:8f37781c0054 178
tzxl10000 0:8f37781c0054 179 int main()
tzxl10000 0:8f37781c0054 180 {
tzxl10000 0:8f37781c0054 181 //serial to BBB setup
tzxl10000 0:8f37781c0054 182 char str[9];
tzxl10000 0:8f37781c0054 183 char *token;
tzxl10000 0:8f37781c0054 184
tzxl10000 0:8f37781c0054 185 pc.baud(9600);
tzxl10000 0:8f37781c0054 186 BB.baud(9600);
tzxl10000 0:8f37781c0054 187
tzxl10000 0:8f37781c0054 188 //Actuator & servo setup
tzxl10000 0:8f37781c0054 189
tzxl10000 0:8f37781c0054 190 period_pwm_ac=0.0020; //500hz
tzxl10000 0:8f37781c0054 191 period_pwm=0.020; //20ms
tzxl10000 0:8f37781c0054 192 PWMA.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 193 PWMB.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 194 PWMC.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 195 PWMD.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 196 PWME.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 197
tzxl10000 0:8f37781c0054 198 //initial direction & current
tzxl10000 0:8f37781c0054 199 directionA=1;
tzxl10000 0:8f37781c0054 200 directionB=0;
RenjianHao 2:d5dc0db74d84 201 directionE=1;
tzxl10000 0:8f37781c0054 202 flag=1;
tzxl10000 0:8f37781c0054 203 speedA=0.2;
tzxl10000 0:8f37781c0054 204 speedB=0.2; //Actuator speed control
tzxl10000 0:8f37781c0054 205
tzxl10000 0:8f37781c0054 206 while(1)
tzxl10000 0:8f37781c0054 207 {
tzxl10000 0:8f37781c0054 208
RenjianHao 1:9a7e97e643bc 209
RenjianHao 1:9a7e97e643bc 210 // moveA();
RenjianHao 1:9a7e97e643bc 211 // moveB();
RenjianHao 1:9a7e97e643bc 212 // wait(0.2);
RenjianHao 1:9a7e97e643bc 213 // pc.printf(" x=%d y=%d", x,y);
RenjianHao 1:9a7e97e643bc 214 // pc.printf(str);
RenjianHao 1:9a7e97e643bc 215
tzxl10000 0:8f37781c0054 216 //Talk to BBB
RenjianHao 1:9a7e97e643bc 217
tzxl10000 0:8f37781c0054 218 int i;
tzxl10000 0:8f37781c0054 219 if(BB.readable()>0)
tzxl10000 0:8f37781c0054 220 {
tzxl10000 0:8f37781c0054 221 for(i=0;i<9;i++)
tzxl10000 0:8f37781c0054 222 str[i] = BB.getc();
tzxl10000 0:8f37781c0054 223
tzxl10000 0:8f37781c0054 224 if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
tzxl10000 0:8f37781c0054 225 {
tzxl10000 0:8f37781c0054 226 token = strtok(str, ",");
tzxl10000 0:8f37781c0054 227 x = atoi(token); //100-420(width_320);No target:555
tzxl10000 0:8f37781c0054 228
tzxl10000 0:8f37781c0054 229 token = strtok(NULL, ",");
tzxl10000 0:8f37781c0054 230 y = atoi(token); //100-340(height_240); No target:555
RenjianHao 1:9a7e97e643bc 231 //pc.printf(" x=%d y=%d", x,y);
RenjianHao 1:9a7e97e643bc 232 //pc.printf("\n");
tzxl10000 0:8f37781c0054 233 //pc.printf(str);
tzxl10000 0:8f37781c0054 234 }
tzxl10000 0:8f37781c0054 235 }
tzxl10000 0:8f37781c0054 236
RenjianHao 1:9a7e97e643bc 237
RenjianHao 1:9a7e97e643bc 238 if(y>210&&y<400)
RenjianHao 1:9a7e97e643bc 239 {
RenjianHao 1:9a7e97e643bc 240 //FA=2000000;
RenjianHao 1:9a7e97e643bc 241 //FB=200000;
RenjianHao 1:9a7e97e643bc 242 FA=20;//FA=40;
RenjianHao 1:9a7e97e643bc 243 FB=20;
RenjianHao 1:9a7e97e643bc 244 speedB=0.3;
RenjianHao 1:9a7e97e643bc 245 speedA=0.02;
RenjianHao 2:d5dc0db74d84 246 Tailbias=0.015;
RenjianHao 2:d5dc0db74d84 247 AngleTail_stand_bias=0.015;
RenjianHao 2:d5dc0db74d84 248 moveE_stand();
RenjianHao 1:9a7e97e643bc 249 }
RenjianHao 1:9a7e97e643bc 250
RenjianHao 1:9a7e97e643bc 251 else if(y<190&&y>100)
RenjianHao 1:9a7e97e643bc 252 {
RenjianHao 1:9a7e97e643bc 253 FB=20;//FB=40;
RenjianHao 1:9a7e97e643bc 254 FA=20;
RenjianHao 1:9a7e97e643bc 255 speedA=0.3;
RenjianHao 1:9a7e97e643bc 256 speedB=0.02;
RenjianHao 2:d5dc0db74d84 257 Tailbias=-0.015;
RenjianHao 2:d5dc0db74d84 258 AngleTail_stand_bias=-0.015;
RenjianHao 2:d5dc0db74d84 259 moveE_stand();
RenjianHao 1:9a7e97e643bc 260 }
RenjianHao 1:9a7e97e643bc 261 else
RenjianHao 1:9a7e97e643bc 262 {
RenjianHao 1:9a7e97e643bc 263 FA=20;
RenjianHao 1:9a7e97e643bc 264 FB=20;
RenjianHao 1:9a7e97e643bc 265 directionB=!directionA;
RenjianHao 1:9a7e97e643bc 266 CountB=CountA;
RenjianHao 1:9a7e97e643bc 267 speedA=0.2;
RenjianHao 1:9a7e97e643bc 268 speedB=0.2;
RenjianHao 2:d5dc0db74d84 269 Tailbias=0;
RenjianHao 2:d5dc0db74d84 270 AngleTail_stand_bias=0.0;
RenjianHao 2:d5dc0db74d84 271 moveE_stand();
RenjianHao 1:9a7e97e643bc 272 }
RenjianHao 1:9a7e97e643bc 273 if(CountA>=FA)
RenjianHao 1:9a7e97e643bc 274 {
RenjianHao 1:9a7e97e643bc 275 moveA();
RenjianHao 1:9a7e97e643bc 276 CountA=0;
RenjianHao 1:9a7e97e643bc 277 }
RenjianHao 1:9a7e97e643bc 278 else
RenjianHao 1:9a7e97e643bc 279 {
RenjianHao 1:9a7e97e643bc 280 CountA++;
RenjianHao 1:9a7e97e643bc 281 }
RenjianHao 1:9a7e97e643bc 282 if(CountB>=FB)
RenjianHao 1:9a7e97e643bc 283 {
RenjianHao 1:9a7e97e643bc 284 moveB();
RenjianHao 2:d5dc0db74d84 285 //moveE();
RenjianHao 1:9a7e97e643bc 286 CountB=0;
RenjianHao 1:9a7e97e643bc 287 }
RenjianHao 1:9a7e97e643bc 288 else
RenjianHao 1:9a7e97e643bc 289 {
RenjianHao 1:9a7e97e643bc 290 CountB++;
RenjianHao 1:9a7e97e643bc 291 }
RenjianHao 1:9a7e97e643bc 292
RenjianHao 2:d5dc0db74d84 293 /*
RenjianHao 2:d5dc0db74d84 294 if(CountE>=FE)
RenjianHao 2:d5dc0db74d84 295 {
RenjianHao 2:d5dc0db74d84 296 moveE();
RenjianHao 2:d5dc0db74d84 297 CountE=0;
RenjianHao 2:d5dc0db74d84 298 }
RenjianHao 2:d5dc0db74d84 299 else
RenjianHao 2:d5dc0db74d84 300 {
RenjianHao 2:d5dc0db74d84 301 CountE++;
RenjianHao 2:d5dc0db74d84 302 }
RenjianHao 2:d5dc0db74d84 303
RenjianHao 2:d5dc0db74d84 304 */ //6/29/2015 Tail
RenjianHao 1:9a7e97e643bc 305 if(x<300&&x>100)
RenjianHao 1:9a7e97e643bc 306 {
RenjianHao 1:9a7e97e643bc 307 //AngleLeft=-0.0005*x+0.18;
RenjianHao 1:9a7e97e643bc 308 //AngleRight=0.0005*x-0.12;
RenjianHao 1:9a7e97e643bc 309 AngleRight=-0.0005*x+0.18;
RenjianHao 1:9a7e97e643bc 310 AngleLeft=0.0005*x-0.02;
RenjianHao 1:9a7e97e643bc 311 }
RenjianHao 1:9a7e97e643bc 312
RenjianHao 1:9a7e97e643bc 313
RenjianHao 1:9a7e97e643bc 314 /*
RenjianHao 1:9a7e97e643bc 315 if(x<300&&x>100)
RenjianHao 1:9a7e97e643bc 316 {
RenjianHao 1:9a7e97e643bc 317 if(x<220&&x>210)
RenjianHao 1:9a7e97e643bc 318 {
RenjianHao 1:9a7e97e643bc 319 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 320 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 321 }
RenjianHao 1:9a7e97e643bc 322 else if(x>=220)
RenjianHao 1:9a7e97e643bc 323 {
RenjianHao 1:9a7e97e643bc 324 AngleLeft=0.04;
RenjianHao 1:9a7e97e643bc 325 AngleRight=0.12;
RenjianHao 1:9a7e97e643bc 326 }
RenjianHao 1:9a7e97e643bc 327 else if(x<=210)
RenjianHao 1:9a7e97e643bc 328 {
RenjianHao 1:9a7e97e643bc 329 AngleLeft=0.12;
RenjianHao 1:9a7e97e643bc 330 AngleRight=0.04;
RenjianHao 1:9a7e97e643bc 331 }
RenjianHao 1:9a7e97e643bc 332 }
RenjianHao 1:9a7e97e643bc 333 else
RenjianHao 1:9a7e97e643bc 334 {
RenjianHao 1:9a7e97e643bc 335 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 336 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 337 }
RenjianHao 1:9a7e97e643bc 338 */
RenjianHao 1:9a7e97e643bc 339 /*
RenjianHao 1:9a7e97e643bc 340 AngleLeft=0.08;
RenjianHao 1:9a7e97e643bc 341 AngleRight=0.08;
RenjianHao 1:9a7e97e643bc 342 */
RenjianHao 1:9a7e97e643bc 343 //moveC();
RenjianHao 1:9a7e97e643bc 344 //moveD();
RenjianHao 1:9a7e97e643bc 345 wait(0.03);
RenjianHao 1:9a7e97e643bc 346
tzxl10000 0:8f37781c0054 347 //Serial test
RenjianHao 1:9a7e97e643bc 348 /*
tzxl10000 0:8f37781c0054 349 if(x>210)
tzxl10000 0:8f37781c0054 350 moveA();
tzxl10000 0:8f37781c0054 351 else if (x<210)
tzxl10000 0:8f37781c0054 352 moveB();
tzxl10000 0:8f37781c0054 353 else if (x==555)
tzxl10000 0:8f37781c0054 354 {
tzxl10000 0:8f37781c0054 355 moveA();
tzxl10000 0:8f37781c0054 356 moveB();
tzxl10000 0:8f37781c0054 357 wait(0.2);
tzxl10000 0:8f37781c0054 358 }
RenjianHao 1:9a7e97e643bc 359 else
RenjianHao 1:9a7e97e643bc 360 {
RenjianHao 1:9a7e97e643bc 361 moveA();
RenjianHao 1:9a7e97e643bc 362 moveB();
RenjianHao 1:9a7e97e643bc 363 wait(0.2);
RenjianHao 1:9a7e97e643bc 364 }
RenjianHao 1:9a7e97e643bc 365 */
tzxl10000 0:8f37781c0054 366
tzxl10000 0:8f37781c0054 367
tzxl10000 0:8f37781c0054 368 //Actuator test
tzxl10000 0:8f37781c0054 369 // moveA();
tzxl10000 0:8f37781c0054 370 // moveB();
tzxl10000 0:8f37781c0054 371 // wait(0.2);
tzxl10000 0:8f37781c0054 372
tzxl10000 0:8f37781c0054 373 //Servo test
tzxl10000 0:8f37781c0054 374
tzxl10000 0:8f37781c0054 375 // PWMC.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 376 // PWMD.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 377 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 378 // wait(0.05);
tzxl10000 0:8f37781c0054 379 // PWMC.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 380 // PWMD.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 381 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 382 //
tzxl10000 0:8f37781c0054 383
tzxl10000 0:8f37781c0054 384
tzxl10000 0:8f37781c0054 385
tzxl10000 0:8f37781c0054 386
tzxl10000 0:8f37781c0054 387 }
tzxl10000 0:8f37781c0054 388
tzxl10000 0:8f37781c0054 389 }