Diff: main.cpp

Revision:

48:cb3ebbc27db3

Parent:

47:4909e97570f6

Child:

49:120da6c65e08

Child:

54:e0e58c36658a

--- a/main.cpp	Thu Mar 26 18:04:23 2015 +0000
+++ b/main.cpp	Fri Mar 27 19:49:22 2015 +0000
@@ -12,9 +12,9 @@
 #include "Motor.h"
 /*----------------------------------------------------------------------------------------------*/
 /*Serial*/    
-//Serial logger(OUT_TX, OUT_RX); // tx, rx
-Serial logger(USBTX,USBRX);
-logger.baud(115200);
+Serial logger(OUT_TX, OUT_RX); // tx, rx
+//Serial logger(USBTX,USBRX);
+//logger.baud((int)115200);
 /*----------------------------------------------------------------------------------------------*/
 
 /* --- Initialisation de la liste des obstable --- */
@@ -53,8 +53,8 @@
     DigitalOut dir2(DIR_MOT2);
     
         
-    Motor motorR(PWM_MOT1,DIR_MOT1);
-    Motor motorL(PWM_MOT2,DIR_MOT2);    
+    Motor motorL(PWM_MOT1,DIR_MOT1);
+    Motor motorR(PWM_MOT2,DIR_MOT2);    
     logger.printf("mise a jour des pwm.\r\n");
     Timer t;
     
@@ -67,8 +67,8 @@
     
     /*----------------------------------------------------------------------------------------------*/
     /*Odometry*/
-    QEI qei_left(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
-    QEI qei_right(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
+    QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
+    QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
     Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
     DigitalOut led(LED1);
     
@@ -89,13 +89,13 @@
         wait(1);
         motorR.setSpeed(0.0);
     }*/
-    bool testOdo = true;
+    bool testOdo = false;
     
     
     Asserv<float> instanceAsserv(&odometry);
     
     /*----------------------------------------------------------------------------------------------*/
-    instanceAsserv.setGoal( (float)200,(float)0, (float)0);        
+    instanceAsserv.setGoal( (float)150,(float)0, (float)PI/4);        
     
     while(!testOdo)
     {       
@@ -122,8 +122,8 @@
         //blue.printf("Odometry : \n\r");
         //z.afficherMblue();  
         
-        motorR.setSpeed(phi_r/phi_max);
-        motorL.setSpeed(phi_l/phi_max);     
+        motorR.setSpeed(0.08+(phi_r/phi_max) );
+        motorL.setSpeed(0.06+(phi_l/phi_max) );     
         
         //Timer Handling :
         t.stop();        
@@ -133,10 +133,34 @@
     }
         
     
+    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(1.0);
-        motorL.setSpeed(1.0);
-        wait(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++;
+        }
     }
 }