blender-edutech-tutoriels/labyrinthe/6-jumeaux/test/manette-test/manette-test.ino

302 lines
9.0 KiB
Arduino
Raw Normal View History

2023-10-12 00:09:05 +02:00
#include "Servo.h"
#include "Wire.h"
#include "MPU6050.h"
#include <stdlib.h>
/******************************************************************************
* manette-test.ino
* @title: Programme pour la carte Arduino (capteur imu + 2 servomoteurs)
* @project: Blender-EduTech - Tutoriel 6 : Labyrinthe à bille - Développer le jumeau numérique du labyrinthe
* @lang: fr
* @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
* @copyright: Copyright (C) 2023 Philippe Roy
* @license: GNU GPL
******************************************************************************/
/******************************************************************************
2023-10-12 00:09:05 +02:00
* Précautions :
* - Régler l'alimentation à 6V DC - 3 A maxi
* - Brochage pupitre : joystick -> A0, bouton -> D2, voyant -> D3
* - Brochage capteur : IMU en I2C
* - Brochage actionneurs : axes (x,y) -> D5/D6 (x,y)
* - Mettre les servomoteurs à 45° soit 1000 µs
******************************************************************************/
/******************************************************************************
* Servomoteurs
* Alimentation par servoteur : 6V DC à 1.5 A
******************************************************************************/
Servo servo_x; // Servomoteur axe x
Servo servo_y; // Servomoteur axe y
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 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)
2023-10-12 00:09:05 +02:00
/******************************************************************************
* Pupitre : joystick + 1 bouton + 1 voyant
******************************************************************************/
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
2023-10-12 00:09:05 +02:00
/******************************************************************************
* Capteur IMU - I2C
******************************************************************************/
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
float Axyz[3];
float roll;
float pitch;
float roll_deg;
float pitch_deg;
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);
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° */
Serial.println("Initialisation des servomoteurs : tout à 1000 µs -> 45°");
2023-10-12 00:09:05 +02:00
// Pupitre
pinMode(bt_pin, INPUT);
pinMode(jstk_x_pin, INPUT);
pinMode(jstk_y_pin, INPUT);
pinMode(led_pin, OUTPUT);
2023-10-12 00:09:05 +02:00
// IMU
Wire.begin();
accelgyro.initialize();
Serial.println();
}
/******************************************************************************
* Boucle principale
******************************************************************************/
void loop() {
/*****
* Lecture des accélérations
*****/
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
Axyz[0] = (double) ax / 16384;
Axyz[1] = (double) ay / 16384;
Axyz[2] = (double) az / 16384;
roll = asin(-Axyz[0]);
roll_deg = roll*57.3;
roll_txt = String(roll_deg);
pitch = -asin(Axyz[1]/cos(roll)); // position capteur (X vers la gauche, Y vers l'arriere, Z vers le haut)
pitch_deg = pitch*57.3;
pitch_txt = String(pitch_deg);
/*****
2023-10-12 00:09:05 +02:00
* Affiche les angles de roulie et de tangage
*****/
/* if (auto_horizontal) */
/* { */
/* Serial.print(roll_txt); */
/* Serial.print(","); */
/* Serial.print(pitch_txt); */
/* Serial.println(); */
/* } */
2023-10-12 00:09:05 +02:00
/*****
* Mouvement manuel : Axe x
*****/
// en C : sur x de 237 à 784 (centre à 510) et sur y de 249 à 773 (centre à 511)
2023-10-12 00:09:05 +02:00
// 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))
{
// Désactivation de l'horizontalité automatique
if (auto_horizontal)
2023-10-12 00:09:05 +02:00
{
auto_horizontal = false;
2023-10-12 00:09:05 +02:00
digitalWrite(led_pin, LOW);
}
// Mouvement x -
if (analogRead(jstk_x_pin) <491)
{
servo_pas = (servo_pas_maxi/(511-249))*(511-analogRead(jstk_x_pin));
angle_x_ms=angle_x_ms-servo_pas;
}
2023-10-12 00:09:05 +02:00
// Mouvement x +
if (analogRead(jstk_x_pin) >532)
{
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;
2023-10-12 00:09:05 +02:00
if (angle_x_ms>1600)
angle_x_ms=1600;
// Moteur
2023-10-12 00:09:05 +02:00
servo_x.writeMicroseconds(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 : ");
2023-10-12 00:09:05 +02:00
Serial.print(String(angle_x_ms));
Serial.print(" µs -> ");
Serial.print(String(servo_x.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();
2023-10-12 00:09:05 +02:00
}
2023-10-12 00:09:05 +02:00
/*****
* Mouvement manuel : Axe y
*****/
// en C : sur x de 249 à 773 (centre à 511) et sur y de 237 à 784 (centre à 510)
2023-10-12 00:09:05 +02:00
// 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))
{
// Désactivation de l'horizontalité automatique
if (auto_horizontal)
2023-10-12 00:09:05 +02:00
{
auto_horizontal = false;
2023-10-12 00:09:05 +02:00
digitalWrite(led_pin, LOW);
}
// Mouvement y -
if (analogRead(jstk_y_pin) <491)
{
servo_pas = (servo_pas_maxi/(511-237))*(511-analogRead(jstk_y_pin));
angle_y_ms=angle_y_ms-servo_pas;
}
2023-10-12 00:09:05 +02:00
// Mouvement y +
if (analogRead(jstk_y_pin) >532)
{
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;
2023-10-12 00:09:05 +02:00
if (angle_y_ms>1600)
angle_y_ms=1600;
// Moteur
2023-10-12 00:09:05 +02:00
servo_y.writeMicroseconds(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 : ");
2023-10-12 00:09:05 +02:00
Serial.print(String(angle_y_ms));
Serial.print(" µs -> ");
2023-10-12 00:09:05 +02:00
Serial.print(String(servo_y.read()));
// Affichage du capteur IMU
Serial.print(" ; IMU roll : ");
Serial.print(roll_txt);
Serial.print(" ; IMU pitch : ");
Serial.print(pitch_txt);
Serial.println();
2023-10-12 00:09:05 +02:00
}
/*****
* Mise à l'horizontale automatique
*****/
2023-10-12 00:09:05 +02:00
if (digitalRead(bt_pin) == HIGH)
auto_horizontal = true;
if (auto_horizontal)
2023-10-12 00:09:05 +02:00
{
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();
}
}
}