une 30aine passés sans pb entre les trames
Dependencies: ADS1015 ARNSRS_SENSORS DmTftLibrary MS5803_14BA SDFileSystem mbed
Fork of ARNSRS_SERVOS_USB_TFT by
Diff: main.cpp
- Revision:
- 22:8e4ace42e4f5
- Parent:
- 21:28a3f60b4bc2
- Child:
- 23:40a6039cb140
diff -r 28a3f60b4bc2 -r 8e4ace42e4f5 main.cpp --- a/main.cpp Thu May 18 17:33:38 2017 +0000 +++ b/main.cpp Fri May 19 00:01:17 2017 +0000 @@ -263,9 +263,12 @@ { //Variables pour décomposition du message - char *commande, *valeur; + char *commande=" "; + char *valeur=" "; //La commande + // un message valide est du type [Sx];yyy avec x numero du servo et yyy compris entre 000 et 100 + commande = strtok(message_Android, ",;"); DEBUG("Commande = %s\n", commande); @@ -277,17 +280,19 @@ //Commande servo1 if(strcmp(commande, "[S1]") == 0) { - servo1 = atoi(valeur) / 100.f; + servo1= atoi(valeur) / 100.f; DEBUG(" Servo 1 = %s\n", valeur); - indexCommande = 0; + //indexCommande = 0; } //Commande servo2 if(strcmp(commande, "[S2]") == 0) { - servo2 = atoi(valeur) / 100.f; + servo2 =atoi(valeur) / 100.f; DEBUG("Servo 2 = %f\r\n", valeur); - indexCommande = 0; + //indexCommande = 0; } + + indexCommande = 0; } void callbackServo() @@ -380,47 +385,48 @@ void Calibration_servo() { //Servos, calibration du feedback - DEBUG(" Check Servos :\r\n") + DEBUG(" Check Servos :\r\n"); DEBUG("\r\n"); - servo1.period(0.001); + servo1.period(0.001); // à mettre dans le setup si cette calib n'est pas appelée (je l ai trouvée commentée) servo2.period(0.001); //Rentrer les servos à fond servo1 = 0; servo2 = 0; wait(4); - Limit_max_S1 = FeedBack1.read(); + Limit_min_S1 = 1-FeedBack1.read(); + wait_ms(100); - Limit_max_S2 = FeedBack2.read(); + Limit_min_S2 = 1-FeedBack2.read(); - DEBUG(" FeedBack 1 MIN = %f\n", Limit_max_S1); - DEBUG(" FeedBack 2 MIN = %f\n", Limit_max_S2); + DEBUG(" FeedBack 1 min = %f\n", Limit_min_S1); + DEBUG(" FeedBack 2 min = %f\n", Limit_min_S2); DEBUG("\r\n"); //Sortir les servos à fond servo1 = 1; servo2 = 1; wait(4); - Limit_min_S1 = FeedBack1.read(); + Limit_max_S1 = 1-FeedBack1.read(); wait_ms(100); - Limit_min_S2 = FeedBack2.read(); + Limit_max_S2 = 1-FeedBack2.read(); - DEBUG(" FeedBack 1 MAX = %f\n", Limit_min_S1); - DEBUG(" FeedBack 2 MAX = %f\n", Limit_min_S2); + DEBUG(" FeedBack 1 Max = %f\n", Limit_max_S1); + DEBUG(" FeedBack 2 Max = %f\n", Limit_max_S2); DEBUG("\r\n"); //Position milieu - servo1 = 0.5; - servo2 = 0.5; - wait(2); + servo1 = (Limit_min_S1+Limit_max_S1)*0.5; + servo2 = (Limit_min_S2+Limit_max_S2)*0.5; + wait(4); //Mesure du delta consigne / feedback - Delta_FB_1 = abs(remap(FeedBack1, Limit_max_S1, Limit_min_S1, 0, 100) - 50); + Delta_FB_1 = abs(remap(FeedBack1, Limit_min_S1, Limit_max_S1, 0, 100)-50); Delta_FB_2 = abs(remap(FeedBack2, Limit_max_S2, Limit_min_S2, 0, 100) - 50); DEBUG(" Delta Servo 1 = %f\n", Delta_FB_1); DEBUG(" Delta Servo 2 = %f\n", Delta_FB_2); - if(Delta_FB_1 > 10 || Delta_FB_1 > 10) DEBUG(" Delta Servos non satisfaisant..."); + if(Delta_FB_1 > 10 || Delta_FB_2 > 10) DEBUG(" Delta Servos non satisfaisant..."); } //////////////////////////////////////////////////////////////////////////////// @@ -560,7 +566,9 @@ //SetTime(); //Calibration des limites des servos pour remapper les feedback - //Calibration_servo(); + Calibration_servo(); + //servo1.period(0.001); // a mettre si pas de calib + //servo2.period(0.001); // a mettre si pas de calib //Démarrage de l'écran DEBUG("Initialisation TFT\r\n"); @@ -606,8 +614,8 @@ ppO2 = arnsrs.requestPpO2(false); //Retour position des servos - Servo1_POS = remap(FeedBack1, Limit_max_S1, Limit_min_S1, 0, 100); - Servo2_POS = remap(FeedBack2, Limit_max_S2, Limit_min_S2, 0, 100); + Servo1_POS = 1-FeedBack1;//remap(FeedBack1, Limit_max_S1, Limit_min_S1, 0, 100); + //Servo2_POS = remap(FeedBack2, Limit_max_S2, Limit_min_S2, 0, 100); //Debug Debug_Complet(); @@ -621,7 +629,7 @@ build_send_Message_float("v", Humi, pression); build_send_Message_float("f", Servo1_POS, Servo2_POS); - Servo_USB(commande_servo); + if (indexCommande>0) Servo_USB(commande_servo); if(FLAG_PAGE_1) { tft.drawNumber(65,35,co2 ,3, false);