Diff: Functions/func.cpp

Revision:

41:b5a2fbc20beb

Child:

44:b1fd7489369f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions/func.cpp	Wed Apr 13 11:27:34 2016 +0000
@@ -0,0 +1,134 @@
+#include "func.h"
+
+void pressed(void)
+{
+    if(i==0) {
+        roboclaw.ForwardM1(ADR, 0);
+        roboclaw.ForwardM2(ADR, 0);
+        i++;
+    }
+}
+
+void unpressed(void)
+{
+    if(i==1) {
+        i--;
+    }
+}
+
+void ELpressed(void)
+{
+    //bleu = 1;
+    EL = true;
+}
+void ELunpressed(void)
+{
+    //bleu = 0;
+    EL = false;
+}
+
+void EZpressed(void)
+{
+    //blanc = 1;
+    EZ = true;
+}
+void EZunpressed(void)
+{
+    //blanc = 0;
+    EZ = false;
+}
+
+void ERpressed(void)
+{
+    //rouge = 1;
+    ER = true;
+}
+void ERunpressed(void)
+{
+    //rouge = 0;
+    ER = 0;
+}
+
+void JPO(void)
+{
+    char c = logger.getc();
+    if(c=='z') {
+        if (state != 1) {
+            state = 1;
+            logger.printf("Avant (Z) \r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+        }
+    } else if(c == 's') {
+        if (state != 2) {
+            state = 2;
+            logger.printf("Stop (S) \r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
+        }
+    } else if(c == 'd') {
+        if (state != 3) {
+            state = 3;
+            logger.printf("Droite (D) \r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+        }
+    } else if(c == 'q') {
+        if (state != 4) {
+            state = 4;
+            logger.printf("Gauche (Q)\r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, 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);
+        }
+    } else {
+        if (state != 0) {
+            roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
+            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);
+    RMot.step(5, 0, DELAY);
+    while(EL==false) LMot.step(1, 0, DELAY);
+    LMot.step(10, 1, DELAY);
+    LMot.step(5, 0, DELAY);
+}
+
+void checkAround(void)
+{
+    if(capt1 > SEUIL+0.1) bleu = 1;
+    else bleu = 0;
+    if(capt2 > SEUIL) blanc = 1;
+    else blanc = 0;
+    if(capt3 > SEUIL+0.1) rouge = 1;
+    else rouge = 0;
+}
+
+void init_ax12(void)
+{
+    left_hand.setMode(0);
+    wait_ms(10);
+    right_hand.setMode(0);
+    wait_ms(50);
+    left_hand.setMode(0);
+    wait_ms(10);
+    right_hand.setMode(0);
+    wait_ms(50);
+    right_hand.setGoal(0);
+    left_hand.setGoal(0);
+    wait(2);
+    right_hand.setGoal(180);
+    left_hand.setGoal(180);
+    wait(2);
+}
\ No newline at end of file