Revision 93:c0b040954eac, committed 2016-05-06

Comitter:

IceTeam

Date:

Fri May 06 17:33:43 2016 +0000

Parent:

92:f09f55aa992b

Child:

94:86b9bd6d5c28

Commit message:

Petits changements pas test?s. Int?gration totale de la pince.;

Changed in this revision

--- a/ControlleurPince/ControlleurPince.cpp	Fri May 06 15:25:21 2016 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Fri May 06 17:33:43 2016 +0000
@@ -1,5 +1,4 @@
 
-#include "defines.h"
 #include "ControlleurPince.h"
 
 ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12) :
@@ -58,9 +57,7 @@
         if(delta < 0)
             delta = 130 - pos_r - pos_l;
         if(center < -500)
-            center = - pos_r/2 + pos_l/2;
-            
-        
+            center = - pos_r/2 + pos_l/2;        
         
         /*
         => pos_l = center*2 + pos_r

--- a/ControlleurPince/ControlleurPince.h	Fri May 06 15:25:21 2016 +0000
+++ b/ControlleurPince/ControlleurPince.h	Fri May 06 17:33:43 2016 +0000
@@ -3,15 +3,15 @@
 
 //#include "defines.h"
 #include "Stepper.h"
+#include "../AX12/AX12.h"
 #include "mbed.h"
+#include "defines_Pince.h"
 
 class ControlleurPince
 {
-    
     public:
+        ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12);
     
-        ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12);
-        
         void init();
         void home(bool r=true, bool z=true, bool l=true);
         void setPos(float z, float delta, float center = -600);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/defines_Pince.h	Fri May 06 17:33:43 2016 +0000
@@ -0,0 +1,3 @@
+#define HAUT 1
+#define BAS 0
+#define DELAY 0.007
\ No newline at end of file

--- a/StepperMotor/Stepper.cpp	Fri May 06 15:25:21 2016 +0000
+++ b/StepperMotor/Stepper.cpp	Fri May 06 17:33:43 2016 +0000
@@ -1,7 +1,5 @@
-#include "defines.h"
 #include "Stepper.h"
-#include "mbed.h"
-
+// ID AX12 : 3 et 1 
 Stepper::Stepper(PinName _en, PinName _stepPin, PinName _dir, PinName _minEndStop, float step_per_mm):en(_en),
     stepPin(_stepPin),
     direction(_dir),

--- a/StepperMotor/Stepper.h	Fri May 06 15:25:21 2016 +0000
+++ b/StepperMotor/Stepper.h	Fri May 06 17:33:43 2016 +0000
@@ -2,6 +2,7 @@
 #define MBED_STEPPER_H
 
 #include "mbed.h"
+#include "defines_Stepper.h"
 
 class Stepper
 {

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor/defines_Stepper.h	Fri May 06 17:33:43 2016 +0000
@@ -0,0 +1,3 @@
+#define HAUT 1
+#define BAS 0
+#define DELAY 0.007
\ No newline at end of file

--- a/deplacement.cpp	Fri May 06 15:25:21 2016 +0000
+++ b/deplacement.cpp	Fri May 06 17:33:43 2016 +0000
@@ -13,8 +13,10 @@
         if (Ravance != 1) {
             roboclaw.ForwardM1(0);
             roboclaw.ForwardM2(0);
+            wait_ms(1);
             t.stop();
             while (Ravance != 1);
+            wait_ms(1);
             roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
             roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
             t.start();
@@ -30,55 +32,12 @@
     wait(waiting_time);
 }
 
-void GotoThet (float timer, int signe) {
-    Timer t;
-    
-    if (signe == GAUCHE) {
-        //roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
-        //roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
-        roboclaw.SpeedM1(vitesse_angle);
-        roboclaw.SpeedM2(-vitesse_angle);
-    }
-    if (signe == DROITE) {
-        roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
-        roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
-    }
-    
-    t.reset();
-    t.start();
-    
-    while (t.read() < timer) {
-        if (Ravance != 1) {
-            roboclaw.ForwardM1(0);
-            roboclaw.ForwardM2(0);
-            t.stop();
-            while (Ravance != 1);
-            t.start();
-        }
-        if (signe < 0) {
-            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
-            roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
-        }
-        else {
-            roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
-            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
-        }
-    }
-    
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    
-    t.stop();
-    t.reset();
-    
-    wait(waiting_time);
-}
-
 void GotoArr(float timer) {
     Timer t;
     
     roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
     roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
+    wait_ms(1);
     
     t.reset();
     t.start();
@@ -86,16 +45,19 @@
     while (t.read() < timer) {
         if (Ravance != 1) {
             roboclaw.ForwardM1(0);
+            wait_ms(1);
             roboclaw.ForwardM2(0);
             t.stop();
             while (Ravance != 1);
-            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
-            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+            wait_ms(1);
+            roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
+            roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
             t.start();
         }
     }
     
     roboclaw.ForwardM1(0);
+    wait_ms(1);
     roboclaw.ForwardM2(0);
     
     t.stop();

--- a/entete.h	Fri May 06 15:25:21 2016 +0000
+++ b/entete.h	Fri May 06 17:33:43 2016 +0000
@@ -7,6 +7,8 @@
 /* Classes du projet : 
 AX12
 Roboclaw
+StepperMotor
+AX12
 */
 
 extern BusOut drapeau;
@@ -31,19 +33,14 @@
 // Fonctions main.cpp
 void Sharps();
 void endFonc ();
-void init();
+void init_globals();
 
 //vFonctions deplacement.cpp
-void GotoThet (float timer, int signe);
 void GotoDist (float timer);
 void GotoArr(float timer);
 void GotoThet(double theta_);
 
 // Fonctions test.cpp
-void wait_start();
-void TestDist3(float start, float pas);
-void TestThet3(float start, float pas);
-void compareThet(float start);
 
 // Fonctions couleur.cpp
 void depart(void);

--- a/main.cpp	Fri May 06 15:25:21 2016 +0000
+++ b/main.cpp	Fri May 06 17:33:43 2016 +0000
@@ -1,6 +1,7 @@
 #include "entete.h"
 
 #include "AX12/AX12.h"
+#include "ControlleurPince/ControlleurPince.h"
 
 DigitalIn CAMP(PA_15);
 DigitalIn START(PB_7);
@@ -11,6 +12,11 @@
 
 BusOut drapeau (PC_8, PC_6, PC_5);
 RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
+DigitalIn start(PB_7);
+
+AX12 Parasol(PA_9, PA_10, 2, 250000);
+
+/* Sharps */
 AnalogIn Rcapt1(PA_4);
 int RvalRcapt1 = 0;
 AnalogIn Rcapt2(PB_0);
@@ -18,14 +24,29 @@
 AnalogIn Rcapt3(PC_1);
 int RvalRcapt3 = 0;
 int Ravance = 1;
-DigitalIn start(PB_7);
-AX12 umbrella(PA_9, PA_10, 2, 250000);
 float R_SEUIL_SHARP = 1;
 
+/* Pour la pince */
+AX12 left_hand(PA_9, PA_10, 3, 250000);
+AX12 right_hand(PA_9, PA_10, 1, 250000);
+Stepper RMot(NC, PA_8, PC_7, PB_15, 4);
+Stepper ZMot(NC, PB_4, PB_10, PB_14, 5);
+Stepper LMot(NC, PB_5, PB_3, PB_13, 4);
+DigitalIn EnR(PB_15);
+DigitalIn EnZ(PB_14);
+DigitalIn EnL(PB_13);
+
+ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL,left_hand,right_hand);
+
 /* Debut du programme */
 int main(void)
 {
-
+    Ticker ticker;
+    Timeout end;
+    
+    ticker.attach(&Sharps, 0.01);
+    
+    init_globals();
     
     if (SCouleur == VERT) {
         end.attach(&endFonc, 90);
@@ -44,7 +65,7 @@
         GotoDist(3.5);
         GotoThet(-3.04);
         R_SEUIL_SHARP = 1; 
-        GotoDist(4.5);
+        GotoDist(4.6);
     }
     else if (SCouleur == VIOLET) {  
         end.attach(&endFonc, 90);
@@ -63,11 +84,11 @@
         GotoDist(3.5);
         GotoThet(3.04);
         R_SEUIL_SHARP = 1; 
-        GotoDist(4.5);
+        GotoDist(4.6);
     }
     else if (SCouleur == NOIR) {
-        TestDist3(2,2);
-        TestThet3(1,1);
+        end.attach(&endFonc, 90);
+        GotoDist(9.0);
     }
 
     while(1);
@@ -104,28 +125,25 @@
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
     wait(1);
-    umbrella.setMaxTorque(400);
+    Parasol.setMaxTorque(1000);
     wait(1);
-    umbrella.setGoal(300);
+    Parasol.setGoal(300);
     while(1);
 }
 
-void init() {
+void init_globals() {
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
-    Ticker ticker;
-    Timeout end;
-    ticker.attach(&Sharps, 0.01);
 
-    umbrella.setMode(0);
-    umbrella.setMaxTorque(200);
-    umbrella.setGoal(150);
+    Parasol.setMode(0);
+    Parasol.setMaxTorque(200);
+    Parasol.setGoal(150);
     wait(1);
-    umbrella.setGoal(160);
+    Parasol.setGoal(160);
     wait(1);
-    umbrella.setGoal(150);
+    Parasol.setGoal(150);
     wait(1);
-    umbrella.setMaxTorque(0);
+    Parasol.setMaxTorque(0);
     
     while(START == 1)
     {

--- a/tests.cpp	Fri May 06 15:25:21 2016 +0000
+++ b/tests.cpp	Fri May 06 17:33:43 2016 +0000
@@ -1,27 +1,2 @@
 #include "entete.h"
 
-void wait_start() {
-    while (CAMP != 0) ;
-}
-
-void TestDist3(float start, float pas) {
-    GotoDist(start);
-    wait_start();
-    GotoDist(start + pas);
-    wait_start();
-    GotoDist(start + 2 * pas);    
-}  
-
-void TestThet3(float start, float pas) {
-    GotoThet(start, GAUCHE);
-    wait_start();
-    GotoThet(start + pas, GAUCHE);
-    wait_start();
-    GotoThet(start + 2 * pas, GAUCHE);    
-}  
-
-void compareThet(float start) {
-    GotoThet(start, DROITE);
-    wait_start();
-    GotoThet(start, GAUCHE);    
-}
\ No newline at end of file