Diff: main.cpp

Revision:

40:309f38d1e49c

Parent:

38:da3a2c781672

--- a/main.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ b/main.cpp	Tue Apr 05 15:02:12 2016 +0000
@@ -5,7 +5,6 @@
 #define ATTENTE 0
 #define GO 1
 #define STOP 2
-#define ENTRAXE 248.25
 
 InterruptIn mybutton(USER_BUTTON);
 DigitalIn button(USER_BUTTON);
@@ -14,7 +13,7 @@
 //Serial pc(USBTX, USBRX);
 Serial logger (PA_9, PA_10);
 RoboClaw roboclaw(460800, PA_11, PA_12);
-Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);
+Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
 int i = 0;
 
@@ -27,7 +26,9 @@
 int main(void)
 {
     init();
-    /*double angle_v = 2*PI/5;
+    /* Code AStar */
+    
+    double angle_v = 2*PI/5;
     double distance_v = 200.0;
     std::vector<SimplePoint> path;
     Map map;
@@ -40,7 +41,7 @@
     float y=odo.getY();
     float the = 0;
     
-    map.AStar(x, y, 1400, 1000, 75);
+    map.AStar(x, y, 1600, 1000, 75);
     path = map.path;
     
     for(int i=0; i<path.size();i++) {
@@ -62,37 +63,75 @@
     
     //odo.GotoThet(-PI/2);
     wait(2000);
-    //odo.GotoXYT(2250,500,0);*/
+    //odo.GotoXYT(2250,500,0);
+    
+    //odo.TestEntraxe(5);
+    //odo.Forward(1000);
+    
+    /* Code JPO :
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    int state = 0;
     while(1)
     {
-        while(logger.readable()) 
-        {
+       // while(logger.readable()) 
+        //{
             char c = logger.getc();
             if(c=='z')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, speed_angle);
+                if (state != 1) {
+                    state = 1;
+                    logger.printf("Avant (Z) \r\n");
+                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+                }
             }
             else if(c == 's')
             {
-                roboclaw.ForwardM1(ADR, 0);
-                roboclaw.ForwardM2(ADR, 0);
+                if (state != 2) {
+                    state = 2;
+                    logger.printf("Stop (S) \r\n");
+                    roboclaw.ForwardM1(ADR, 0);
+                    roboclaw.ForwardM2(ADR, 0);
+                }
             }
             else if(c == 'd')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, speed_angle);
+                if (state != 3) {
+                    state = 3;
+                    logger.printf("Droite (D) \r\n");
+                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+                    roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+                }
             }
             else if(c == 'q')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, -speed_angle);
+                if (state != 4) {
+                    state = 4;
+                    logger.printf("Gauche (Q)\r\n");
+                    roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
+                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+                }
             }
             else if(c == 'x')
             {
-                roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, -speed_angle);
+                if (state != 5) {
+                    state = 5;
+                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
+                }
             }
-        }
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-    }
+            else if (c == '\0') { ; }
+            else {
+                if (state != 0) {
+                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
+                    state = 0;
+                }
+            }
+       // }
+       // roboclaw.ForwardM1(ADR, 0);
+       // roboclaw.ForwardM2(ADR, 0);
+       
+    }*/
 }
 
 void init(void)
@@ -108,9 +147,10 @@
     while(button);
     wait(1);
     mybutton.fall(&pressed);
-    mybutton.rise(&unpr
-    essed);
+    mybutton.rise(&unpressed);
     ticker.attach_us(&update_main,dt); // 100 Hz
+    
+    logger.printf("End init\n\r");
 }
 
 void update_main(void)