Robot's source code

Dependencies:   mbed

Revision:
63:fd9af0693e50
Parent:
62:454cd844fe1e
Child:
65:b5d7f13870be
--- a/main.cpp	Thu Apr 09 17:05:06 2015 +0000
+++ b/main.cpp	Thu Apr 09 17:32:36 2015 +0000
@@ -36,7 +36,7 @@
     
     Timer t;
     
-    AX12 test(PA_9,NC,0x03,250000);
+    //AX12 test(PA_9,NC,0x03,250000);
     
     AnalogIn ain0(PA_0);
     AnalogIn ain1(PA_1);
@@ -50,35 +50,25 @@
     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);
+    DigitalOut led1(LED1);
+    DigitalOut led2(LED2);
+    DigitalOut led3(LED3);
     
-    test.SetMode(0); // Position
-    
+    t.start();
+    t.reset();
     while(1)
     {
-        test.SetGoal(0);
-        wait(1);
-        test.SetGoal(90);
-        wait(1);
+        odometry.update(t.read());
+        t.reset();
+        
+        logger.printf("---------\r\n");
+        logger.printf("%f %f %f\r\n",odometry.getX(),odometry.getY(),odometry.getTheta());
+        logger.printf("%f %f %f\r\n",odometry.getVitLeft(),odometry.getVitRight(),t.read());
+        t.reset();
+        wait(0.5);
+        motorL.setSpeed(1);
     }
     
-    
-    
-    /*char dataOff[] = {0};
-    char dataOn[] = {1};
-    while(1)
-    {
-        logger.printf("0: %f\r\n",ain0.read()*3.3);
-        logger.printf("1: %f\r\n",ain1.read()*3.3);
-        logger.printf("2: %f\r\n",ain2.read()*3.3);
-        logger.printf("3: %f\r\n",ain3.read()*3.3);
-        logger.printf("4: %f\r\n\r\n",ain4.read()*3.3);
-        led = !led;
-        wait(1);
-        motorR.setSpeed(1.0);
-        wait(1);
-        motorR.setSpeed(0.0);
-    }*/
     bool testOdo = false;
     
     
@@ -90,6 +80,7 @@
     aserv_planB asserv(odometry,motorL,motorR);
     
     t.start();
+    t.reset();
     
     while(!testOdo)
     {