Diff: main.cpp

Revision:

40:ca4dd3faffa8

Parent:

37:da3a2c781672

diff -r da3a2c781672 -r ca4dd3faffa8 main.cpp
--- a/main.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ b/main.cpp	Wed Apr 13 11:27:01 2016 +0000
@@ -1,5 +1,5 @@
 #include "Odometry/Odometry.h"
-#include "Map/Map.h"
+#include "map/map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -27,33 +27,35 @@
 int main(void)
 {
     init();
-    /*double angle_v = 2*PI/5;
+    double angle_v = 2*PI/5;
     double distance_v = 200.0;
-    std::vector<SimplePoint> path;
-    Map map;
-    
+    float the;
     
-    //Construction des obstacles
-    map.build();
+    odo.setPos(0, 1000, 0);
+    point dep (odo.getX(), odo.getY());
+    point arr (1400, 1000);
     
-    float x=odo.getX();
-    float y=odo.getY();
-    float the = 0;
+    map m;
+    m.addObs(obsCarr (550, 1030, 250, 230));
+    m.addObs(obsCarr (740, 720, 240, 240));
+    m.addObs(obsCarr (1170, 1260, 220, 220));
     
-    map.AStar(x, y, 1400, 1000, 75);
-    path = map.path;
+    /*m.addObst (carre (1000, 1000, 210, 210));
+    m.addObst (carre (1300, 700, 180, 180));*/
+    m.FindWay (dep, arr);
+    logger.printf("On a cherche un chemin\n\r");
     
-    for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
-        odo.GotoXYT(path[i].x, path[i].y, the);
+    if (m.getEnded()) {
+        nVector<pointParcours> p = m.getParc ();
+        logger.printf ("Il y a %d points de parcours\n\r", p.size());
+        for (int i = 0; i < p.size (); i++) {
+            logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
+            //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
+            odo.GotoXY(p[i].getX(), p[i].getY());
+        }
     }
-    
-    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
-    path = map.path;
-    
-    for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
-        odo.GotoXYT(path[i].x, path[i].y, the);
+    else {
+        logger.printf("Chemin pas trouve ...");
     }
     
     //odo.GotoThet(PI);
@@ -62,37 +64,69 @@
     
     //odo.GotoThet(-PI/2);
     wait(2000);
+    while (1);
     //odo.GotoXYT(2250,500,0);*/
+    /*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("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;
+                    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("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("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 +142,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)