DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: main.cpp
- Revision:
- 15:7729da55873a
- Parent:
- 14:4839989ae907
- Child:
- 16:d6f15a13c3aa
--- a/main.cpp Thu Mar 24 03:54:29 2016 +0000 +++ b/main.cpp Thu Mar 24 07:20:51 2016 +0000 @@ -2,9 +2,6 @@ #include "LOCOMOTION.h" #include "WATCHDOG.h" - -#define SPEED_TURN_MIN 0.15 -#define SPEED_TURN_MAX 0.35 #define SPEED_FB_MIN 0.15 #define SPEED_FB_MAX 0.50 @@ -15,41 +12,29 @@ I2C i2c1(p28, p27); I2C i2c2(p9, p10); -LOCALIZE loc(i2c1, i2c2, p26); +LOCALIZE loc(i2c1, i2c2, p26, p20, p19, p18, p17); LOCALIZE_xya xya; - -DigitalOut dir1(p15); -DigitalOut dir2(p16); - -PwmOut motor1F(p21); -PwmOut motor1B(p22); -PwmOut motor2F(p23); -PwmOut motor2B(p24); +LOCOMOTION motion(p21, p22, p23, p24, p15, p16); Ticker t; bool flag=false; void send(); -void setAngle(int angle); +//void setAngle(int angle); int wrap(int a); int main() { wdt.kick(5); pc.baud(9600); - dir1=0; - dir2=0; - motor1F=0; - motor1B=0; - motor2F=0; - motor2B=0; //pc.printf("Initialized Localization: %d\n",loc.init()); t.attach(&send,1); while(1) { + motion.setAngle(0,0,5,ANGLE_TURN); //loc.get_angle(&xya); loc.get_xy(&xya); //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); - setAngle(0); + //setAngle(0); wdt.kick(); } } @@ -58,7 +43,7 @@ { pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); } - +/* void setAngle(int angle) { float s = 0; @@ -84,7 +69,7 @@ motor2F=0; motor2B=0; } -} +}*/ int wrap(int a) {