Diff: main.cpp

Revision:

99:1fcb088f8f36

Parent:

98:2426d699362b

Child:

100:7c7174048302

--- a/main.cpp	Fri May 06 23:55:01 2016 +0000
+++ b/main.cpp	Sat May 07 02:15:23 2016 +0000
@@ -49,43 +49,111 @@
     init_globals();
     
     if (SCouleur == VERT) {
+        // Chateau
         end.attach(&endFonc, 90);
-        GotoDist(9.0);
-        GotoArr(9.2);
+        GotoDist(9.0,2000,2000,2000);
+        
+        // Callage
+        GotoArr(5);
         R_SEUIL_SHARP = 0.25;
-        GotoDist(2.5);
+        
+        // Cabane 1
+        GotoDist(1.25);
         GotoThet(-PI/2.f);
         R_SEUIL_SHARP = 0.35;
-        GotoDist(5.5);
+        GotoDist(2.75);
         R_SEUIL_SHARP = 1;
-        GotoDist(4.5);
-        GotoArr(3);
+        controlleurPince.setPos(-1,150);
+        GotoDist(2.25);
+        
+        // Cabane 2
+        GotoArr(2);
         R_SEUIL_SHARP = 0.35;
         GotoThet(PI/2.f);
+        GotoDist(1.75);
+        GotoThet(PI/2.f);
+        GotoArr(3);
+        
+        // Coquillage 1
+        R_SEUIL_SHARP = 0.35;
         GotoDist(3.5);
-        GotoThet(-PI/2.f);
-        R_SEUIL_SHARP = 1; 
-        GotoDist(4.6);
+        controlleurPince.setPos(0);
+        controlleurPince.angle(50);
+        
+        GotoThet(-0.3f);
+        
+        GotoDist(2);
+        
+        controlleurPince.close();
+        controlleurPince.setPos(-1,70);
+        
+        // Coquillage 2
+        GotoThet(PI/2.f-0.3f);
+        R_SEUIL_SHARP = 1;
+        controlleurPince.angle(50);
+        GotoDist(2);
+        controlleurPince.close();
+        
+        // Fin !!!
+        GotoThet(PI/2.f);
+        GotoDist(1.75);
+        controlleurPince.angle(50);
+        GotoArr(1);
+        
     }
     else// if (SCouleur == VIOLET)
     {  
+    
+        // Chateau
         end.attach(&endFonc, 90);
         GotoDist(9.0,2000,2000,2000);
+        
+        // Callage
         GotoArr(5);
         R_SEUIL_SHARP = 0.25;
+        
+        // Cabane 1
         GotoDist(1.25);
         GotoThet(PI/2.f);
         R_SEUIL_SHARP = 0.35;
         GotoDist(2.75);
         R_SEUIL_SHARP = 1;
+        controlleurPince.setPos(-1,150);
         GotoDist(2.25);
+        
+        // Cabane 2
         GotoArr(2);
         R_SEUIL_SHARP = 0.35;
         GotoThet(-PI/2.f);
         GotoDist(1.75);
-        GotoThet(PI/2.f);
-        R_SEUIL_SHARP = 1; 
-        GotoDist(2.3);
+        GotoThet(-PI/2.f);
+        GotoArr(3);
+        
+        // Coquillage 1
+        R_SEUIL_SHARP = 0.35;
+        GotoDist(3.5);
+        controlleurPince.setPos(0);
+        controlleurPince.angle(50);
+        
+        GotoThet(0.3f);
+        
+        GotoDist(2);
+        
+        controlleurPince.close();
+        controlleurPince.setPos(-1,70);
+        
+        // Coquillage 2
+        GotoThet(-PI/2.f+0.3f);
+        R_SEUIL_SHARP = 1;
+        controlleurPince.angle(50);
+        GotoDist(2);
+        controlleurPince.close();
+        
+        // Fin !!!
+        GotoThet(-PI/2.f);
+        GotoDist(1.75);
+        controlleurPince.angle(50);
+        GotoArr(1);
     }
    /* else {
         GotoDistPos(200);
@@ -138,9 +206,9 @@
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
     wait(1);
-    Parasol.setMaxTorque(1000);
+    Parasol.setGoal(300);
     wait(1);
-    Parasol.setGoal(300);
+    Parasol.setMaxTorque(1000);
     while(1);
 }
 
@@ -148,16 +216,22 @@
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
 
+
+    controlleurPince.init();
     Parasol.setMode(0);
     Parasol.setMaxTorque(200);
     Parasol.setGoal(150);
-    wait(1);
+    wait(0.5);
     Parasol.setGoal(160);
-    wait(1);
+    wait(0.5);
     Parasol.setGoal(150);
     wait(1);
     Parasol.setMaxTorque(0);
     
+    controlleurPince.home();
+    controlleurPince.setPos(130,0,0);
+    controlleurPince.open();
+    
     while(START == 1)
     {
         LEDR = 1;