Diff: main.cpp

Revision:

10:ae3178aa94e9

Parent:

9:e39b218ab20d

Child:

12:5355aed288b0

Child:

14:d5e21f71c2a9

--- a/main.cpp	Tue Nov 24 23:05:12 2015 +0000
+++ b/main.cpp	Fri Dec 04 11:18:13 2015 +0000
@@ -1,41 +1,81 @@
 #include "Odometry.h"
 
 #define dt 10000
+#define ATTENTE 0
+#define GO 1
+#define STOP 2
 
-InterruptIn button(USER_BUTTON);
+InterruptIn mybutton(USER_BUTTON);
+DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
 Ticker ticker;
-Serial pc(USBTX, USBRX);
-//Serial pc(PA_9, PA_10);
-RoboClaw roboclaw(115200, PA_11, PA_12);
-Odometry odo(47.4, 47.1, 242.0, roboclaw);
+//Serial pc(USBTX, USBRX);
+Serial pc(PA_9, PA_10);
+RoboClaw roboclaw(460800, PA_11, PA_12);
+Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
+
+int i = 0;
 
 void update_main(void);
 void init(void);
+void pressed(void);
+void unpressed(void);
 
 /** Debut du programme */
 int main(void)
-{   
+{
+    double angle_v = 2*PI/5;
+    double distance_v = 200.0;
     init();
-    odo.GotoXYT(2250,500,0);
+
+    odo.GotoXYT(500, 100, 0);
+    odo.GotoXYT(500, 50, 0);
+    //odo.GotoXYT(200, 0, 0);
+    //odo.GotoXYT(0, 0, 0);
+
+    //odo.GotoThet(-PI/2);
+    wait(2000);
+    //odo.GotoXYT(2250,500,0);
     while(1);
 }
 
 void init(void)
 {
-    pc.baud(115200);
+    pc.baud(9600);
     pc.printf("Hello from main !\n\r");
+    wait_ms(500);
+    
+    odo.setPos(0, 0, 0);
     roboclaw.ForwardM1(ADR, 0);
     roboclaw.ForwardM2(ADR, 0);
-    wait_ms(500);
-    roboclaw.ResetEnc(ADR);
-    odo.setPos(1500, 1000, 2*PI/3);
+    
+    while(button);
+    wait(1);
+    mybutton.fall(&pressed);
+    mybutton.rise(&unpressed);
     ticker.attach_us(&update_main,dt); // 100 Hz
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    //pc.printf("EncM1 : %6d\tEncM2 : %6d\tTheta : %3.2f\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR), odo.getTheta()*180/PI);
+    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
     //if(pc.readable()) if(pc.getc()=='l') led = !led;
 }
+
+void pressed(void)
+{
+    if(i==0) {
+        roboclaw.ForwardM1(ADR, 0);
+        roboclaw.ForwardM2(ADR, 0);
+        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
+        i++;
+    }
+}
+
+void unpressed(void)
+{
+    if(i==1) {
+        i--;
+    }
+}
\ No newline at end of file