Diff: main.cpp

Revision:

39:09c04fd42c94

Parent:

38:28f476eacde4

Child:

40:83ce8d1072ef

--- a/main.cpp	Tue Mar 10 22:10:04 2015 +0000
+++ b/main.cpp	Mon Mar 16 21:46:12 2015 +0000
@@ -1,12 +1,16 @@
 #include "mbed.h"
+
+#include "defines.h"
+
 #include "QEI.h"
 #include "Map.h"
+#include "AX12.h"
 
 #include "Asserv.h"
 #include "Motor.h"
 /*----------------------------------------------------------------------------------------------*/
-    /*Serial*/    
-    Serial pcs(USBTX, USBRX); // tx, rx
+/*Serial*/    
+Serial logger(OUT_TX, OUT_RX); // tx, rx
 /*----------------------------------------------------------------------------------------------*/
 
 /* --- Initialisation de la liste des obstable --- */
@@ -14,7 +18,7 @@
 
 int main()
 {
-    pcs.printf("Initialisation...");
+    logger.printf("Initialisation...\r\n");
     //mbed
     /*
     PwmOut pw1(p22);
@@ -47,8 +51,15 @@
         
     Motor motorR(&pw1,&dir1);
     Motor motorL(&pw2,&dir2);    
-    pcs.printf("mise a jour des pwm.\n");
-    Timer t;    
+    logger.printf("mise a jour des pwm.\r\n");
+    Timer t;
+    
+    AX12 test(PA_9,NC,0x05,250000);
+    AnalogIn ain0(PA_0);
+    AnalogIn ain1(PA_1);
+    AnalogIn ain2(PA_4);
+    AnalogIn ain3(PB_0);
+    AnalogIn ain4(PC_1);
     
     /*----------------------------------------------------------------------------------------------*/
     /*Odometry*/
@@ -56,12 +67,24 @@
     QEI qei_right(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
     Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
     DigitalOut led(LED1);
-    while(1)
+    
+    test.SetMode(0); // Position
+    
+    char dataOff[] = {0};
+    char dataOn[] = {1};
+    /*while(1)
     {
-        blue.printf("x: %f\ty: %f\ttheta: %f\r\n",odometry.getX(),odometry.getY(),odometry.getTheta());
+        logger.printf("0: %f\r\n",ain0);
+        logger.printf("1: %f\r\n",ain1);
+        logger.printf("2: %f\r\n",ain2);
+        logger.printf("3: %f\r\n",ain3);
+        logger.printf("4: %f\r\n\r\n",ain4);
         led = !led;
         wait(0.5);
-    }
+        test.write(0x05,0x19,1,dataOff);
+        wait(0.5);
+        test.write(0x05,0x19,1,dataOn);
+    }*/
     bool testOdo = true;
     
     
@@ -80,7 +103,7 @@
         float phi_r = instanceAsserv.getPhiR();
         float phi_l = instanceAsserv.getPhiL();
         float phi_max = instanceAsserv.getPhiMax();
-        blue.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t phiD = %f ;\t phiG = %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 ;\t phiD = %f ;\t phiG = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
         //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();
@@ -93,11 +116,11 @@
             motorR.update((float)phi_r/phi_max);        
             motorL.update((float)phi_l/phi_max);
         }            
-        pcs.printf("\n\n----------------- Commandes moteurs : mises a jour. ------------------ \n");
+        logger.printf("\n\n----------------- Commandes moteurs : mises a jour. ------------------ \n");
                 
         //Timer Handling :
         t.stop();
-        pcs.printf("%f s ecoulee.\n\r", t.read());
+        logger.printf("%f s ecoulee.\n\r", t.read());
         t.reset();
         t.start();                
     }