Diff: Functions/func.cpp

Revision:

47:5658af4e5149

Parent:

45:b53ae54062c6

Child:

48:03da1aead032

--- a/Functions/func.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Functions/func.cpp	Mon Apr 25 12:38:58 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);
 }
 
@@ -135,20 +135,36 @@
 
 void init_interrupt(void)
 {
+    CAMP.fall(&changeCamp);
+    CAMP.rise(&changeCampoff);
+    START.rise(&letsgo);
+    START.fall(&letsgooff);
     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
+}
+
+void changeCamp(void)
+{
+    LEDV = 1;
 }
 
-void update_main(void)
+void changeCampoff(void)
+{
+    LEDV = 0;
+}
+
+void letsgo(void)
 {
-    odo.update_odo();
-    checkAround();
+    LEDR = 0;
+}
+
+void letsgooff(void)
+{
+    LEDR = 1;
 }
\ No newline at end of file