Diff: main.cpp

Revision:

80:5399183aa39b

Parent:

78:7c7cefbe1772

Parent:

79:d97090bb6470

Child:

82:3581a768f2db

Child:

83:6bcc38b1c5b5

--- a/main.cpp	Tue Apr 14 15:49:22 2015 +0000
+++ b/main.cpp	Tue Apr 14 18:30:54 2015 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 
-#define PLAN_A
+#define PLAN_B
 
 #include "defines.h"
 
@@ -8,7 +8,6 @@
 #include "Map.h"
 #include "AX12.h"
 
-
 #ifdef PLAN_A
     #include "Odometry.h"
     #include "Asserv.h"
@@ -20,7 +19,10 @@
 #include "Motor.h"
 /*----------------------------------------------------------------------------------------------*/
 /*Serial*/    
+
 Serial logger(OUT_TX, OUT_RX); // tx, rx
+//Serial logger(USBTX,USBRX);
+//logger.baud((int)115200);
 /*----------------------------------------------------------------------------------------------*/
 
 /* --- Initialisation de la liste des obstable --- */
@@ -70,7 +72,7 @@
         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);
+        Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255);
         odometry.setTheta(0);
         odometry.setX(0);
         odometry.setY(0);
@@ -78,7 +80,7 @@
         QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
         QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
         
-        Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,280);
+        Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255);
         
         odometry.setTheta(PI/2);
         odometry.setX(0);
@@ -139,7 +141,7 @@
         }
     #else
         aserv_planB asserv(odometry,motorL,motorR);
-        asserv.setGoal(-5000,-5000,0);
+        asserv.setGoal(1,-5000,0);
         
         t.start();
         t.reset();
@@ -149,9 +151,9 @@
             //Parametrage des coef par bluetooth
             while(logger.readable()) {
                 char c = logger.getc();
-                if(c=='a') asserv.Kp += 0.001;
-                else if(c=='z') asserv.Kp -= 0.001;
-                //logger.printf("%f\n\r",asserv.Kp);
+                if(c=='a') asserv.Kd += 0.001;
+                else if(c=='z') asserv.Kd -= 0.001;
+                logger.printf("%f\n\r",asserv.Kd);
             }
             
             //Asservissement :
@@ -160,7 +162,7 @@
             odometry.update(dt);
             asserv.update(dt);
             
-            wait(0.1);             
+            wait_ms(100);            
         }
     #endif
 }