Diff: main.cpp

Revision:

41:53d5990ff99d

Parent:

39:ca4dd3faffa8

--- a/main.cpp	Wed Apr 13 11:27:01 2016 +0000
+++ b/main.cpp	Wed Apr 13 16:17:19 2016 +0000
@@ -27,21 +27,37 @@
 int main(void)
 {
     init();
+    
     double angle_v = 2*PI/5;
     double distance_v = 200.0;
     float the;
     
-    odo.setPos(0, 1000, 0);
+    logger.printf("Yeah !\n\r");
+    
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
+    odo.setPos(110, 1000, 0);
+    
+    odo.GotoXY(1500,1500);
+    odo.GotoXY(1500,500);
+    odo.GotoXY(1000,1500);
+    odo.GotoXY(500,1000);
+    
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
+    /*
     point dep (odo.getX(), odo.getY());
-    point arr (1400, 1000);
+    point arr (1500, 1000);
     
     map m;
-    m.addObs(obsCarr (550, 1030, 250, 230));
-    m.addObs(obsCarr (740, 720, 240, 240));
+    m.addObs(obsCarr (550, 600, 50, 600));
     m.addObs(obsCarr (1170, 1260, 220, 220));
+
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
     
-    /*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");
     
@@ -50,6 +66,11 @@
         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());
+        }
+        logger.printf("\n\n\r");
+        
+        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());
         }
@@ -63,70 +84,13 @@
     //odo.TestEntraxe(3);
     
     //odo.GotoThet(-PI/2);
+    
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
     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()) 
-        //{
-            char c = logger.getc();
-            if(c=='z')
-            {
-                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')
-            {
-                if (state != 2) {
-                    state = 2;
-                    roboclaw.ForwardM1(ADR, 0);
-                    roboclaw.ForwardM2(ADR, 0);
-                }
-            }
-            else if(c == 'd')
-            {
-                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')
-            {
-                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')
-            {
-                if (state != 5) {
-                    state = 5;
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
-                }
-            }
-            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)