Diff: Actionneurs/Actionneurs.cpp

Revision:

2:9e63099cca99

Parent:

1:568955af8c2b

Child:

3:e2336813297b

diff -r 568955af8c2b -r 9e63099cca99 Actionneurs/Actionneurs.cpp
--- a/Actionneurs/Actionneurs.cpp	Tue May 07 17:27:38 2019 +0000
+++ b/Actionneurs/Actionneurs.cpp	Fri May 10 09:11:09 2019 +0000
@@ -5,7 +5,7 @@
 #include "main.h"
 
 #ifdef ROBOT_SMALL
-void init_petit_robot(void)
+void gabarit_petit_robot(void)
 {
 
     uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
@@ -26,7 +26,7 @@
 
 
     uint8_t servos_sol[4] = {GLED_ON, AR_sol, GLED_ON, AV_sol};
-    uint16_t pos_sol[2] = {200,800}; //90°
+    uint16_t pos_sol[2] = {200,800}; //200 ,800 (90°)
 
     int speed=100;
 
@@ -52,17 +52,13 @@
     uint16_t pos_av_centre[2] = {470,512};
     uint16_t pos_av_droite[2] = {550,540};
 
-    int speed=25;
+    int speed=1;
 
-    deverouillage_torque();
+    //deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
     positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
     positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
-    /*
-      for(char pompe=0;pompe<8;pompe++){
-       can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
-        wait(1);
-        }*/
+
     can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AV));
 }
 
@@ -76,9 +72,9 @@
     uint16_t pos_ar_centre[2] = {500,430};
     uint16_t pos_ar_droite[2] = {430,470};
 
-    int speed=25;
+    int speed=1;
 
-    deverouillage_torque();
+    //deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
@@ -99,7 +95,6 @@
 
     int speed=25;
 
-    deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
     positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
     positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
@@ -117,7 +112,6 @@
 
     int speed=25;
 
-    deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
@@ -130,14 +124,11 @@
     uint16_t pos_av_centre[2] = {200,220};
     int speed=25;
 
-    deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
 
     char pompe=AV_CENTRE;
     can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
-    wait(1);
-    can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-    wait(3);
+
 }
 
 void goldenium_arriere(void)
@@ -145,14 +136,12 @@
     uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON,  AR_poigne_C};
     uint16_t pos_ar_centre[2] = {230,150};
     int speed=25;
-    deverouillage_torque();
+
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
 
     char pompe=AR_CENTRE;
     can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
-    wait(1);
-    can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-    wait(3);
+
 }
 
 void accelerateur_avant(void)
@@ -167,10 +156,14 @@
 
     int speed=25;
 
-    deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
     positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
     positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
+/*
+    for(char pompe=1; pompe<=3; pompe++) {
+        can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
+    }*/
+    can.write(CANMessage(HACHEUR_RELEASE_AV));
 }
 void accelerateur_arriere(void)
 {
@@ -184,10 +177,15 @@
 
     int speed=25;
 
-    deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
     positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
+/*
+    for(char pompe=5; pompe<=7; pompe++) {
+        can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
+    }*/
+        can.write(CANMessage(HACHEUR_RELEASE_AR));
+
 }
 
 
@@ -203,8 +201,6 @@
     wait(0.5);
 
     positionControl(AV_sol,800,speed,BLED_ON,4);//remonte
-    can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-    wait(3);
 }
 
 void sol_arriere(void)
@@ -218,8 +214,33 @@
     wait(0.5);
 
     positionControl(AR_sol,200,speed,BLED_ON,4);//remonte
+}
+
+
+void sol_avant_relache(void)
+{
+    int speed=1;
+    char pompe=AV_BAS;
+
+    positionControl(AV_sol,512,speed,BLED_ON,4);//descend
     can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-    wait(3);
+
+    wait(0.5);
+
+    positionControl(AV_sol,800,speed,BLED_ON,4);//remonte
+}
+
+void sol_arriere_relache(void)
+{
+    int speed=1;
+    char pompe=AR_BAS;
+
+    positionControl(AR_sol,512,speed,BLED_ON,4);//descend
+    can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
+
+    wait(0.5);
+
+    positionControl(AR_sol,200,speed,BLED_ON,4);//remonte
 }