From 54349651c5214fb134357cdd1431810cdcbae70e Mon Sep 17 00:00:00 2001 From: Philippe Roy Date: Fri, 13 Oct 2023 18:36:48 +0200 Subject: [PATCH] =?UTF-8?q?Commande=20manuelle=20avec=20auto-horizontalit?= =?UTF-8?q?=C3=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../test/manette-test/manette-test.ino | 220 ++++++++++++------ 1 file changed, 148 insertions(+), 72 deletions(-) diff --git a/labyrinthe/6-jumeaux/test/manette-test/manette-test.ino b/labyrinthe/6-jumeaux/test/manette-test/manette-test.ino index c794751..b3764c4 100644 --- a/labyrinthe/6-jumeaux/test/manette-test/manette-test.ino +++ b/labyrinthe/6-jumeaux/test/manette-test/manette-test.ino @@ -1,6 +1,7 @@ #include "Servo.h" #include "Wire.h" #include "MPU6050.h" +#include /****************************************************************************** * manette-test.ino @@ -32,18 +33,18 @@ const int servo_x_pin = 5; // Servomoteur axe x sur broche D5 const int servo_y_pin = 6; // Servomoteur axe y sur broche D6 int angle_x_ms; // Angle en µs : 500 -> 0° ; 1500->90° et 2500-> 180° int angle_y_ms; // Angle en µs : 500 -> 0° ; 1500->90° et 2500-> 180° -const int servo_resolution = 6; // Angle en µs pleine puissance -int servo_pas = 0; // Angle proportionelle en µs du pas en cours +const float servo_pas_maxi = 20; // Pas angulaire (µs) maxi (pas de calcul en int) +const int servo_pas_auto = 5; // Pas angulaire (µs) pour le mode auto-horizontalité +int servo_pas = 0; // Pas angulaire (µs) du mouvement en cours (pas propotionel au joystick) /****************************************************************************** * Pupitre : joystick + 1 bouton + 1 voyant ******************************************************************************/ -const int bt_pin = 2; // Bouton sur broche D2 -const int led_pin = 3; // Led sur broche D3 -const int jstk_x_pin = A1; // Joystick axe x sur broche A1 -const int jstk_y_pin = A0; // Joystick axe y sur broche A0 -bool mode_auto_horizontal = false; // Flag du mode de la mise à l'auto-horizontalité +const int bt_pin = 2; // Bouton du mode d'auto-horizontalité sur la broche D2 +const int led_pin = 3; // Led du mode d'auto-horizontalité sur la broche D3 +const int jstk_x_pin = A1; // Joystick axe x sur la broche A1 +const int jstk_y_pin = A0; // Joystick axe y sur la broche A0 /****************************************************************************** * Capteur IMU - I2C @@ -63,34 +64,41 @@ String roll_txt; String pitch_txt; String serial_msg_int_txt; +bool auto_horizontal = false; // Flag du mode de la mise à l'auto-horizontalité +bool auto_horizontal_mvt = false; // Flag du mode de la mise à l'auto-horizontalité +const int auto_horizontal_sensibilite = 1; // Angle maxi pour l'auto-horizontalité + /****************************************************************************** * Initialisation ******************************************************************************/ void setup() { - // Liaison série - Serial.begin(115200); + // Liaison série + Serial.begin(115200); + Serial.println("Initialisation ... "); + Serial.println("Initialisation du port série : 115200 bauds"); - // Servomoteurs - servo_x.attach(servo_x_pin); // Brochage - servo_y.attach(servo_y_pin); // Brochage - angle_x_ms = 1000; // 500 -> 0°, 1500 -> 90° et 2500-> 180° - angle_y_ms = 1000; // 500 -> 0°, 1500 -> 90° et 2500-> 180° - servo_x.writeMicroseconds(angle_x_ms); // 1000 -> 45° - servo_y.writeMicroseconds(angle_y_ms); // 1000 -> 45° + // Servomoteurs + servo_x.attach(servo_x_pin); // Brochage + servo_y.attach(servo_y_pin); // Brochage + angle_x_ms = 1000; // 500 -> 0°, 1500 -> 90° et 2500-> 180° + angle_y_ms = 1000; // 500 -> 0°, 1500 -> 90° et 2500-> 180° + servo_x.writeMicroseconds(angle_x_ms); // 1000 -> 45° */ + servo_y.writeMicroseconds(angle_y_ms); // 1000 -> 45° */ + Serial.println("Initialisation des servomoteurs : tout à 1000 µs -> 45°"); // Pupitre - pinMode(bt_pin, INPUT); - pinMode(jstk_x_pin, INPUT); - pinMode(jstk_y_pin, INPUT); - pinMode(led_pin, OUTPUT); + pinMode(bt_pin, INPUT); + pinMode(jstk_x_pin, INPUT); + pinMode(jstk_y_pin, INPUT); + pinMode(led_pin, OUTPUT); - // IMU - Wire.begin(); - accelgyro.initialize(); - - } + // IMU + Wire.begin(); + accelgyro.initialize(); + Serial.println(); +} /****************************************************************************** * Boucle principale @@ -117,96 +125,130 @@ void loop() { * Affiche les angles de roulie et de tangage *****/ - /* Serial.print(roll_txt); */ - /* Serial.print(","); */ - /* Serial.print(pitch_txt); */ - /* Serial.println(); */ + /* if (auto_horizontal) */ + /* { */ + /* Serial.print(roll_txt); */ + /* Serial.print(","); */ + /* Serial.print(pitch_txt); */ + /* Serial.println(); */ + /* } */ /***** * Mouvement manuel : Axe x *****/ - // en C : sur x de 237 à 784 (centre à 510) et sur y de 249 à 773 (centre à 511) -> ok + // en C : sur x de 237 à 784 (centre à 510) et sur y de 249 à 773 (centre à 511) // en Python de 0,25 à 0,75 avec une zone morte entre 0,48 et 0,52 (soit 491 et 532 pour une base sur 1024) if ((analogRead(jstk_x_pin) <491) or (analogRead(jstk_x_pin) >532)) { - Serial.print("jstk_x : "); - Serial.print(String(analogRead(jstk_x_pin))); - Serial.println(); // Désactivation de l'horizontalité automatique - if (mode_auto_horizontal) + if (auto_horizontal) { - mode_auto_horizontal = false; + auto_horizontal = false; digitalWrite(led_pin, LOW); } // Mouvement x - if (analogRead(jstk_x_pin) <491) - // FIXME : faire la commande proportionelle - /* servo_pas = round((servo_resolution/(511-249))*(511-analogRead(jstk_x_pin))); */ - /* angle_x_ms=angle_x_ms-servo_pas; */ - angle_x_ms=angle_x_ms-servo_resolution; - if (angle_x_ms<600) - angle_x_ms=600; + { + servo_pas = (servo_pas_maxi/(511-249))*(511-analogRead(jstk_x_pin)); + angle_x_ms=angle_x_ms-servo_pas; + } // Mouvement x + if (analogRead(jstk_x_pin) >532) - // FIXME : faire la commande proportionelle - /* servo_pas = round((servo_resolution/(773-511))*(analogRead(jstk_x_pin)-511)); */ - /* angle_x_ms=angle_x_ms+servo_pas; */ - angle_x_ms=angle_x_ms+servo_resolution; + { + servo_pas = (servo_pas_maxi/(773-511))*(analogRead(jstk_x_pin)-511); + angle_x_ms=angle_x_ms+servo_pas; + } + + // Fin de course logiciel + if (angle_x_ms<600) + angle_x_ms=600; if (angle_x_ms>1600) angle_x_ms=1600; - + + // Moteur servo_x.writeMicroseconds(angle_x_ms); - Serial.print("angle_x_ms : "); + + // Affichage de l'angle du mouvement + Serial.print("Joystick x : "); + Serial.print(String(analogRead(jstk_x_pin))); + Serial.print(" -> pas : "); + Serial.print(String(servo_pas)); + Serial.print(" µs ; "); + Serial.print("Angle x : "); Serial.print(String(angle_x_ms)); Serial.print(" µs -> "); Serial.print(String(servo_x.read())); - Serial.print("° ; servo_pas : "); - Serial.print(String(servo_pas)); - Serial.println(" µs"); + Serial.println(); + + // Affichage du capteur IMU + Serial.print("IMU roll : "); + Serial.print(roll_txt); + Serial.print(" ; IMU pitch : "); + Serial.print(pitch_txt); + Serial.println(); } - + /***** * Mouvement manuel : Axe y *****/ - // en C : sur x de 249 à 773 (centre à 511) et sur y de 237 à 784 (centre à 510) -> ok + // en C : sur x de 249 à 773 (centre à 511) et sur y de 237 à 784 (centre à 510) // en Python de 0,25 à 0,75 avec une zone morte entre 0,48 et 0,52 (soit 491 et 532 pour une base sur 1024) if ((analogRead(jstk_y_pin) <491) or (analogRead(jstk_y_pin) >532)) { - Serial.print("jstk_y : "); - Serial.print(String(analogRead(jstk_y_pin))); - Serial.println(); // Désactivation de l'horizontalité automatique - if (mode_auto_horizontal) + if (auto_horizontal) { - mode_auto_horizontal = false; + auto_horizontal = false; digitalWrite(led_pin, LOW); } // Mouvement y - if (analogRead(jstk_y_pin) <491) - angle_y_ms=angle_y_ms-servo_resolution; - if (angle_y_ms<600) - angle_y_ms=600; - + { + servo_pas = (servo_pas_maxi/(511-237))*(511-analogRead(jstk_y_pin)); + angle_y_ms=angle_y_ms-servo_pas; + } + // Mouvement y + if (analogRead(jstk_y_pin) >532) - angle_y_ms=angle_y_ms+servo_resolution; + { + servo_pas = (servo_pas_maxi/(784-511))*(analogRead(jstk_y_pin)-511); + angle_y_ms=angle_y_ms+servo_pas; + } + + // Fin de course logiciel + if (angle_y_ms<600) + angle_y_ms=600; if (angle_y_ms>1600) angle_y_ms=1600; - + + // Moteur servo_y.writeMicroseconds(angle_y_ms); - Serial.print("angle_y_ms : "); + + // Affichage de l'angle du mouvement + Serial.print("Joystick y : "); + Serial.print(String(analogRead(jstk_y_pin))); + Serial.print(" -> pas : "); + Serial.print(String(servo_pas)); + Serial.print(" µs ; "); + Serial.print("Angle y : "); Serial.print(String(angle_y_ms)); - Serial.print(" -> "); + Serial.print(" µs -> "); Serial.print(String(servo_y.read())); - Serial.println("°"); + + // Affichage du capteur IMU + Serial.print(" ; IMU roll : "); + Serial.print(roll_txt); + Serial.print(" ; IMU pitch : "); + Serial.print(pitch_txt); + Serial.println(); } /***** @@ -214,12 +256,46 @@ void loop() { *****/ if (digitalRead(bt_pin) == HIGH) + auto_horizontal = true; + + if (auto_horizontal) { - Serial.print("Mise à l'horizontale automatique"); - Serial.println(); - mode_auto_horizontal = true; - } - if (mode_auto_horizontal) digitalWrite(led_pin, HIGH); // Led - + auto_horizontal_mvt = false; + + // Mouvemeent + if ((roll_deg < -1) or (roll_deg > 1) or (pitch_deg < -1) or (pitch_deg > 1)) + auto_horizontal_mvt = true; + if (roll_deg < -1) + angle_y_ms=angle_y_ms-servo_pas_auto; + if (roll_deg > 1) + angle_y_ms=angle_y_ms+servo_pas_auto; + if (pitch_deg < -1) + angle_x_ms=angle_x_ms+servo_pas_auto; + if (pitch_deg > 1) + angle_x_ms=angle_x_ms-servo_pas_auto; + + // Fin de course logiciel + if (angle_x_ms<600) + angle_x_ms=600; + if (angle_x_ms>1600) + angle_x_ms=1600; + if (angle_y_ms<600) + angle_y_ms=600; + if (angle_y_ms>1600) + angle_y_ms=1600; + + // Moteurs + if (auto_horizontal_mvt) + { + servo_x.writeMicroseconds(angle_x_ms); + servo_y.writeMicroseconds(angle_y_ms); + Serial.print("Mise à l'horizontale automatique : "); + Serial.print("IMU roll : "); + Serial.print(roll_txt); + Serial.print(" ; IMU pitch : "); + Serial.print(pitch_txt); + Serial.println(); + } + } }