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@1:9a7e97e643bc, 2015-06-26 (annotated)

Committer:
RenjianHao
Date:
Fri Jun 26 17:31:28 2015 +0000
Revision:
1:9a7e97e643bc
Parent:
0:8f37781c0054
Child:
2:d5dc0db74d84
Renjian Hao

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