Revision 49:5e2f7323f280, committed 2016-04-28

Comitter:

sype

Date:

Thu Apr 28 08:24:02 2016 +0000

Parent:

47:be4eebf40568

Parent:

48:03da1aead032

Child:

50:2dea82393beb

Commit message:

On monte le moteur, on d?-commente certaines lignes et c'est parti baby !

Changed in this revision

--- a/AX12/AX12.h	Mon Apr 25 12:42:32 2016 +0000
+++ b/AX12/AX12.h	Thu Apr 28 08:24:02 2016 +0000
@@ -48,7 +48,7 @@
 #define AX12_CW 1
 #define AX12_CCW 0
 
-#/** Servo control class, based on a PwmOut
+/** Servo control class, based on a PwmOut
  *
  * Example:
  * @code

--- a/Functions/func.cpp	Mon Apr 25 12:42:32 2016 +0000
+++ b/Functions/func.cpp	Thu Apr 28 08:24:02 2016 +0000
@@ -2,106 +2,106 @@
 
 void pressed(void)
 {
-    if(i==0) {
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-        i++;
+    if(SIMON_i==0) {
+        roboclaw.ForwardM1(0);
+        roboclaw.ForwardM2(0);
+        SIMON_i++;
     }
 }
 
 void unpressed(void)
 {
-    if(i==1) {
-        i--;
+    if(SIMON_i==1) {
+        SIMON_i--;
     }
 }
 
 void ELpressed(void)
 {
-    //bleu = 1;
-    EL = true;
+    bleu = 1;
+    SIMON_EL = true;
 }
 void ELunpressed(void)
 {
-    //bleu = 0;
-    EL = false;
+    bleu = 0;
+    SIMON_EL = false;
 }
 
 void EZpressed(void)
 {
-    //blanc = 1;
-    EZ = true;
+    blanc = 1;
+    SIMON_EZ = true;
 }
 void EZunpressed(void)
 {
-    //blanc = 0;
-    EZ = false;
+    blanc = 0;
+    SIMON_EZ = false;
 }
 
 void ERpressed(void)
 {
-    //rouge = 1;
-    ER = true;
+    rouge = 1;
+    SIMON_ER = true;
 }
 void ERunpressed(void)
 {
-    //rouge = 0;
-    ER = 0;
+    rouge = 0;
+    SIMON_ER = 0;
 }
 
 void JPO(void)
 {
     char c = logger.getc();
     if(c=='z') {
-        if (state != 1) {
-            state = 1;
+        if (SIMON_state != 1) {
+            SIMON_state = 1;
             logger.printf("Avant (Z) \r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
         }
     } else if(c == 's') {
-        if (state != 2) {
-            state = 2;
+        if (SIMON_state != 2) {
+            SIMON_state = 2;
             logger.printf("Stop (S) \r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
+            roboclaw.SpeedAccelM1(accel_angle, 0);
+            roboclaw.SpeedAccelM2(accel_angle, 0);
         }
     } else if(c == 'd') {
-        if (state != 3) {
-            state = 3;
+        if (SIMON_state != 3) {
+            SIMON_state = 3;
             logger.printf("Droite (D) \r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
         }
     } else if(c == 'q') {
-        if (state != 4) {
-            state = 4;
+        if (SIMON_state != 4) {
+            SIMON_state = 4;
             logger.printf("Gauche (Q)\r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
         }
     } else if(c == 'x') {
-        if (state != 5) {
-            state = 5;
-            roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+        if (SIMON_state != 5) {
+            SIMON_state = 5;
+            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
         }
     } else {
-        if (state != 0) {
-            roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
-            state = 0;
+        if (SIMON_state != 0) {
+            roboclaw.SpeedAccelM1M2(accel_angle, 0, accel_angle, 0);
+            SIMON_state = 0;
         }
     }
 }
 
 void goHome(void)
 {
-    while(EZ==false) ZMot.step(1, 0, DELAY);
-    while(ER==false) RMot.step(1, 0, DELAY);
-    RMot.step(10, 1, DELAY);
+    //while(EZ==false) ZMot.step(1, 0, DELAY);
+    while(SIMON_ER==false) RMot.step(1, 0, DELAY);
+    RMot.step(10, 1, DELAY+0.01);
     RMot.step(5, 0, DELAY);
-    while(EL==false) LMot.step(1, 0, DELAY);
-    LMot.step(10, 1, DELAY);
+    while(SIMON_EL==false) LMot.step(1, 0, DELAY);
+    LMot.step(10, 1, DELAY+0.01);
     LMot.step(5, 0, DELAY);
 }
 
@@ -115,7 +115,7 @@
     else rouge = 0;
 }
 
-void init_ax12(void)
+/*void init_ax12(void)
 {
     left_hand.setMode(0);
     wait_ms(10);
@@ -131,24 +131,38 @@
     right_hand.setGoal(180);
     left_hand.setGoal(180);
     wait(2);
-}
+}*/
 
 void init_interrupt(void)
 {
+    CAMP.fall(&changeCamp);
+    START.rise(&go);
     mybutton.fall(&pressed);
     mybutton.rise(&unpressed);
-
     EndL.fall(&ELpressed);
     EndL.rise(&ELunpressed);
     EndZ.fall(&EZpressed);
     EndZ.rise(&EZunpressed);
     EndR.fall(&ERpressed);
     EndR.rise(&ERunpressed);
-    ticker.attach_us(&update_main,dt); // 100 Hz
+    ticker.attach_us(&update_main, dt); // 100 Hz
 }
 
-void update_main(void)
+void changeCamp(void)
 {
-    odo.update_odo();
-    checkAround();
+    if(SCouleur==VERT) {
+        SCouleur = VIOLET;
+        LEDR = 1;
+        LEDV = 0;
+    }
+    else {
+        SCouleur = VERT;
+        LEDV = 1;
+        LEDR = 0;        
+    }
+}
+
+void go(void)
+{
+    SStart = 1;
 }
\ No newline at end of file

--- a/Functions/func.h	Mon Apr 25 12:42:32 2016 +0000
+++ b/Functions/func.h	Thu Apr 28 08:24:02 2016 +0000
@@ -9,6 +9,12 @@
 #include "AX12.h"
 
 #define SEUIL 0.25
+#define VERT 1
+#define VIOLET 2
+#define ATTENTE 0
+#define GO 1
+#define STOP 2
+#define ADR 0x80
 #define dt 10000
 
 extern Serial logger;
@@ -26,14 +32,21 @@
 extern InterruptIn EndR;
 extern InterruptIn EndZ;
 extern InterruptIn EndL;
-extern AX12 left_hand;
-extern AX12 right_hand;
-extern Ticker ticker;
+//extern AX12 left_hand;
+//extern AX12 right_hand;
 extern Odometry odo;
+extern Ticker ticker;
+extern InterruptIn CAMP;
+extern InterruptIn START;
+extern DigitalOut LEDR;
+extern DigitalOut LEDV;
 
-extern int i, state;
-extern bool EL, EZ, ER;
 
+extern int SIMON_i, SIMON_state, SCouleur, SStart;
+extern bool SIMON_EL, SIMON_EZ, SIMON_ER;
+
+void changeCamp(void);
+void go(void);
 void ELpressed(void);
 void ELunpressed(void);
 void EZpressed(void);

--- a/Odometry/Odometry.cpp	Mon Apr 25 12:42:32 2016 +0000
+++ b/Odometry/Odometry.cpp	Thu Apr 28 08:24:02 2016 +0000
@@ -8,9 +8,9 @@
     m_distPerTick_left = diameter_left*PI/quadrature;
     m_distPerTick_right = diameter_right*PI/quadrature;
 
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    roboclaw.ResetEnc(ADR);
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    roboclaw.ResetEnc();
     // Erreur autorisée sur le déplacement en angle
     erreur_ang = 0.01;
 
@@ -28,7 +28,7 @@
 }
 void Odometry::getEnc()
 {
-    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
+    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(), roboclaw.ReadEncM2());
 }
 
 void Odometry::setX(double x)
@@ -48,8 +48,8 @@
 
 void Odometry::update_odo(void)
 {
-    int32_t roboclawENCM1 = roboclaw.ReadEncM1(ADR); 
-    int32_t roboclawENCM2 = roboclaw.ReadEncM2(ADR); 
+    int32_t roboclawENCM1 = roboclaw.ReadEncM1(); 
+    int32_t roboclawENCM2 = roboclaw.ReadEncM2();
     int32_t delta_right = roboclawENCM1 - m_pulses_right;
     m_pulses_right = roboclawENCM1;
     int32_t delta_left = roboclawENCM2 - m_pulses_left;
@@ -84,7 +84,6 @@
     theta += deltaTheta;
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
-    
 }
 
 void Odometry::GotoXY(double x_goal, double y_goal)
@@ -108,7 +107,6 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    led = 0;
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -138,7 +136,7 @@
     //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
     //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
 
     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
 
@@ -148,14 +146,12 @@
     
     wait(0.4);
     setTheta(theta_);
-    led = 1;
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
 }
 
 void Odometry::GotoDist(double distance)
 {
-    led = 0;
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
@@ -165,13 +161,12 @@
     int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
     int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
     
     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
     wait(0.4);
-    led = 1;
     //logger.printf("arrivey %d\n\r",pos_prog);
     //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
@@ -191,7 +186,7 @@
         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     }
     
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
     
     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left))
         logger.printf ("[Dist] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left);
@@ -205,11 +200,10 @@
     int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
     int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
     
     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
     wait(0.4);
-    led = 1;
 }
\ No newline at end of file

--- a/Odometry/Odometry.h	Mon Apr 25 12:42:32 2016 +0000
+++ b/Odometry/Odometry.h	Thu Apr 28 08:24:02 2016 +0000
@@ -8,13 +8,13 @@
 #define C 1.0
 
 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
-#define accel_angle 8000
-#define vitesse_angle 12000
-#define deccel_angle 8000
+#define accel_angle 0x1000
+#define vitesse_angle 0x1000
+#define deccel_angle 0x1000
 
-#define accel_dista 12000
-#define vitesse_dista 20000
-#define deccel_dista 12000
+#define accel_dista 0x1000
+#define vitesse_dista 0x1000
+#define deccel_dista 0x1000
 
 /* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
 #define ENTRAXE 243.8
@@ -25,7 +25,6 @@
 */
 
 extern Serial logger;
-extern DigitalOut led;
 
 /** Permet la gestion de l'odometrie du robot **/
 class Odometry

--- a/RoboClaw.lib	Mon Apr 25 12:42:32 2016 +0000
+++ b/RoboClaw.lib	Thu Apr 28 08:24:02 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#ece8a8cdf4b0
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#0fc5514faed9

--- a/main.cpp	Mon Apr 25 12:42:32 2016 +0000
+++ b/main.cpp	Thu Apr 28 08:24:02 2016 +0000
@@ -1,21 +1,28 @@
 #include "func.h"
 #include "map.h"
 
-#define ATTENTE 0
-#define GO 1
-#define STOP 2
+/* Déclaration des différents éléments de l'IHM */
 
-/* Déclaration des différents éléments de l'IHM */
+InterruptIn CAMP(PA_15);
+InterruptIn START(PB_7);
+DigitalOut LEDR(PC_2);
+DigitalOut LEDV(PC_3);
+
 InterruptIn mybutton(USER_BUTTON);
 DigitalIn button(USER_BUTTON);
-DigitalOut led(LED1);
+
+DigitalIn ChannelA1(PA_1);
+DigitalIn ChannelB1(PA_0);
+DigitalIn ChannelA2(PA_5);
+DigitalIn ChannelB2(PA_6);
+
 DigitalOut bleu(PC_5);
 DigitalOut blanc(PC_6);
 DigitalOut rouge(PC_8);
 
-/* AX12 */
-AX12 left_hand(PA_15, PB_7, 4, 250000);
-AX12 right_hand(PA_15, PB_7, 1, 250000);
+/* AX12 - non fonctionnels, je pense que le souci vient de la bibliothèque car la trame n'est pas correcte (analyseur logique) */
+//AX12 left_hand(PA_9, PA_10, 4, 250000);
+//AX12 right_hand(PA_9, PA_10, 1, 250000);
 
 /* Sharp */
 AnalogIn capt1(PA_4);
@@ -24,25 +31,25 @@
 AnalogIn capt4(PC_0);
 
 /* Moteurs pas à pas */
-Stepper RMot(NC, PB_10, PA_8);
-Stepper ZMot(NC, PB_5, PB_4);
-Stepper LMot(NC, PB_3, PA_10);
+Stepper RMot(NC, PA_8, PC_7);
+Stepper ZMot(NC, PB_4, PB_10);
+Stepper LMot(NC, PB_5, PB_3);
 /* Fins de course */
 InterruptIn EndR(PB_15);
 InterruptIn EndZ(PB_14);
 InterruptIn EndL(PB_13);
 
 Ticker ticker;
-
 //Serial logger(USBTX, USBRX);
 Serial logger(PA_9, PA_10);
-RoboClaw roboclaw(460800, PA_11, PA_12);
+RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
-int i = 0, state = 0;
-bool EL = false, EZ = false, ER = false;
+int SIMON_i = 0, SIMON_state = 0, SCouleur = 1, SStart = 0;
+bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false;
 
 void init(void);
+void update_main(void);
 
 /* Debut du programme */
 int main(void)
@@ -60,31 +67,39 @@
     m.Execute(110,1000);
     
     odo.GotoThet(0);
-    
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
     
     logger.printf ("Chemin Fini !");*/
-    odo.GotoXY(500,1000);
-    while (1);
+    while(1);
 }
 
 void init(void)
 {
-    odo.setPos(110, 1000, 0);
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
+    
     bleu = 1, blanc = 1, rouge = 1;
+    
+    odo.setPos(110, 1000, 0);
+    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);    
     wait_ms(500);
+    
     bleu = 0, blanc = 0, rouge = 0;
     
-    while(button);
-    wait(1);
+    while(SStart==0);
     
-    init_ax12();
+    roboclaw.ResetEnc();
     init_interrupt();
     wait_ms(100);
+    
     logger.printf("End init\n\r");
 }
+
+void update_main(void)
+{
+    odo.update_odo();
+    //checkAround();
+}
\ No newline at end of file

--- a/main.cpp.orig	Mon Apr 25 12:42:32 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,102 +0,0 @@
-#include "Odometry.h"
-/** Dependencies :
-    Map/Point.h
-    Map/define.h
-    Map/Obstacles/Obstacle.h
-
-    Test en cours :
-    * Un obstacle seulement
-    * Radius du robot réduit à 1 (defines.cpp)
-    * Nombreux
-*/ 
-#include "Map/Map.h"
-
-#define dt 10000
-#define ATTENTE 0
-#define GO 1
-#define STOP 2
-
-InterruptIn mybutton(USER_BUTTON);
-DigitalIn button(USER_BUTTON);
-DigitalOut led(LED1);
-Ticker ticker;
-//Serial pc(USBTX, USBRX);
-Serial pc(PA_9, PA_10);
-RoboClaw roboclaw(460800, PA_11, PA_12);
-Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
-
-int i = 0;
-
-void update_main(void);
-void init(void);
-void pressed(void);
-void unpressed(void);
-
-/** Debut du programme */
-int main(void)
-{
-    double angle_v = 2*PI/5;
-    double distance_v = 200.0;
-    std::vector<SimplePoint> path;
-    Map map;
-    
-    init();
-    //Construction des obstacles
-    map.build();
-    map.Astar(0, 1000, 2000, 1000);
-    path = map.path;
-    
-    for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, 0);
-    }
-        
-    //odo.GotoXYT(500, 50, 0);
-    //odo.GotoXYT(200, 0, 0);
-    //odo.GotoXYT(0, 0, 0);
-
-    //odo.GotoThet(-PI/2);
-    wait(2000);
-    //odo.GotoXYT(2250,500,0);
-    while(1);
-}
-
-void init(void)
-{
-    pc.baud(9600);
-    pc.printf("Hello from main !\n\r");
-    wait_ms(500);
-    
-    odo.setPos(0, 0, 0);
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    
-    while(button);
-    wait(1);
-    mybutton.fall(&pressed);
-    mybutton.rise(&unpressed);
-    ticker.attach_us(&update_main,dt); // 100 Hz
-}
-
-void update_main(void)
-{
-    odo.update_odo();
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-    //if(pc.readable()) if(pc.getc()=='l') led = !led;
-}
-
-void pressed(void)
-{
-    if(i==0) {
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-        i++;
-    }
-}
-
-void unpressed(void)
-{
-    if(i==1) {
-        i--;
-    }
-}
\ No newline at end of file