Diff: fonctions_herkulex.cpp

Revision:

13:050a0cb110b5

Parent:

12:562aa318952c

Child:

14:a6e5d1ce2133

--- a/fonctions_herkulex.cpp	Mon May 27 15:55:43 2019 +0000
+++ b/fonctions_herkulex.cpp	Fri May 31 20:52:03 2019 +0000
@@ -1083,9 +1083,14 @@
 
     posCible=getPos(ID,serial);//position;
     
-    pc.printf("\nPos depart :%d",posCible);
+    //pc.printf("\nPos depart :%d",posCible);
     velocityControl(ID,speed,setLED,serial);
     wait_ms(100);
+    if(tour == 0){
+        etat = 2;
+        tour = 1;
+    }
+        
 
     if(speed > 0) {
         while(end != 1) {
@@ -1094,20 +1099,19 @@
                     pos = getPos(ID,serial);
                     posAct = pos - posCible;
                     if(posAct<0){
-                        pc.printf("\n0 pos :%d",pos);
+                        //pc.printf("\n0 pos :%d",pos);
                         clear(ID,serial);
                         setTorque(ID, TORQUE_ON,serial);
                         etat=1;
                     }
                    
                     break;
-
                 case 1 :
                     pos = getPos(ID,serial);
                     velocityControl(ID,speed,RLED_ON,serial);
                     posAct = posCible - pos;
                     if (posAct < 0 ){
-                        pc.printf("\n1 pos :%d",pos);
+                        //pc.printf("\n1 pos :%d",pos);
                          etat = 2;
                     }
                     break;
@@ -1119,13 +1123,13 @@
                     
                     if (flagTour == tour-1) {
                         if (getPos(ID,serial) > position && !flagDernierTour){
-                           pc.printf("\nTour sup");
+                           //pc.printf("\nTour sup");
                            // flagTour++;
                             flagDernierTour =1;
                             etat =0;
                             posCible = position;
                         }else{
-                            pc.printf("\ngoto pos finale");
+                            //pc.printf("\ngoto pos finale");
                         velocityControl(ID,0,setLED,serial);
                         positionControl(ID,position,1,setLED,serial);
                         end = 1;
@@ -1134,7 +1138,7 @@
                     
                     else {
                         flagTour=flagTour+1;
-                        pc.printf("\ntours execute : %d",flagTour);
+                        //pc.printf("\ntours execute : %d",flagTour);
                         etat = 0;
                     }
                     break;
@@ -1147,7 +1151,7 @@
                     pos = getPos(ID,serial);
                     posAct = pos - posCible;
                     if(posAct>0){
-                        pc.printf("\n0 pos :%d",pos);
+                        //pc.printf("\n0 pos :%d",pos);
                         clear(ID,serial);
                         setTorque(ID, TORQUE_ON,serial);
                         etat=1;
@@ -1160,7 +1164,7 @@
                     velocityControl(ID,speed,RLED_ON,serial);
                     posAct = posCible - pos;
                     if (posAct > 0 ){
-                        pc.printf("\n1 pos :%d",pos);
+                        //pc.printf("\n1 pos :%d",pos);
                          etat = 2;
                     }
                     break;
@@ -1172,13 +1176,13 @@
                     
                     if (flagTour == tour-1) {
                         if (getPos(ID,serial) < position && !flagDernierTour){
-                           pc.printf("\nTour sup");
+                           //pc.printf("\nTour sup");
                            // flagTour++;
                             flagDernierTour =1;
                             etat =0;
                             posCible = position;
                         }else{
-                            pc.printf("\ngoto pos finale");
+                            //pc.printf("\ngoto pos finale");
                         velocityControl(ID,0,setLED,serial);
                         positionControl(ID,position,1,setLED,serial);
                         end = 1;
@@ -1187,7 +1191,7 @@
                     
                     else {
                         flagTour=flagTour+1;
-                        pc.printf("\ntours execute : %d",flagTour);
+                        //pc.printf("\ntours execute : %d",flagTour);
                         etat = 0;
                     }
                     break;