Diff: main.cpp

Revision:

71:5590dbe8393a

Parent:

58:02dc8328975a

Child:

74:07cdad6e861a

--- a/main.cpp	Wed May 04 21:51:00 2016 +0000
+++ b/main.cpp	Thu May 05 03:47:05 2016 +0000
@@ -24,8 +24,8 @@
 DigitalOut blanc(PC_6);
 DigitalOut rouge(PC_8);
 
-//AX12 left_hand(PA_9, PA_10, 4, 250000);
-//AX12 right_hand(PA_9, PA_10, 1, 250000);
+AX12 left_hand(PA_9, PA_10, 3, 250000);
+AX12 right_hand(PA_9, PA_10, 2, 250000);
 
 /* Sharp */
 AnalogIn capt1(PA_4);
@@ -45,7 +45,7 @@
 DigitalIn EnZ(PB_14);
 DigitalIn EnL(PB_13);
 
-ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL);
+ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL, left_hand, right_hand);
 
 Ticker ticker;
 //Serial logger(USBTX, USBRX);
@@ -62,19 +62,45 @@
 /* Debut du programme */
 int main(void)
 {
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    
     //logger.printf("Depart homologation !\n\r");
     //homologation();
-    logger.printf("Homming ...\n\r");
+    
+    controlleurPince.init();
     controlleurPince.home();
     wait(1);
-    controlleurPince.setPos(10.f,0.f,0.f);
+    controlleurPince.open();
+    logger.printf("z 10 \r\n");
+    controlleurPince.setPos(10,-1);
+    wait(1);
+    logger.printf("z 20 \r\n");
+    controlleurPince.setPos(20,-1);
+    wait(1);
+    controlleurPince.close();
+    logger.printf("d 30 \r\n");
+    controlleurPince.setPos(-1,30);
     wait(1);
+    logger.printf("c 50 \r\n");
+    controlleurPince.setPos(-1,-1,50);
+    wait(1);
+    logger.printf("d 0 \r\n");
+    controlleurPince.setPos(-1,0);
+    
+    while(1);
+    
+    
+    /*wait(1);
+    logger.printf("set pos 20 ...\n\r");
     controlleurPince.setPos(20.f,0.f,0.f);
     wait(1);
+    logger.printf("set pos 70 ...\n\r");
     controlleurPince.setPos(70.f,0.f,0.f);
     wait(1);
+    logger.printf("set pos 20 ...\n\r");
     controlleurPince.setPos(20.f,0.f,0.f);
-    logger.printf("Done ...\n\r");
+    logger.printf("Done ...\n\r");*/
     
     /*drapeau = 0;
     init();