Diff: main.cpp

Revision:

2:abdf8c6823a1

Parent:

0:ad9600df4a70

Child:

3:62e9d715de65

--- a/main.cpp	Mon Nov 16 11:34:08 2015 +0000
+++ b/main.cpp	Tue Nov 24 15:02:01 2015 +0000
@@ -2,35 +2,39 @@
 
 #define dt 10000
 
+InterruptIn button(USER_BUTTON);
+DigitalOut led(LED1);
 Ticker ticker;
-//Timer t;
-
-Odometry odo(47.4, 47.2, 231.0);
-
+Serial pc(USBTX, USBRX);
 //Serial pc(PA_9, PA_10);
-//Serial pc(USBTX, USBRX);
+RoboClaw roboclaw(115200, PA_11, PA_12);
+Odometry odo(47.4, 47.1, 242.0, roboclaw);
 
 void update_main(void);
+void init(void);
 
 int main(void)
+{   
+    init();
+    odo.GotoXYT(1000,300,0);
+    while(1);
+}
+
+void init(void)
 {
-    odo.setPos(0, 0, 0);
-    //pc.baud(115200);
-    //pc.printf("Hello world !\n\r");
+    pc.baud(115200);
+    pc.printf("Hello from main !\n\r");
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    wait_ms(500);
+    roboclaw.ResetEnc(ADR);
+    odo.setPos(100, 1000, -PI/2);
     ticker.attach_us(&update_main,dt); // 100 Hz
-    wait_ms(1000);
-    
-    while(1)
-    {
-        //pc.printf("Enc1 : %6.2f\tSpeed1 :%6.2f\tEnc2 : %6.2f\tSpeed2 : %6.2f\n\r",enc1,speed1,enc2,speed2);
-        //pc.printf("Theta : %5d\n\r", _roboclaw.ReadEncM1(ADR));
-        wait_ms(10);
-    }
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    odo.GotoXYT(100,100,0);
-    //pc.printf("Theta : %3.3f\n\r", odo.getTheta()*180/PI);
+    //pc.printf("EncM1 : %6d\tEncM2 : %6d\tTheta : %3.2f\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR), odo.getTheta()*180/PI);
+    //if(pc.readable()) if(pc.getc()=='l') led = !led;
 }