Revision 78:c28bdbf29b6e, committed 2016-05-05

Comitter:

IceTeam

Date:

Thu May 05 13:17:00 2016 +0000

Parent:

77:f19cc7f81f2a

Child:

79:b11b50108ae5

Commit message:

Des trucs qui compilent

Changed in this revision

--- a/Functions/DefinesSharps.h	Thu May 05 08:46:08 2016 +0000
+++ b/Functions/DefinesSharps.h	Thu May 05 13:17:00 2016 +0000
@@ -1,6 +1,6 @@
 #ifndef DEFINES_SHARPS_H
 #define DEFINES_SHARPS_H
 
-extern bool SGauche, SDevant, SDroite, SHomologation;
+//extern bool SGauche, SDevant, SDroite, SHomologation;
 
 #endif
\ No newline at end of file

--- a/Functions/defines.h	Thu May 05 08:46:08 2016 +0000
+++ b/Functions/defines.h	Thu May 05 13:17:00 2016 +0000
@@ -48,6 +48,8 @@
 extern DigitalIn EnR;
 extern DigitalIn EnZ;
 extern DigitalIn EnL;
+extern float seuils[3];
+extern int SGauche, SDevant, SDroite;
 
 
 extern int SIMON_i, SIMON_state, SCouleur, SStart, SSchema;

--- 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)

--- a/Functions/func.h	Thu May 05 08:46:08 2016 +0000
+++ b/Functions/func.h	Thu May 05 13:17:00 2016 +0000
@@ -1,24 +1,16 @@
 #include "defines.h"
 
-void homologation(void);
 void depart(void);
 void changeCamp(void);
-void ELpressed(void);
-void ELunpressed(void);
-void EZpressed(void);
-void EZunpressed(void);
-void ERpressed(void);
-void ERunpressed(void);
 
 void pressed(void);
 void unpressed(void);
-void JPO(void);
 
 void init_ax12(void);
 void init_interrupt(void);
-void goHome(void);
-void goHomeL(void);
-void goHomeZ(void);
-void goHomeR(void);
 void checkAround(void);
 void update_main(void);
+
+int max(int a, int b);
+
+int max(int a, int b, int c);
\ No newline at end of file

--- a/Odometry/Odometry.cpp	Thu May 05 08:46:08 2016 +0000
+++ b/Odometry/Odometry.cpp	Thu May 05 13:17:00 2016 +0000
@@ -17,6 +17,12 @@
     m_pulses_right = 0;
     m_pulses_left = 0;
     pos_prog = 0;
+    
+    paused = false;
+    
+    timer.stop();
+    timer.reset();
+    
     wait_ms(100);
 }
 
@@ -88,20 +94,29 @@
 
 void Odometry::GotoXY(double x_goal, double y_goal)
 {
+    saved_x_goal = x_goal;
+    saved_y_goal = y_goal;
+    
+    currentStep = STEP_D;
+    currentMainFunction = MAIN_FCT_XY;
+    
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
-    logger.printf("J'ai fais theta je suis pas trop un fdp\n\r");
     GotoDist(dist_);
-    logger.printf("J'ai fais Dist je suis pas trop un fdp\n\r");
 }
 
 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
 {
+    saved_x_goal = x_goal;
+    saved_y_goal = y_goal;
+    saved_theta_goal = theta_goal;
+    
+    currentStep = STEP_D;
+    currentMainFunction = MAIN_FCT_XYT;
+    
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
     GotoThet(theta_goal);
@@ -109,6 +124,8 @@
 
 void Odometry::GotoThet(double theta_)
 {
+    saved_theta_goal = theta_;
+    currentStep = STEP_T;
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -123,8 +140,6 @@
     while(erreur_theta >= PI) erreur_theta -= 2*PI;
     while(erreur_theta < -PI) erreur_theta += 2*PI;
     
-    logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
-    
     if(erreur_theta < 0) {
         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
@@ -141,23 +156,33 @@
 
     //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(abs(m_pulses_right - distance_ticks_right) >= 1 && abs(m_pulses_left - distance_ticks_left) >= 1) {   
-        if (SDevant && SHomologation) {
-            roboclaw.SpeedAccelM1M2(accel_dista, 0, 0);
-            while(1);
-        }
+    //while(abs(m_pulses_right - distance_ticks_right) >= 1 && abs(m_pulses_left - distance_ticks_left) >= 1) wait(0.001);
+    
+    while(abs(erreur_theta) > 2.f*PI/180.f)
+    {
+        wait(0.001);
+        
+        erreur_theta = theta_ - getTheta();
+        while(erreur_theta >= PI) erreur_theta -= 2*PI;
+        while(erreur_theta < -PI) erreur_theta += 2*PI;
     }
+    
     //logger.printf ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left); 
     //logger.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
     
     wait(0.4);
-    theta = theta_;
+    
+    /*roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);*/
+    
+    //theta = theta_;
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
 }
 
 void Odometry::GotoDist(double distance)
 {
+    currentStep = STEP_D;
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
@@ -166,22 +191,28 @@
 
     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;
-    logger.printf("Je suis un fdp a l'envoi d'une commande 1 !\n\r");
     if (distance >= 0)
         roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
     else
         roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, -vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, -vitesse_dista, deccel_dista, distance_ticks_left, 1);   
-    logger.printf("J'suis un fdp 2\n\r");
     //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(abs(m_pulses_right - distance_ticks_right) >= 1 && abs(m_pulses_left - distance_ticks_left) >= 1) //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);
+    while(abs(m_pulses_right - distance_ticks_right) >= 30 || abs(m_pulses_left - distance_ticks_left) >= 30) //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);
     {  
-        if (SDevant && SHomologation) {
-            roboclaw.SpeedAccelM1M2(accel_dista, 0, 0);
-            while(1) ;
-        }
+        wait(0.1);
     }
+    
+    /*while( carre(x-saved_x_goal) + carre(y-saved_y_goal) > 4*4)
+    {
+        wait(0.1);
+        logger.printf("Dist error : %f  (%f,%f -> %f,%f)\r\n", sqrt(carre(x-saved_x_goal) + carre(y-saved_y_goal)),x,y,saved_x_goal,saved_y_goal);
+    }*/
+    
     wait(0.4);
+    
+    /*roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);*/
+    
     //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);
 }
@@ -203,8 +234,7 @@
     
     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);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
         
     wait(0.4);
 }
@@ -221,4 +251,37 @@
     
     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);
+}
+
+void Odometry::stop() { 
+    paused = false; 
+    timer.stop();
+    timer.reset();
+    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+}
+
+void Odometry::pause() { 
+    timer.start();
+    timer.reset();
+    paused = true;
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+}
+
+void Odometry::resume() {    
+    if(paused && timer.read() > 0.5f)
+    {
+        paused = false;
+        timer.stop();
+        timer.reset();
+        
+        if(currentMainFunction == MAIN_FCT_XY)
+            GotoXY(saved_x_goal,saved_y_goal);
+        else
+            GotoXYT(saved_x_goal,saved_y_goal,saved_theta_goal);
+            
+        
+    }
 }
\ No newline at end of file

--- a/Odometry/Odometry.h	Thu May 05 08:46:08 2016 +0000
+++ b/Odometry/Odometry.h	Thu May 05 13:17:00 2016 +0000
@@ -20,6 +20,12 @@
 /* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
 #define ENTRAXE 243.8
 
+#define STEP_T 0
+#define STEP_D 1
+
+#define MAIN_FCT_XY 0
+#define MAIN_FCT_XYT 1
+
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre
 */
@@ -119,6 +125,11 @@
     /** Permet de mettre à jour les valeurs de l'odometrie
     */
     void update_odo(void);
+    
+    void stop();
+    void pause();
+    void resume();
+    char getCurrentStep(){return currentStep;}
 
 private:
     RoboClaw &roboclaw;
@@ -128,11 +139,18 @@
     double x, y, theta;
     double m_vitLeft, m_vitRight;
     double m_distLeft, m_distRight;
-
+    
+    double saved_x_goal, saved_y_goal, saved_theta_goal;
+    char currentStep;
+    char currentMainFunction;
+    bool paused;
+    
     double m_distPerTick_left, m_distPerTick_right, m_v;
 
     double erreur_ang;
     bool arrived;
+    
+    Timer timer;
 };
 
 #endif

--- a/main.cpp	Thu May 05 08:46:08 2016 +0000
+++ b/main.cpp	Thu May 05 13:17:00 2016 +0000
@@ -50,8 +50,12 @@
 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
 int SIMON_i = 0, SIMON_state = 0, SCouleur = VERT, SStart = 0, SSchema = 1;
-bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false, SGauche = false, SDevant = false, SDroite = false;
-bool Sharps_actives = true; bool SHomologation = false;
+bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false;
+int SGauche = false, SDevant = false, SDroite = false;
+bool Sharps_actives = false;
+bool SHomologation = false;
+
+float seuils[3] = {0.25, 0.40, 0.25};
 
 void init(void);
 void update_main(void);
@@ -69,18 +73,27 @@
     wait_ms(100);
     while(START==0);
     odo.GotoXY(1000, 1000);*/
+    
+    roboclaw.ResetEnc();
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    logger.printf("Depart");
     odo.setPos(110,850,0);
+    
     init_interrupt();
+    
     while(START==0);
+    
+    logger.printf("Depart");
     odo.GotoXY(450,850);
+    logger.printf("Premier point atteint");
     odo.GotoXY(1050,1100);
-    odo.GotoXY(700,1100);
-    SHomologation = true;
-    odo.GotoXY(250,850);
-    odo.GotoXY(250,400);
-    odo.GotoXY(800,400);
-    odo.GotoXY(800,800);
-    
+    Sharps_actives = true;
+    logger.printf("Second point atteint");
+    odo.GotoXY(400,850);
+    logger.printf("Troisieme point atteint");
+    odo.GotoXY(400,1000);
+
     LEDV = 1;
     while(1);
     //homologation();
@@ -111,7 +124,6 @@
     logger.printf("Hello from main !\n\r");
 
     init_interrupt();
-    goHome();
 
     SCouleur = VERT;
     LEDV = 1;
@@ -123,15 +135,25 @@
     roboclaw.ForwardM2(0);
     wait_ms(500);
     while(1);
-    //depart();
+    /*depart();
     init_interrupt();
     wait_ms(100);
     while(START==0);
-    logger.printf("End init\n\r");
+    logger.printf("End init\n\r");*/
 }
 
 void update_main(void)
 {
     odo.update_odo();
+    
     checkAround();
+    
+    if (Sharps_actives && max(SGauche,SDevant,SDroite)>0 && odo.getCurrentStep() != STEP_T)
+    {
+        odo.pause();
+    }
+    /*else
+    {
+        odo.resume();
+    }*/
 }
\ No newline at end of file