Diff: main.cpp

Revision:

2:91b6646ea994

Parent:

1:fce3323d874f

--- a/main.cpp	Fri Nov 25 17:38:14 2016 +0000
+++ b/main.cpp	Wed Feb 01 13:42:30 2017 +0000
@@ -180,10 +180,11 @@
 {
     AX12 *bas_myAX12,*milieu_myAX12, *haut_myAX12;
     unsigned char numero_ID;
+    int i=0;
     char status=-1;
-    unsigned long vitesse=1023,vitesseNew,position;
-    bas_myAX12 = new AX12(p9, p10, 2,1000000);                  // Create objects
-    milieu_myAX12 = new AX12(p9, p10, 1,1000000);                  // Create objects
+    unsigned long vitesse=1023,vitesseNew,position=1;
+    bas_myAX12 = new AX12(p9, p10, 1,1000000);                  // Create objects
+    milieu_myAX12 = new AX12(p9, p10, 2,1000000);                  // Create objects
     haut_myAX12 = new AX12(p9, p10, 3,1000000);                  // Create objects
     bas_myAX12->Set_Goal_speed(1023);//wait(0.2);
     milieu_myAX12->Set_Goal_speed(1023);//wait(0.2);
@@ -197,33 +198,52 @@
 
         switch(position) 
         {
+             case 0:
+                bas_myAX12->Set_Secure_Goal(150);
+                milieu_myAX12->Set_Secure_Goal(150);
+                haut_myAX12->Set_Secure_Goal(150);
+                break;
+                
             case 1:
                 bas_myAX12->Set_Secure_Goal(150);
-                milieu_myAX12->Set_Secure_Goal(150-60);
-                haut_myAX12->Set_Secure_Goal(150+60);
+                haut_myAX12->Set_Secure_Goal(150+55);
+                milieu_myAX12->Set_Secure_Goal(150-47);
+                //position=2;
                 break;
             
             case 2:
+                haut_myAX12->Set_Secure_Goal(150+65);
+                milieu_myAX12->Set_Secure_Goal(150-60);
                 bas_myAX12->Set_Secure_Goal(60);
+                //position=3;
                 break;
                 
             case 3:
                 milieu_myAX12->Set_Secure_Goal(150-90);
-                bas_myAX12->Set_Secure_Goal(60+45);
+                bas_myAX12->Set_Secure_Goal(150-45);
                 haut_myAX12->Set_Secure_Goal(150+45);
+                //position=4;
                 break;
                 
             case 4:
-                haut_myAX12->Set_Secure_Goal(150);
-                milieu_myAX12->Set_Secure_Goal(105);
-                bas_myAX12->Set_Secure_Goal(110);
+                bas_myAX12->Set_Secure_Goal(150-60);
+                
+                for (i=0; i<5; i++)
+                {
+                haut_myAX12->Set_Secure_Goal((150+45)-(i*14)); //150    4.5
+                milieu_myAX12->Set_Secure_Goal((150-90)+(i*8)); //150-45   4.5
+                bas_myAX12->Set_Secure_Goal((150-60)+(i*9));  //150-40     2
+                }
+                //position=5;
                 break;
                 
             case 5:
-                bas_myAX12->Set_Secure_Goal(105);
-                haut_myAX12->Set_Secure_Goal(105);
-                milieu_myAX12->Set_Secure_Goal(150);
-                break;
+                bas_myAX12->Set_Secure_Goal(150);
+                milieu_myAX12->Set_Secure_Goal(150-80);
+                haut_myAX12->Set_Secure_Goal(150);
+                //position=1;
+                break;   
+                
 
         }
         //wait(0.2);