Sébastien Posada Ramirez
Lib herkulex utilisée pour la carte du phare
Diff: fonctions_herkulex.cpp
- Revision:
- 5:3de53c9af201
- Parent:
- 3:5cc8c7dfd3b7
- Child:
- 6:81733a7b69e9
--- a/fonctions_herkulex.cpp Thu May 16 10:54:05 2019 +0000
+++ b/fonctions_herkulex.cpp Mon May 20 11:38:39 2019 +0000
@@ -89,6 +89,233 @@
{
serial1.attach(&receive_serial1,Serial::RxIrq);
}
+void verification()
+{
+ uint8_t i = 0;
+ switch(etat) {
+ case pos:
+ //------------------------Status--------------------
+ Status = getStatus(pos_ID,serial_numero);
+ wait_ms(3);
+ //pc.printf("status = %d",Status);
+ switch(Status) {
+ case 0:
+ break;
+
+ case 2: //Exceed allowed POT limit
+ pc.printf("ERR-Depasse la limite de position\n");
+ //clean_ERR(pos_ID);
+ //wait_ms(500);
+ clear(pos_ID,serial_numero);
+ //positionControl(pos_ID, 1000, 3, GLED_ON);
+ wait_ms(3);
+ Status = getStatus(pos_ID,serial_numero);
+ wait_ms(3);
+ //pc.printf("status = %d",Status);
+ break;
+ }
+ //------------------Torque et position------------------------------
+ my_Tor = Get_Torque(pos_ID,serial_numero);
+ wait_ms(5);
+ //pc.printf("my_Tor = %x\n",my_Tor);
+ while(my_Tor != 0x60) {
+ setTorque(pos_ID,TORQUE_ON,serial_numero);
+ my_Tor = Get_Torque(pos_ID,serial_numero);
+ wait_ms(5);
+ }
+ Tension_inter = Get_Tension_actuelle(pos_ID,serial_numero);
+ Tension = Tension_inter*0.074;
+ if(Tension <=6.60) {
+ coeffient_time = 6;
+ } else if(Tension <= 6.90) {
+ coeffient_time = 4;
+ } else if(Tension <= 7.10) {
+ coeffient_time = 2;
+ } else if(Tension > 7.10) {
+ coeffient_time = 1;
+ }
+ get_pos = getPos(pos_ID,serial_numero);
+ pc.printf("P4=%d ",get_pos);
+ if(((get_pos - pos_position)>tolerance_en_position)||((get_pos - pos_position)<tolerance_en_position_negatif)) {
+ if((get_pos - pos_position)>tolerance_en_position) {
+ new_tempo=(get_pos - pos_position)*0.084*coeffient_time + 1;
+ if (new_tempo > 254) new_tempo = 254;
+ } else if((get_pos - pos_position)<tolerance_en_position_negatif) {
+ new_tempo=(get_pos - pos_position)*0.084*coeffient_time +1;
+ if (new_tempo > 254) new_tempo = 254;
+ }
+ positionControl(pos_ID, pos_position, new_tempo, pos_led,serial_numero);
+ pc.printf("Correction!\n");
+ }
+ break;
+ case pos_mul_complex:
+ //---------------------------Status---------------------------
+ for(i=0; i<nombre_servo; i++) {
+ Status = getStatus(data_servo_mul[1+2*i],serial_numero);
+ pc.printf("status = %d",Status);
+ switch(Status) {
+ case 0:
+ break;
+
+ case 2: //Exceed allowed POT limit
+ //pc.printf("ERR-Depasse la limite de position\n");
+ //clean_ERR(id);
+ //wait_ms(500);
+ clear(data_servo_mul[1+2*i],serial_numero);
+ //positionControl(id, 1000, 3, GLED_ON);
+ //wait_ms(3);
+ //Status = getStatus(data_servo_mul[1+2*i]);
+ //wait_ms(3);
+ //pc.printf("status = %d",Status);
+ break;
+ }
+ }
+ //----------------------Torque et position--------------------------
+ for(i=0; i<nombre_servo; i++) {
+ my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
+ while(my_Tor != 0x60) {
+ setTorque(data_servo_mul[1+2*i],TORQUE_ON,serial_numero);
+ my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
+ //pc.printf(" SET_TORQUE ");
+
+ Status = getStatus(data_servo_mul[1+2*i],serial_numero);
+ clear(data_servo_mul[1+2*i],serial_numero);
+ Status = getStatus(data_servo_mul[1+2*i],serial_numero);
+ }
+ }
+ veri = 0;
+ while(veri < nombre_servo) {
+ for(i=0; i<nombre_servo; i++) {
+ my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
+ while(my_Tor != 0x60) {
+ setTorque(data_servo_mul[1+2*i],TORQUE_ON,serial_numero);
+ my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
+ //pc.printf(" SET_TORQUE ");
+
+ Status = getStatus(data_servo_mul[1+2*i],serial_numero);
+ clear(data_servo_mul[1+2*i],serial_numero);
+ Status = getStatus(data_servo_mul[1+2*i],serial_numero);
+ }
+ }
+ for(i=0; i<nombre_servo; i++) {
+ Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i],serial_numero);
+ Tension = Tension_inter*0.074;
+ if(Tension <=6.60) {
+ coeffient_time = 6;
+ } else if(Tension <= 6.90) {
+ coeffient_time = 4;
+ } else if(Tension <= 7.10) {
+ coeffient_time = 2;
+ } else if(Tension > 7.10) {
+ coeffient_time = 1;
+ }
+ get_pos = getPos(data_servo_mul[1+2*i],serial_numero);
+ pc.printf("PosiM=%d ",get_pos);
+ if((get_pos - position_servo_mul[i])>tolerance_en_position) {
+ tab_tempo[i]=(get_pos - position_servo_mul[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ } else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
+ tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1;
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ }
+ }
+ if(flag_correction == 1) {
+ new_tempo = 0;
+ for(i=0; i<nombre_servo; i++) {
+ if(tab_tempo[i]>new_tempo) {
+ new_tempo = tab_tempo[i];
+ }
+ }
+ flag_correction = 0;
+ positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul,serial_numero);
+ pc.printf("Correction!\n");
+ }
+ veri = 0;
+ for(i=0; i<nombre_servo; i++) {
+ get_pos = getPos(data_servo_mul[1+2*i],serial_numero);
+ pc.printf("PosiM=%d ",get_pos);
+ if((get_pos - position_servo_mul[i])>tolerance_en_position) {
+ tab_tempo[i]=(get_pos - position_servo_mul[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ } else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
+ tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1;
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ } else { //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif))
+ veri++;
+ }
+ }
+ }
+ break;
+ case pos_mul_complex_different:
+ //---------------------------Status---------------------------
+ for(i=0; i<nombre_servo; i++) {
+ Status = getStatus(data_servo_mul_different[1+3*i],serial_numero);
+ //pc.printf("status = %d",Status);
+ switch(Status) {
+ case 0:
+ break;
+
+ case 2: //Exceed allowed POT limit
+ //pc.printf("ERR-Depasse la limite de position\n");
+ //clean_ERR(id);
+ //wait_ms(500);
+ clear(data_servo_mul_different[1+3*i],serial_numero);
+ //positionControl(id, 1000, 3, GLED_ON);
+ //wait_ms(3);
+ //Status = getStatus(data_servo_mul_different[1+2*i]);
+ //wait_ms(3);
+ //pc.printf("status = %d",Status);
+ break;
+ }
+ }
+ //-------------------Torque et position-----------------------------
+ for(i=0; i<nombre_servo; i++) {
+ my_Tor = Get_Torque(data_servo_mul_different[1+3*i],serial_numero);
+ while(my_Tor != 0x60) {
+ setTorque(data_servo_mul_different[1+3*i],TORQUE_ON,serial_numero);
+ my_Tor = Get_Torque(data_servo_mul_different[1+3*i],serial_numero);
+ //wait_ms(5);
+ //pc.printf(" SET_TORQUE ");
+ }
+ }
+ for(i=0; i<nombre_servo; i++) {
+ Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i],serial_numero);
+ Tension = Tension_inter*0.074;
+ if(Tension <=6.60) {
+ coeffient_time = 6;
+ } else if(Tension <= 6.90) {
+ coeffient_time = 4;
+ } else if(Tension <= 7.10) {
+ coeffient_time = 2;
+ } else if(Tension > 7.10) {
+ coeffient_time = 1;
+ }
+ get_pos = getPos(data_servo_mul_different[1+3*i],serial_numero);
+ pc.printf("PosiM=%d ",get_pos);
+ if((get_pos - position_servo_mul_different[i])>tolerance_en_position) {
+ tab_tempo[i]=(get_pos - position_servo_mul_different[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ data_servo_mul_different[2+3*i] = tab_tempo[i];
+ flag_correction = 1;
+ } else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) {
+ tab_tempo[i]=(position_servo_mul_different[i] - get_pos)*0.084*coeffient_time+1;
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ data_servo_mul_different[2+3*i] = tab_tempo[i];
+ flag_correction = 1;
+ }
+ }
+ if(flag_correction == 1) {
+ flag_correction = 0;
+ positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different,serial_numero);
+ pc.printf("Correction!\n");
+ }
+ break;
+ }
+}
//
//La fonction automate_serial1() sert à vérifier la bonne réception des données
//elle est automatiquement appelée par la fonction receive_serial1()
@@ -2251,9 +2478,9 @@
//---------------------------------------------------------------------------------------------
void deverouillage_torque (void) //débloquer les servomoteurs
{
- deverouillage_torque_avant();
- deverouillage_torque_arriere();
- deverouillage_torque_sol();
+ deverouillage_torque_avant();
+ deverouillage_torque_arriere();
+ deverouillage_torque_sol();
}
void deverouillage_torque_avant (void)
{
@@ -2290,12 +2517,13 @@
}
-void deverouillage_torque_sol (void){
-
+void deverouillage_torque_sol (void)
+{
+
clear(AR_sol,4);
clear(AV_sol,4);
setTorque(AR_sol, TORQUE_ON,4);
setTorque(AV_sol, TORQUE_ON,4);
-
- }
+
+}