Revision 46:5658af4e5149, committed 2016-04-25

Comitter:

sype

Date:

Mon Apr 25 12:38:58 2016 +0000

Parent:

45:b53ae54062c6

Child:

48:03da1aead032

Commit message:

ajout de la carte boutons ;

Changed in this revision

--- a/AX12/AX12.h	Wed Apr 13 12:47:47 2016 +0000
+++ b/AX12/AX12.h	Mon Apr 25 12:38:58 2016 +0000
@@ -48,7 +48,7 @@
 #define AX12_CW 1
 #define AX12_CCW 0
 
-#/** Servo control class, based on a PwmOut
+/** Servo control class, based on a PwmOut
  *
  * Example:
  * @code

--- a/Functions/func.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Functions/func.cpp	Mon Apr 25 12:38:58 2016 +0000
@@ -2,106 +2,106 @@
 
 void pressed(void)
 {
-    if(i==0) {
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-        i++;
+    if(SIMON_i==0) {
+        roboclaw.ForwardM1(0);
+        roboclaw.ForwardM2(0);
+        SIMON_i++;
     }
 }
 
 void unpressed(void)
 {
-    if(i==1) {
-        i--;
+    if(SIMON_i==1) {
+        SIMON_i--;
     }
 }
 
 void ELpressed(void)
 {
-    //bleu = 1;
-    EL = true;
+    bleu = 1;
+    SIMON_EL = true;
 }
 void ELunpressed(void)
 {
-    //bleu = 0;
-    EL = false;
+    bleu = 0;
+    SIMON_EL = false;
 }
 
 void EZpressed(void)
 {
-    //blanc = 1;
-    EZ = true;
+    blanc = 1;
+    SIMON_EZ = true;
 }
 void EZunpressed(void)
 {
-    //blanc = 0;
-    EZ = false;
+    blanc = 0;
+    SIMON_EZ = false;
 }
 
 void ERpressed(void)
 {
-    //rouge = 1;
-    ER = true;
+    rouge = 1;
+    SIMON_ER = true;
 }
 void ERunpressed(void)
 {
-    //rouge = 0;
-    ER = 0;
+    rouge = 0;
+    SIMON_ER = 0;
 }
 
 void JPO(void)
 {
     char c = logger.getc();
     if(c=='z') {
-        if (state != 1) {
-            state = 1;
+        if (SIMON_state != 1) {
+            SIMON_state = 1;
             logger.printf("Avant (Z) \r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
         }
     } else if(c == 's') {
-        if (state != 2) {
-            state = 2;
+        if (SIMON_state != 2) {
+            SIMON_state = 2;
             logger.printf("Stop (S) \r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
+            roboclaw.SpeedAccelM1(accel_angle, 0);
+            roboclaw.SpeedAccelM2(accel_angle, 0);
         }
     } else if(c == 'd') {
-        if (state != 3) {
-            state = 3;
+        if (SIMON_state != 3) {
+            SIMON_state = 3;
             logger.printf("Droite (D) \r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
         }
     } else if(c == 'q') {
-        if (state != 4) {
-            state = 4;
+        if (SIMON_state != 4) {
+            SIMON_state = 4;
             logger.printf("Gauche (Q)\r\n");
-            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-            roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(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);
+        if (SIMON_state != 5) {
+            SIMON_state = 5;
+            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
         }
     } else {
-        if (state != 0) {
-            roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
-            state = 0;
+        if (SIMON_state != 0) {
+            roboclaw.SpeedAccelM1M2(accel_angle, 0, accel_angle, 0);
+            SIMON_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);
+    //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(EL==false) LMot.step(1, 0, DELAY);
-    LMot.step(10, 1, DELAY);
+    while(SIMON_EL==false) LMot.step(1, 0, DELAY);
+    LMot.step(10, 1, DELAY+0.01);
     LMot.step(5, 0, DELAY);
 }
 
@@ -135,20 +135,36 @@
 
 void init_interrupt(void)
 {
+    CAMP.fall(&changeCamp);
+    CAMP.rise(&changeCampoff);
+    START.rise(&letsgo);
+    START.fall(&letsgooff);
     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)
+{
+    LEDV = 1;
 }
 
-void update_main(void)
+void changeCampoff(void)
+{
+    LEDV = 0;
+}
+
+void letsgo(void)
 {
-    odo.update_odo();
-    checkAround();
+    LEDR = 0;
+}
+
+void letsgooff(void)
+{
+    LEDR = 1;
 }
\ No newline at end of file

--- a/Functions/func.h	Wed Apr 13 12:47:47 2016 +0000
+++ b/Functions/func.h	Mon Apr 25 12:38:58 2016 +0000
@@ -9,7 +9,6 @@
 #include "AX12.h"
 
 #define SEUIL 0.25
-#define dt 10000
 
 extern Serial logger;
 extern RoboClaw roboclaw;
@@ -28,12 +27,19 @@
 extern InterruptIn EndL;
 extern AX12 left_hand;
 extern AX12 right_hand;
-extern Ticker ticker;
 extern Odometry odo;
+extern InterruptIn CAMP;
+extern InterruptIn START;
+extern DigitalOut LEDR;
+extern DigitalOut LEDV;
 
-extern int i, state;
-extern bool EL, EZ, ER;
+extern int SIMON_i, SIMON_state;
+extern bool SIMON_EL, SIMON_EZ, SIMON_ER;
 
+void changeCamp(void);
+void changeCampoff(void);
+void letsgo(void);
+void letsgooff(void);
 void ELpressed(void);
 void ELunpressed(void);
 void EZpressed(void);
@@ -49,6 +55,5 @@
 void init_interrupt(void);
 void goHome(void);
 void checkAround(void);
-void update_main(void);
 
 #endif
\ No newline at end of file

--- a/Odometry/Odometry.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Odometry/Odometry.cpp	Mon Apr 25 12:38:58 2016 +0000
@@ -8,9 +8,9 @@
     m_distPerTick_left = diameter_left*PI/quadrature;
     m_distPerTick_right = diameter_right*PI/quadrature;
 
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    roboclaw.ResetEnc(ADR);
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    roboclaw.ResetEnc();
     // Erreur autorisée sur le déplacement en angle
     erreur_ang = 0.01;
 
@@ -28,7 +28,7 @@
 }
 void Odometry::getEnc()
 {
-    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
+    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(), roboclaw.ReadEncM2());
 }
 
 void Odometry::setX(double x)
@@ -48,8 +48,8 @@
 
 void Odometry::update_odo(void)
 {
-    int32_t roboclawENCM1 = roboclaw.ReadEncM1(ADR); 
-    int32_t roboclawENCM2 = roboclaw.ReadEncM2(ADR); 
+    int32_t roboclawENCM1 = roboclaw.ReadEncM1(); 
+    int32_t roboclawENCM2 = roboclaw.ReadEncM2();
     int32_t delta_right = roboclawENCM1 - m_pulses_right;
     m_pulses_right = roboclawENCM1;
     int32_t delta_left = roboclawENCM2 - m_pulses_left;
@@ -84,7 +84,6 @@
     theta += deltaTheta;
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
-    
 }
 
 void Odometry::GotoXY(double x_goal, double y_goal)
@@ -108,7 +107,6 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    led = 0;
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -138,21 +136,19 @@
     //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
     //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
 
     //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("%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_);
-    led = 1;
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
 }
 
 void Odometry::GotoDist(double distance)
 {
-    led = 0;
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
@@ -162,13 +158,12 @@
     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(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
     //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);
     wait(0.4);
-    led = 1;
     //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);
 }
@@ -188,7 +183,7 @@
         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     }
     
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    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)); 
     wait(0.4);
@@ -200,11 +195,10 @@
     int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
     int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
     //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);
     wait(0.4);
-    led = 1;
 }
\ No newline at end of file

--- a/Odometry/Odometry.h	Wed Apr 13 12:47:47 2016 +0000
+++ b/Odometry/Odometry.h	Mon Apr 25 12:38:58 2016 +0000
@@ -25,7 +25,6 @@
 */
 
 extern Serial logger;
-extern DigitalOut led;
 
 /** Permet la gestion de l'odometrie du robot **/
 class Odometry

--- a/RoboClaw.lib	Wed Apr 13 12:47:47 2016 +0000
+++ b/RoboClaw.lib	Mon Apr 25 12:38:58 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#ece8a8cdf4b0
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#af0e3da221a9

--- a/main.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/main.cpp	Mon Apr 25 12:38:58 2016 +0000
@@ -3,18 +3,32 @@
 #define ATTENTE 0
 #define GO 1
 #define STOP 2
+#define ADR 0x80
+#define dt 10000
 
 /* Déclaration des différents éléments de l'IHM */
+
+/* Carte boutons, la NUCLEO n'arrive pas à récupérer les signaux CAMP et START */
+InterruptIn CAMP(PH_0);
+InterruptIn START(PH_1);
+DigitalOut LEDR(PC_2);
+DigitalOut LEDV(PC_3);
+
 InterruptIn mybutton(USER_BUTTON);
 DigitalIn button(USER_BUTTON);
-DigitalOut led(LED1);
+
+DigitalIn ChannelA1(PA_1);
+DigitalIn ChannelB1(PA_0);
+DigitalIn ChannelA2(PA_5);
+DigitalIn ChannelB2(PA_6);
+
 DigitalOut bleu(PC_5);
 DigitalOut blanc(PC_6);
 DigitalOut rouge(PC_8);
 
-/* AX12 */
-AX12 left_hand(PA_15, PB_7, 4, 250000);
-AX12 right_hand(PA_15, PB_7, 1, 250000);
+/* AX12 - non fonctionnels, je pense que le souci vient de la bibliothèque car la trame n'est pas correcte (analyseur logique) */
+AX12 left_hand(PA_9, PA_10, 4, 250000);
+AX12 right_hand(PA_9, PA_10, 1, 250000);
 
 /* Sharp */
 AnalogIn capt1(PA_4);
@@ -23,9 +37,9 @@
 AnalogIn capt4(PC_0);
 
 /* Moteurs pas à pas */
-Stepper RMot(NC, PB_10, PA_8);
-Stepper ZMot(NC, PB_5, PB_4);
-Stepper LMot(NC, PB_3, PA_10);
+Stepper RMot(NC, PA_8, PC_7);
+Stepper ZMot(NC, PB_4, PB_10);
+Stepper LMot(NC, PB_5, PB_3);
 /* Fins de course */
 InterruptIn EndR(PB_15);
 InterruptIn EndZ(PB_14);
@@ -34,14 +48,15 @@
 Ticker ticker;
 
 Serial logger(USBTX, USBRX);
-//Serial logger(PA_9, PA_10);
-RoboClaw roboclaw(460800, PA_11, PA_12);
+//Serial logger(PA_2, PA_3);
+RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
-int i = 0, state = 0;
-bool EL = false, EZ = false, ER = false;
+int SIMON_i = 0, SIMON_state = 0;
+bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false;
 
 void init(void);
+void update_main(void);
 
 /* Debut du programme */
 int main(void)
@@ -52,20 +67,22 @@
 
 void init(void)
 {
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
-    wait_ms(500);
-    bleu = 1, blanc = 1, rouge = 1;
-    wait_ms(1000);
-    bleu = 0, blanc = 0, rouge = 0;
-    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    LEDV = 1;
     while(button);
-    wait(1);
-    
-    init_ax12();
+    LEDV = 0;
+    roboclaw.ResetEnc();
     init_interrupt();
+    ticker.attach_us(&update_main, dt); // 100 Hz    
     wait_ms(100);
     logger.printf("End init\n\r");
 }
+
+void update_main(void)
+{
+    odo.update_odo();
+    //checkAround();
+}
\ No newline at end of file

--- a/main.cpp.orig	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,102 +0,0 @@
-#include "Odometry.h"
-/** Dependencies :
-    Map/Point.h
-    Map/define.h
-    Map/Obstacles/Obstacle.h
-
-    Test en cours :
-    * Un obstacle seulement
-    * Radius du robot réduit à 1 (defines.cpp)
-    * Nombreux
-*/ 
-#include "Map/Map.h"
-
-#define dt 10000
-#define ATTENTE 0
-#define GO 1
-#define STOP 2
-
-InterruptIn mybutton(USER_BUTTON);
-DigitalIn button(USER_BUTTON);
-DigitalOut led(LED1);
-Ticker ticker;
-//Serial pc(USBTX, USBRX);
-Serial pc(PA_9, PA_10);
-RoboClaw roboclaw(460800, PA_11, PA_12);
-Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
-
-int i = 0;
-
-void update_main(void);
-void init(void);
-void pressed(void);
-void unpressed(void);
-
-/** Debut du programme */
-int main(void)
-{
-    double angle_v = 2*PI/5;
-    double distance_v = 200.0;
-    std::vector<SimplePoint> path;
-    Map map;
-    
-    init();
-    //Construction des obstacles
-    map.build();
-    map.Astar(0, 1000, 2000, 1000);
-    path = map.path;
-    
-    for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, 0);
-    }
-        
-    //odo.GotoXYT(500, 50, 0);
-    //odo.GotoXYT(200, 0, 0);
-    //odo.GotoXYT(0, 0, 0);
-
-    //odo.GotoThet(-PI/2);
-    wait(2000);
-    //odo.GotoXYT(2250,500,0);
-    while(1);
-}
-
-void init(void)
-{
-    pc.baud(9600);
-    pc.printf("Hello from main !\n\r");
-    wait_ms(500);
-    
-    odo.setPos(0, 0, 0);
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    
-    while(button);
-    wait(1);
-    mybutton.fall(&pressed);
-    mybutton.rise(&unpressed);
-    ticker.attach_us(&update_main,dt); // 100 Hz
-}
-
-void update_main(void)
-{
-    odo.update_odo();
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-    //if(pc.readable()) if(pc.getc()=='l') led = !led;
-}
-
-void pressed(void)
-{
-    if(i==0) {
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-        i++;
-    }
-}
-
-void unpressed(void)
-{
-    if(i==1) {
-        i--;
-    }
-}
\ No newline at end of file