Diff: Functions/func.cpp

Revision:

51:1056dd73a748

Parent:

48:03da1aead032

Child:

67:244bc1cc6678

Child:

77:f19cc7f81f2a

--- a/Functions/func.cpp	Mon May 02 08:35:56 2016 +0000
+++ b/Functions/func.cpp	Wed May 04 16:15:13 2016 +0000
@@ -1,5 +1,16 @@
 #include "func.h"
 
+void init_interrupt(void)
+{
+    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 pressed(void)
 {
     if(SIMON_i==0) {
@@ -88,7 +99,7 @@
         }
     } else {
         if (SIMON_state != 0) {
-            roboclaw.SpeedAccelM1M2(accel_angle, 0, accel_angle, 0);
+            roboclaw.SpeedAccelM1M2(accel_angle, 0, 0);
             SIMON_state = 0;
         }
     }
@@ -96,23 +107,57 @@
 
 void goHome(void)
 {
-    //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(SIMON_EL==false) LMot.step(1, 0, DELAY);
-    LMot.step(10, 1, DELAY+0.01);
-    LMot.step(5, 0, DELAY);
+    goHomeZ();
+    goHomeL();
+    goHomeR();
+}
+
+void goHomeL(void)
+{
+    while(EnL==true) LMot.step(1, BAS, DELAY);
+    LMot.step(10, HAUT, DELAY+0.01);
+    LMot.step(5, BAS, DELAY);
+}
+
+void goHomeZ(void)
+{
+    while(EnZ==true) ZMot.step(1, BAS, DELAY);
+    ZMot.step(10, HAUT, DELAY+0.01);
+    ZMot.step(5, BAS, DELAY);
+}
+
+void goHomeR(void)
+{
+    while(EnR==true) RMot.step(1, BAS, DELAY);
+    RMot.step(10, HAUT, DELAY+0.01);
+    RMot.step(5, BAS, 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;
+    if(capt1.read() > SEUIL+0.1) SGauche = true;
+    else SGauche = false;
+    if(capt2.read() > SEUIL) SDevant = true;
+    else SDevant = false;
+    if(capt3.read() > SEUIL+0.1) SDroite = true;
+    else SDroite = false;
+    logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
+}
+
+void homologation(void)
+{
+    goHomeZ();
+    ZMot.mm(50, HAUT);
+    goHomeL();
+    goHomeR();
+    LMot.mm(50, HAUT);
+    RMot.mm(50, HAUT);
+    while(START==0) logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
+    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
+    while(1) {
+        while(capt2.read() < SEUIL) checkAround();
+        while(1) roboclaw.SpeedAccelM1M2(accel_dista, 0, 0);
+    }
 }
 
 /*void init_ax12(void)
@@ -133,36 +178,39 @@
     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
-}
-
 void changeCamp(void)
 {
     if(SCouleur==VERT) {
         SCouleur = VIOLET;
         LEDR = 1;
         LEDV = 0;
-    }
-    else {
+    } else {
         SCouleur = VERT;
         LEDV = 1;
-        LEDR = 0;        
+        LEDR = 0;
     }
 }
 
-void go(void)
+void depart(void)
 {
-    SStart = 1;
-}
\ No newline at end of file
+    while(button==1) {
+        if(CAMP==0) {
+            changeCamp();
+        }
+        wait_ms(100);
+    }
+
+    if(SCouleur == VERT)logger.printf("Couleur VERT !\n\r");
+    else logger.printf("Couleur ROUGE !\n\r");
+
+    while(START==0) {
+        drapeau = SSchema;
+        if(CAMP==0) {
+            SSchema++;
+            if(SSchema == 6) SSchema = 1;
+            drapeau = SSchema;
+        }
+        wait_ms(100);
+    }
+    logger.printf("Schema numero : %d !\n\r", SSchema);
+}