![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 10:cf44f4387bc7
- Parent:
- 9:4fc5d70c3bab
- Child:
- 11:9518f8285906
diff -r 4fc5d70c3bab -r cf44f4387bc7 main.cpp --- a/main.cpp Wed Mar 23 07:53:32 2016 +0000 +++ b/main.cpp Wed Mar 23 16:20:53 2016 +0000 @@ -2,7 +2,8 @@ #include "LOCOMOTION.h" #include "WATCHDOG.h" -#define SPEED_TURN_MIN 0.15 + +#define SPEED_TURN_MIN 0.20 #define SPEED_TURN_MAX 0.35 #define SPEED_FB_MIN 0.15 #define SPEED_FB_MAX 0.50 @@ -38,7 +39,8 @@ bool flag=false; void send(); -void turn(int angle); +void setAngle(int angle); +int wrap(int a); int main() { @@ -63,8 +65,8 @@ 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); + setAngle(20); wdt.kick(); } } @@ -74,52 +76,34 @@ pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); } -void turn(int angle) +void setAngle(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; - } + int diff = 0; + diff = 180-wrap(angle); + if(abs(xya.a-angle)<=5) + s=SPEED_TURN_MIN; + else + s=(SPEED_TURN_MAX*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN; + motor1F=s; + motor1B=s; + motor2F=s; + motor2B=s; + if(wrap(xya.a+diff)>180+2) { + dir1=1; + dir2=0; + } else if(wrap(xya.a+diff)<180-2) { + dir1=0; + dir2=1; + } else { + motor1F=0; + motor1B=0; + motor2F=0; + motor2B=0; } -} \ No newline at end of file +} + +int wrap(int a) +{ + return a%360; +}