Revision 51:1056dd73a748, committed 2016-05-04

Comitter:

sype

Date:

Wed May 04 16:15:13 2016 +0000

Parent:

50:2dea82393beb

Child:

53:bef06d5e827d

Child:

77:f19cc7f81f2a

Commit message:

Impl?mentation de l'homologation

Changed in this revision

--- a/AX12/AX12.cpp	Mon May 02 08:35:56 2016 +0000
+++ b/AX12/AX12.cpp	Wed May 04 16:15:13 2016 +0000
@@ -296,9 +296,10 @@
 
 float AX12::getLoad (void) {
     char data[2];
-    int ErrorCode = read(_ID, AX12_REG_VOLTS, 2, data);
-    float volts = data[0]/10.0;
-    return(volts);
+    int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data);
+    short load1 = data[0] + (data[1] << 8);
+    float load = load1/1023;
+    return(load); //renvoie la charge en % 
 }
 
 int AX12::read(int ID, int start, int bytes, char* data) {

--- a/AX12/AX12.h	Mon May 02 08:35:56 2016 +0000
+++ b/AX12/AX12.h	Wed May 04 16:15:13 2016 +0000
@@ -41,6 +41,7 @@
 #define AX12_REG_TEMP 0x2B
 #define AX12_REG_MOVING 0x2E
 #define AX12_REG_POSITION 0x24
+#define AX12_REG_LOAD 0x28
 
 #define AX12_MODE_POSITION  0
 #define AX12_MODE_ROTATION  1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions/defines.h	Wed May 04 16:15:13 2016 +0000
@@ -0,0 +1,55 @@
+#ifndef DEFINES_H
+#define DEFINES_H
+
+#include "mbed.h"
+#include "../RoboClaw/RoboClaw.h"
+#include "../Odometry/Odometry.h"
+#include "../StepperMotor/Stepper.h"
+#include "Map/map.h"
+#include "AX12.h"
+
+#define SEUIL 0.25
+#define VERT 1
+#define VIOLET 2
+#define ATTENTE 0
+#define GO 1
+#define STOP 2
+#define ADR 0x80
+#define dt 10000
+#define HAUT 1
+#define BAS 0
+
+extern Serial logger;
+extern RoboClaw roboclaw;
+extern DigitalOut bleu;
+extern DigitalOut blanc;
+extern DigitalOut rouge;
+extern Stepper RMot;
+extern Stepper ZMot;
+extern Stepper LMot;
+extern AnalogIn capt1;
+extern AnalogIn capt2;
+extern AnalogIn capt3;
+extern InterruptIn mybutton;
+extern InterruptIn EndR;
+extern InterruptIn EndZ;
+extern InterruptIn EndL;
+//extern AX12 left_hand;
+//extern AX12 right_hand;
+extern Odometry odo;
+extern Ticker ticker;
+extern DigitalIn CAMP;
+extern DigitalIn START;
+extern DigitalOut LEDR;
+extern DigitalOut LEDV;
+extern DigitalIn button;
+extern BusOut drapeau;
+extern DigitalIn EnR;
+extern DigitalIn EnZ;
+extern DigitalIn EnL;
+
+
+extern int SIMON_i, SIMON_state, SCouleur, SStart, SSchema;
+extern bool SIMON_EL, SIMON_EZ, SIMON_ER, SGauche, SDevant, SDroite;
+
+#endif
\ No newline at end of file

--- 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);
+}

--- a/Functions/func.h	Mon May 02 08:35:56 2016 +0000
+++ b/Functions/func.h	Wed May 04 16:15:13 2016 +0000
@@ -1,52 +1,8 @@
-#ifndef FUNC_H
-#define FUNC_H
-
-#include "mbed.h"
-#include "../RoboClaw/RoboClaw.h"
-#include "../Odometry/Odometry.h"
-#include "../StepperMotor/Stepper.h"
-#include "Map/map.h"
-#include "AX12.h"
-
-#define SEUIL 0.25
-#define VERT 1
-#define VIOLET 2
-#define ATTENTE 0
-#define GO 1
-#define STOP 2
-#define ADR 0x80
-#define dt 10000
+#include "defines.h"
 
-extern Serial logger;
-extern RoboClaw roboclaw;
-extern DigitalOut bleu;
-extern DigitalOut blanc;
-extern DigitalOut rouge;
-extern Stepper RMot;
-extern Stepper ZMot;
-extern Stepper LMot;
-extern AnalogIn capt1;
-extern AnalogIn capt2;
-extern AnalogIn capt3;
-extern InterruptIn mybutton;
-extern InterruptIn EndR;
-extern InterruptIn EndZ;
-extern InterruptIn EndL;
-//extern AX12 left_hand;
-//extern AX12 right_hand;
-extern Odometry odo;
-extern Ticker ticker;
-extern InterruptIn CAMP;
-extern InterruptIn START;
-extern DigitalOut LEDR;
-extern DigitalOut LEDV;
-
-
-extern int SIMON_i, SIMON_state, SCouleur, SStart;
-extern bool SIMON_EL, SIMON_EZ, SIMON_ER;
-
+void homologation(void);
+void depart(void);
 void changeCamp(void);
-void go(void);
 void ELpressed(void);
 void ELunpressed(void);
 void EZpressed(void);
@@ -61,7 +17,8 @@
 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);
-
-#endif
\ No newline at end of file

--- a/Odometry/Odometry.h	Mon May 02 08:35:56 2016 +0000
+++ b/Odometry/Odometry.h	Wed May 04 16:15:13 2016 +0000
@@ -8,18 +8,17 @@
 #define C 1.0
 
 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
-#define accel_angle 0x1000
-#define vitesse_angle 0x1000
-#define deccel_angle 0x1000
+#define accel_angle 4096
+#define vitesse_angle 4096
+#define deccel_angle 4096
 
-#define accel_dista 0x1000
-#define vitesse_dista 0x1000
-#define deccel_dista 0x1000
+#define accel_dista 4096
+#define vitesse_dista 4096
+#define deccel_dista 4096
 
 /* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
 #define ENTRAXE 243.8
 
-
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre
 */

--- a/RoboClaw.lib	Mon May 02 08:35:56 2016 +0000
+++ b/RoboClaw.lib	Wed May 04 16:15:13 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#0fc5514faed9
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#f82bc24e3bd4

--- a/StepperMotor.lib	Mon May 02 08:35:56 2016 +0000
+++ b/StepperMotor.lib	Wed May 04 16:15:13 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/sype/code/StepperMotor/#1cec7e9117f2
+https://developer.mbed.org/users/sype/code/StepperMotor/#c7011e72f0c7

--- a/main.cpp	Mon May 02 08:35:56 2016 +0000
+++ b/main.cpp	Wed May 04 16:15:13 2016 +0000
@@ -3,11 +3,13 @@
 
 /* Déclaration des différents éléments de l'IHM */
 
-InterruptIn CAMP(PA_15);
-InterruptIn START(PB_7);
+DigitalIn CAMP(PA_15);
+DigitalIn START(PB_7);
 DigitalOut LEDR(PC_2);
 DigitalOut LEDV(PC_3);
 
+BusOut drapeau(PC_8, PC_6, PC_5);
+
 InterruptIn mybutton(USER_BUTTON);
 DigitalIn button(USER_BUTTON);
 
@@ -20,7 +22,6 @@
 DigitalOut blanc(PC_6);
 DigitalOut rouge(PC_8);
 
-/* 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);
 
@@ -31,22 +32,25 @@
 AnalogIn capt4(PC_0);
 
 /* Moteurs pas à pas */
-Stepper RMot(NC, PA_8, PC_7);
-Stepper ZMot(NC, PB_4, PB_10);
-Stepper LMot(NC, PB_5, PB_3);
+Stepper RMot(NC, PA_8, PC_7, 4);
+Stepper ZMot(NC, PB_4, PB_10, 5);
+Stepper LMot(NC, PB_5, PB_3, 4);
 /* Fins de course */
 InterruptIn EndR(PB_15);
 InterruptIn EndZ(PB_14);
 InterruptIn EndL(PB_13);
+DigitalIn EnR(PB_15);
+DigitalIn EnZ(PB_14);
+DigitalIn EnL(PB_13);
 
 Ticker ticker;
 //Serial logger(USBTX, USBRX);
-Serial logger(PA_9, PA_10);
+Serial logger(PA_2, PA_3);
 RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
-int SIMON_i = 0, SIMON_state = 0, SCouleur = 1, SStart = 0;
-bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false;
+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;
 
 void init(void);
 void update_main(void);
@@ -54,52 +58,56 @@
 /* Debut du programme */
 int main(void)
 {
-    init();    
-    
-    /*map m(&odo);
+    logger.printf("Depart homologation !\n\r");
+    homologation();
+    /*drapeau = 0;
+    init();
+
+    map m(&odo);
     m.addObs(obsCarr (1250, 1000, 220, 220));
     m.addObs(obsCarr (1500, 750, 220, 220));
     m.addObs(obsCarr (1500, 1250, 220, 220));
-    
+
     m.Execute(1000,1000);
     m.Execute(1500,1000);
     m.Execute(1500,1500);
     m.Execute(110,1000);
-    
+
     odo.GotoThet(0);
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
-    
-    logger.printf ("Chemin Fini !");*/
-    while(1);
+    logger.printf ("Chemin Fini !");
+
+    while(1);*/
 }
 
 void init(void)
 {
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
-    
-    bleu = 1, blanc = 1, rouge = 1;
-    
+
+    init_interrupt();
+    goHome();
+
+    SCouleur = VERT;
+    LEDV = 1;
+    LEDR = 0;
+
     odo.setPos(110, 1000, 0);
-    
+    roboclaw.ResetEnc();
     roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);    
+    roboclaw.ForwardM2(0);
     wait_ms(500);
-    
-    bleu = 0, blanc = 0, rouge = 0;
-    
-    while(SStart==0);
-    
-    roboclaw.ResetEnc();
+    while(1);
+    //depart();
     init_interrupt();
     wait_ms(100);
-    
+    while(START==0);
     logger.printf("End init\n\r");
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    //checkAround();
+    checkAround();
 }
\ No newline at end of file