Diff: main.cpp

Revision:

93:c0b040954eac

Parent:

92:f09f55aa992b

Child:

94:86b9bd6d5c28

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