ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Files at this revision

API Documentation at this revision

Revision 77:f19cc7f81f2a, committed 2016-05-05

Comitter:
sype
Date:
Thu May 05 08:46:08 2016 +0000
Parent:
51:1056dd73a748
Commit message:
commit homologation

Changed in this revision

Functions/DefinesSharps.h Show annotated file Show diff for this revision Revisions of this file
Functions/defines.h Show annotated file Show diff for this revision Revisions of this file
Functions/func.cpp Show annotated file Show diff for this revision Revisions of this file
Odometry/Odometry.cpp Show annotated file Show diff for this revision Revisions of this file
Odometry/Odometry.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions/DefinesSharps.h	Thu May 05 08:46:08 2016 +0000
@@ -0,0 +1,6 @@
+#ifndef DEFINES_SHARPS_H
+#define DEFINES_SHARPS_H
+
+extern bool SGauche, SDevant, SDroite, SHomologation;
+
+#endif
\ No newline at end of file

--- a/Functions/defines.h	Wed May 04 16:15:13 2016 +0000
+++ b/Functions/defines.h	Thu May 05 08:46:08 2016 +0000
@@ -1,6 +1,7 @@
 #ifndef DEFINES_H
 #define DEFINES_H
 
+#include "DefinesSharps.h"
 #include "mbed.h"
 #include "../RoboClaw/RoboClaw.h"
 #include "../Odometry/Odometry.h"
@@ -50,6 +51,6 @@
 
 
 extern int SIMON_i, SIMON_state, SCouleur, SStart, SSchema;
-extern bool SIMON_EL, SIMON_EZ, SIMON_ER, SGauche, SDevant, SDroite;
+extern bool SIMON_EL, SIMON_EZ, SIMON_ER;
 
 #endif
\ No newline at end of file

--- a/Functions/func.cpp	Wed May 04 16:15:13 2016 +0000
+++ b/Functions/func.cpp	Thu May 05 08:46:08 2016 +0000
@@ -141,7 +141,7 @@
     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());
+    //logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
 }
 
 void homologation(void)

--- a/Odometry/Odometry.cpp	Wed May 04 16:15:13 2016 +0000
+++ b/Odometry/Odometry.cpp	Thu May 05 08:46:08 2016 +0000
@@ -92,7 +92,9 @@
     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)
@@ -110,7 +112,6 @@
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
-
     int32_t distance_ticks_left;
     int32_t distance_ticks_right;
 
@@ -140,12 +141,17 @@
 
     //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((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left))
-        logger.printf ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_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);
+        }
+    }
+    //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);
-    setTheta(theta_);
+    theta = theta_;
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
 }
@@ -160,12 +166,21 @@
 
     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;
-
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
-
+    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((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);
+    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);
+    {  
+        if (SDevant && SHomologation) {
+            roboclaw.SpeedAccelM1M2(accel_dista, 0, 0);
+            while(1) ;
+        }
+    }
     wait(0.4);
     //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);

--- a/Odometry/Odometry.h	Wed May 04 16:15:13 2016 +0000
+++ b/Odometry/Odometry.h	Thu May 05 08:46:08 2016 +0000
@@ -1,6 +1,7 @@
 #ifndef ODOMETRY_H
 #define ODOMETRY_H
 
+#include "DefinesSharps.h"
 #include "mbed.h"
 #include "../RoboClaw/RoboClaw.h"
 
@@ -8,13 +9,13 @@
 #define C 1.0
 
 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
-#define accel_angle 4096
-#define vitesse_angle 4096
-#define deccel_angle 4096
+#define accel_angle 12000
+#define vitesse_angle 10000
+#define deccel_angle 12000
 
-#define accel_dista 4096
-#define vitesse_dista 4096
-#define deccel_dista 4096
+#define accel_dista 12000
+#define vitesse_dista 12000
+#define deccel_dista 12000
 
 /* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
 #define ENTRAXE 243.8

--- a/main.cpp	Wed May 04 16:15:13 2016 +0000
+++ b/main.cpp	Thu May 05 08:46:08 2016 +0000
@@ -51,6 +51,7 @@
 
 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;
 
 void init(void);
 void update_main(void);
@@ -58,8 +59,31 @@
 /* Debut du programme */
 int main(void)
 {
-    logger.printf("Depart homologation !\n\r");
-    homologation();
+    /*logger.printf("Depart homologation !\n\r");
+    LEDV = 1;
+    LEDR = 0;
+    odo.setPos(110, 1000, 0);
+    roboclaw.ResetEnc();
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    wait_ms(100);
+    while(START==0);
+    odo.GotoXY(1000, 1000);*/
+    odo.setPos(110,850,0);
+    init_interrupt();
+    while(START==0);
+    odo.GotoXY(450,850);
+    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);
+    
+    LEDV = 1;
+    while(1);
+    //homologation();
     /*drapeau = 0;
     init();