Diff: main.cpp

Revision:

74:88be86f83d17

Parent:

73:d8e1b543fbe3

Child:

76:a0c09fd62be1

--- a/main.cpp	Sat Apr 11 10:36:56 2015 +0000
+++ b/main.cpp	Mon Apr 13 16:53:19 2015 +0000
@@ -1,12 +1,13 @@
 #include "mbed.h"
 
+#define PLAN_A
+
 #include "defines.h"
 
 #include "QEI.h"
 #include "Map.h"
 #include "AX12.h"
 
-#define PLAN_A
 
 #ifdef PLAN_A
     #include "Odometry.h"
@@ -20,8 +21,6 @@
 /*----------------------------------------------------------------------------------------------*/
 /*Serial*/    
 Serial logger(OUT_TX, OUT_RX); // tx, rx
-//Serial logger(USBTX,USBRX);
-//logger.baud((int)115200);
 /*----------------------------------------------------------------------------------------------*/
 
 /* --- Initialisation de la liste des obstable --- */
@@ -31,6 +30,21 @@
 {
     logger.printf("Initialisation...\r\n");
     
+    /*AX12 test(PA_9,NC,5,250000);
+    test.setMode(0);
+    
+    while(1)
+    {
+        test.setMaxTorque(0x1FF);
+        test.setGoal(0);
+        logger.printf("0\r\n");
+        wait(2);
+        test.setMaxTorque(0x1FF);
+        test.setGoal(90);
+        logger.printf("180\r\n");
+        wait(4);
+    }*/
+    
     PwmOut pw1(PWM_MOT1);
     DigitalOut dir1(DIR_MOT1);
     PwmOut pw2(PWM_MOT2);
@@ -78,37 +92,27 @@
         Asserv<float> instanceAsserv(&odometry);
         instanceAsserv.setGoal( (float)200,(float)0, (float)0);
         
+        char state = 0;
+        
         t.start();
         t.reset();
         
         while(!testOdo)
         {
-            //logger.printf("%f %f\r\n",odometry.getVitLeft(),odometry.getVitRight());
             float dt = t.read();
             t.reset();
-            //t.start();
+            
             odometry.update(dt);
-            instanceAsserv.update(dt);        
-            //Mat<float> X( instanceAsserv.getX() );        
+            instanceAsserv.update(dt);
             float phi_r = (float)instanceAsserv.getPhiR();
             float phi_l = (float)instanceAsserv.getPhiL();
-            float phi_max = (float)instanceAsserv.getPhiMax();        
-            //logger.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t VD = %f ;\t W = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
-            //logger.printf(" x : %f ;\t y : %f ;\t theta : %f ",X.get(1,1),X.get(2,1),X.get(3,1) );
-            
-            //logger.printf("PhiR = %f ; \t PhiL = %f ; \n\r",phi_r,phi_l);//X.get(4,1),X.get(5,1));
-            
-            //transpose(X).afficherMblue();
-            
+            float phi_max = (float)instanceAsserv.getPhiMax();
             
-            //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                                            
-            //blue.printf("State : \n\r");
-            //(instanceAsserv.getX()).afficherMblue();
-            //blue.printf("Odometry : \n\r");
-            //z.afficherMblue();  
+            motorR.setSpeed(0.08+(phi_r/phi_max));
+            motorL.setSpeed(0.06+(phi_l/phi_max));
             
-            motorR.setSpeed(0.08+(phi_r/phi_max) );
-            motorL.setSpeed(0.06+(phi_l/phi_max) );                   
+            if(state == 0 && instanceAsserv.
+            
         }
     #else
         aserv_planB asserv(odometry,motorL,motorR);
@@ -136,38 +140,4 @@
             wait(0.1);             
         }
     #endif
-    
-    
-    
-    
-    /*int i=0;
-    int nbrech = 100;
-    float tableR[nbrech], tableL[nbrech];
-    
-    motorR.setSpeed(0.0);
-    motorL.setSpeed(0.0);
-    while(i<nbrech)
-    {        
-        tableR[i] = odometry.getPhiright();
-        tableL[i] = odometry.getPhileft();
-        motorR.setSpeed( ((float)i)/100);
-        motorL.setSpeed( ((float)i)/100);
-        logger.printf("vitesse = %f \r\n", (float)i/100);
-        wait(0.5);
-        i++;            
-    }
-    
-    i=0;
-    while(1)
-    {        
-        motorR.setSpeed(0.0);
-        motorL.setSpeed(0.0);
-        
-        if(i<nbrech)
-        {
-            logger.printf("%f , %f \r\n", tableL[i],tableR[i]);
-            wait(1);
-            i++;
-        }
-    }*/
 }