mirror of
https://forge.apps.education.fr/blender-edutech/blender-edutech-tutoriels.git
synced 2024-01-27 09:42:33 +01:00
Commande manuelle avec auto-horizontalité
This commit is contained in:
parent
2aa4b5a4a2
commit
54349651c5
@ -1,6 +1,7 @@
|
||||
#include "Servo.h"
|
||||
#include "Wire.h"
|
||||
#include "MPU6050.h"
|
||||
#include <stdlib.h>
|
||||
|
||||
/******************************************************************************
|
||||
* 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)
|
||||
{
|
||||
Serial.print("Mise à l'horizontale automatique");
|
||||
Serial.println();
|
||||
mode_auto_horizontal = true;
|
||||
}
|
||||
if (mode_auto_horizontal)
|
||||
digitalWrite(led_pin, HIGH); // Led
|
||||
auto_horizontal = true;
|
||||
|
||||
if (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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user