Diff: Functions/func.cpp

Revision:

78:c28bdbf29b6e

Parent:

77:f19cc7f81f2a

--- a/Functions/func.cpp	Thu May 05 08:46:08 2016 +0000
+++ b/Functions/func.cpp	Thu May 05 13:17:00 2016 +0000
@@ -1,13 +1,24 @@
+#include <cmath>
+
 #include "func.h"
 
+int max(int a, int b)
+{
+    return (a>b)?a:b;
+}
+
+int max(int a, int b, int c)
+{
+    return max(a,max(b,c));
+}
+
+int min(int a, int b)
+{
+    return (a<b)?a:b;
+}
+
 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
 }
 
@@ -27,137 +38,25 @@
     }
 }
 
-void ELpressed(void)
-{
-    bleu = 1;
-    SIMON_EL = true;
-}
-void ELunpressed(void)
-{
-    bleu = 0;
-    SIMON_EL = false;
-}
-
-void EZpressed(void)
-{
-    blanc = 1;
-    SIMON_EZ = true;
-}
-void EZunpressed(void)
-{
-    blanc = 0;
-    SIMON_EZ = false;
-}
-
-void ERpressed(void)
-{
-    rouge = 1;
-    SIMON_ER = true;
-}
-void ERunpressed(void)
-{
-    rouge = 0;
-    SIMON_ER = 0;
-}
-
-void JPO(void)
-{
-    char c = logger.getc();
-    if(c=='z') {
-        if (SIMON_state != 1) {
-            SIMON_state = 1;
-            logger.printf("Avant (Z) \r\n");
-            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
-            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
-        }
-    } else if(c == 's') {
-        if (SIMON_state != 2) {
-            SIMON_state = 2;
-            logger.printf("Stop (S) \r\n");
-            roboclaw.SpeedAccelM1(accel_angle, 0);
-            roboclaw.SpeedAccelM2(accel_angle, 0);
-        }
-    } else if(c == 'd') {
-        if (SIMON_state != 3) {
-            SIMON_state = 3;
-            logger.printf("Droite (D) \r\n");
-            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
-            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
-        }
-    } else if(c == 'q') {
-        if (SIMON_state != 4) {
-            SIMON_state = 4;
-            logger.printf("Gauche (Q)\r\n");
-            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
-            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
-        }
-    } else if(c == 'x') {
-        if (SIMON_state != 5) {
-            SIMON_state = 5;
-            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
-            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
-        }
-    } else {
-        if (SIMON_state != 0) {
-            roboclaw.SpeedAccelM1M2(accel_angle, 0, 0);
-            SIMON_state = 0;
-        }
-    }
-}
-
-void goHome(void)
-{
-    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.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);
-    }
+    SDroite += (capt1.read() > seuils[0]?1:-1);
+    SDroite = min(SDroite, 5);
+    SDroite = max(SDroite, -5);
+    
+    SDevant += (capt2.read() > seuils[1]?1:-1);
+    SDevant = min(SDevant, 5);
+    SDevant = max(SDevant, -5);
+    
+    SGauche += (capt3.read() > seuils[2]?1:-1);
+    SGauche = min(SGauche, 5);
+    SGauche = max(SGauche, -5);
+    
+    drapeau[0] = SGauche>0;
+    drapeau[1] = SDevant>0;
+    drapeau[2] = SDroite>0;
+    
+    logger.printf("Sharps : %d %d %d\r\n", SGauche, SDevant, SDroite);
 }
 
 /*void init_ax12(void)