Renjian Hao
Dependencies: L3G4200D L3GD20 LSM303DLHC LSM303DLM PwmIn Servo mbed
Fork of Fish_2014Fall by
main.cpp
- Committer:
- tzxl10000
- Date:
- 2015-04-17
- Revision:
- 0:8f37781c0054
- Child:
- 1:9a7e97e643bc
File content as of revision 0:8f37781c0054:
#include "mbed.h" #include "L3GD20.h" #include "LSM303DLHC.h" #include "Servo.h" #include "math.h" #include "PwmIn.h" #include "iSerial.h" #include "iostream" #include "stdio.h" #include "string.h" #include <string> #include <sstream> #define TS 10 // sample time [ms] #define SIMULATION_TIME 10 // simulation time [s] //serial Serial BB(p13, p14); // tx, rx Serial pc(USBTX,USBRX); //period_pwm setup as 20ms //speed: 0 is off, and 1 is full speed;for servo 0-0.2 duty circle //direction: 0 clockwise, 1 counter-clockwise float period_pwm_ac; // Actuator current float period_pwm; bool directionA; bool directionB; bool directionC; float speedA; float speedB; float speedC; int flag; int x,y; // Image coordinate //funtion declaration void move(int motor, float speed, int direction); PwmOut PWMA(p25);//LEFT FIN PwmOut PWMB(p23);//RIGHT FIN PwmOut PWMC(p24);//Left servo PwmOut PWMD(p26);//Right servo PwmOut PWME(p21);//Tail DigitalOut STBY (p30); //DigitalOut STBY2 (p29); DigitalOut AIN1 (p8); DigitalOut AIN2 (p11); // Fin1 left DigitalOut BIN1 (p7); DigitalOut BIN2 (p6); // Fin2 right DigitalOut myLed (LED1); DigitalOut myLed1(LED2); void moveA() { STBY=1; //disable standby int inPin1=1; int inPin2=0; if(directionA==0) { inPin1 = 0; inPin2 = 1; } AIN1=inPin1; AIN2=inPin2; directionA=!directionA; //pc.printf("dirA = %d\n\r",directionA); PWMA.pulsewidth(period_pwm*speedA); } void moveB() { STBY=1; //disable standby int inPin1=0; int inPin2=1; if(directionB==0) { inPin1 = 1; inPin2 = 0; } //pc.printf("dirB = %d\n\r",directionB); BIN1=inPin1; BIN2=inPin2; directionB=!directionB; PWMB.pulsewidth(period_pwm*speedB); } void stop() { //enable standby STBY=0;; } int main() { //serial to BBB setup char str[9]; char *token; pc.baud(9600); BB.baud(9600); //Actuator & servo setup period_pwm_ac=0.0020; //500hz period_pwm=0.020; //20ms PWMA.period(period_pwm_ac); PWMB.period(period_pwm_ac); PWMC.period(period_pwm); // servo requires a 20ms period PWMD.period(period_pwm); // servo requires a 20ms period PWME.period(period_pwm); // servo requires a 20ms period //initial direction & current directionA=1; directionB=0; flag=1; speedA=0.2; speedB=0.2; //Actuator speed control while(1) { //Talk to BBB int i; if(BB.readable()>0) { for(i=0;i<9;i++) str[i] = BB.getc(); if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C)) { token = strtok(str, ","); x = atoi(token); //100-420(width_320);No target:555 token = strtok(NULL, ","); y = atoi(token); //100-340(height_240); No target:555 // pc.printf(" x=%d y=%d", x,y); //pc.printf(str); } } //Serial test if(x>210) moveA(); else if (x<210) moveB(); else if (x==555) { moveA(); moveB(); wait(0.2); } //Actuator test // moveA(); // moveB(); // wait(0.2); //Servo test // PWMC.pulsewidth(period_pwm*0.08); // PWMD.pulsewidth(period_pwm*0.08); // PWME.pulsewidth(period_pwm*0.08); // wait(0.05); // PWMC.pulsewidth(period_pwm*0.05); // PWMD.pulsewidth(period_pwm*0.05); // PWME.pulsewidth(period_pwm*0.08); // } }