drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 6:0602a9e8118b
- Parent:
- 5:c89308dc1827
- Child:
- 7:d6dca30f7959
diff -r c89308dc1827 -r 0602a9e8118b main.cpp --- a/main.cpp Sun Mar 06 18:31:59 2016 +0000 +++ b/main.cpp Wed Mar 16 22:05:23 2016 +0000 @@ -1,27 +1,179 @@ #include "LOCALIZE.h" +#include "LOCOMOTION.h" +#include "WATCHDOG.h" -//Serial pc(p13, p14); -Serial pc(USBTX, USBRX); +#define SPEED_TURN_MIN 0.15 +#define SPEED_TURN_MAX 0.35 +#define SPEED_FB_MIN 0.15 +#define SPEED_FB_MAX 0.50 + +Serial pc(p13, p14); +//Serial pc(USBTX, USBRX); + +Watchdog wdt; I2C i2c1(p28, p27); I2C i2c2(p9, p10); LOCALIZE loc(i2c1, i2c2, p26); LOCALIZE_xya xya; +DigitalIn sw1(p20); +DigitalIn sw2(p19); +DigitalIn sw3(p18); +DigitalIn sw4(p17); +DigitalOut dir1(p15); +DigitalOut dir2(p16); + +PwmOut motor1F(p21); +PwmOut motor1B(p22); +PwmOut motor2F(p23); +PwmOut motor2B(p24); + DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); +Ticker t; +bool flag=false; + +void send(); +void turn(int angle); + int main() { + wdt.kick(5); pc.baud(9600); - pc.printf("Initialized Localization: %d\n",loc.init()); + dir1=0; + dir2=0; + motor1F=0; + motor1B=0; + motor2F=0; + motor2B=0; + //pc.printf("Initialized Localization: %d\n",loc.init()); + t.attach(&send,1); + sw1.mode(PullUp); + sw2.mode(PullUp); + sw3.mode(PullUp); + sw4.mode(PullUp); + while(abs(xya.a-180)>5) { + loc.get_angle(&xya); + turn(180); + } while(1) { - //loc.get_angle(&xya); - loc.get_xy(&xya); - pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); + led1=!sw1; + led2=!sw2; + led3=!sw3; + led4=!sw4; + loc.get_angle(&xya); + //loc.get_xy(&xya); + loc.get_raw_xy(); //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); - //pc.printf("%c%c%c%c\n",xya.x,xya.y,xya.a/10,xya.a%10); + /*if(loc._rx_n>20) { + dir1=1; + dir2=1; + } else if(loc._rx_n<10) { + dir1=0; + dir2=0; + }*/ + + if((!sw1==1 && !sw2==1) && flag) { + motor1F=0.3; + motor1B=0.3; + motor2F=0.3; + motor2B=0.3; + dir1=0; + dir2=0; + wait(0.5); + while((xya.a>5 && xya.a<355)) { + loc.get_angle(&xya); + turn(0); + } + flag=false; + } else if((!sw1==1 && !sw2==1) && !flag) { + motor1F=0.3; + motor1B=0.3; + motor2F=0.3; + motor2B=0.3; + dir1=0; + dir2=0; + wait(0.5); + while(abs(xya.a-180)>=5) { + loc.get_angle(&xya); + turn(180); + } + flag=true; + } else { + float s=0; + if(loc._rx_n<=10) + s=0.1; + else if(loc._rx_n<=30) + s=0.4*abs(loc._rx_n-30)/30+0.1; + else + s=0.5; + motor1F=s; + motor1B=s; + motor2F=s; + motor2B=s; + dir1=1; + dir2=1; + } + + wdt.kick(); } } + +void send() +{ + pc.printf("%c%c%c%c\n",(char)loc._rx_n,(char)loc._ry_n,xya.a/10,xya.a%10); +} + +void turn(int angle) +{ + float s = 0; + if(angle==180) { + if(abs(xya.a-angle)<=5) + s=SPEED_TURN_MIN; + else + s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN; + motor1F=s; + motor1B=s; + motor2F=s; + motor2B=s; + if(xya.a>angle+5) { + dir1=1; + dir2=0; + } else if(xya.a<angle-5) { + dir1=0; + dir2=1; + } else { + motor1F=0; + motor1B=0; + motor2F=0; + motor2B=0; + } + } else if(angle==0) { + if(xya.a>angle+5 && xya.a<180) { + s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN; + motor1F=s; + motor1B=s; + motor2F=s; + motor2B=s; + dir1=1; + dir2=0; + } else if(xya.a>180 && xya.a<355) { + s=(SPEED_TURN_MAX*abs(xya.a-359)/180)+SPEED_TURN_MIN; + motor1F=s; + motor1B=s; + motor2F=s; + motor2B=s; + dir1=0; + dir2=1; + } else { + motor1F=0; + motor1B=0; + motor2F=0; + motor2B=0; + } + } +} \ No newline at end of file