2023-10-12 00:09:05 +02:00
|
|
|
#include "Servo.h"
|
2023-10-04 15:27:32 +02:00
|
|
|
#include "Wire.h"
|
|
|
|
#include "MPU6050.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 int servo_resolution = 6; // Angle en µs pleine puissance
|
|
|
|
int servo_pas = 0; // Angle proportionelle en µs du pas en cours
|
|
|
|
|
|
|
|
/******************************************************************************
|
|
|
|
* 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é
|
|
|
|
|
|
|
|
/******************************************************************************
|
|
|
|
* Capteur IMU - I2C
|
2023-10-04 15:27:32 +02:00
|
|
|
******************************************************************************/
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
/******************************************************************************
|
|
|
|
* Initialisation
|
|
|
|
******************************************************************************/
|
|
|
|
|
|
|
|
void setup() {
|
|
|
|
|
|
|
|
// Liaison série
|
|
|
|
Serial.begin(115200);
|
|
|
|
|
2023-10-12 00:09:05 +02:00
|
|
|
// 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°
|
|
|
|
|
|
|
|
// Pupitre
|
|
|
|
pinMode(bt_pin, INPUT);
|
|
|
|
pinMode(jstk_x_pin, INPUT);
|
|
|
|
pinMode(jstk_y_pin, INPUT);
|
|
|
|
pinMode(led_pin, OUTPUT);
|
|
|
|
|
2023-10-04 15:27:32 +02:00
|
|
|
// IMU
|
|
|
|
Wire.begin();
|
|
|
|
accelgyro.initialize();
|
2023-10-12 00:09:05 +02:00
|
|
|
|
2023-10-04 15:27:32 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
/******************************************************************************
|
|
|
|
* 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
|
|
|
|
*****/
|
|
|
|
|
|
|
|
/* 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 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)
|
|
|
|
{
|
|
|
|
mode_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;
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
if (angle_x_ms>1600)
|
|
|
|
angle_x_ms=1600;
|
|
|
|
|
|
|
|
servo_x.writeMicroseconds(angle_x_ms);
|
|
|
|
Serial.print("angle_x_ms : ");
|
|
|
|
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");
|
|
|
|
}
|
|
|
|
|
|
|
|
/*****
|
|
|
|
* Mouvement manuel : Axe y
|
|
|
|
*****/
|
|
|
|
|
|
|
|
// en C : sur x de 249 à 773 (centre à 511) et sur y de 237 à 784 (centre à 510) -> ok
|
|
|
|
// 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)
|
|
|
|
{
|
|
|
|
mode_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;
|
|
|
|
|
|
|
|
// Mouvement y +
|
|
|
|
if (analogRead(jstk_y_pin) >532)
|
|
|
|
angle_y_ms=angle_y_ms+servo_resolution;
|
|
|
|
if (angle_y_ms>1600)
|
|
|
|
angle_y_ms=1600;
|
|
|
|
|
|
|
|
servo_y.writeMicroseconds(angle_y_ms);
|
|
|
|
Serial.print("angle_y_ms : ");
|
|
|
|
Serial.print(String(angle_y_ms));
|
|
|
|
Serial.print(" -> ");
|
|
|
|
Serial.print(String(servo_y.read()));
|
|
|
|
Serial.println("°");
|
|
|
|
}
|
|
|
|
|
|
|
|
/*****
|
|
|
|
* Mise à l'horizontale automatique
|
2023-10-04 15:27:32 +02:00
|
|
|
*****/
|
|
|
|
|
2023-10-12 00:09:05 +02:00
|
|
|
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
|
2023-10-04 15:27:32 +02:00
|
|
|
|
|
|
|
}
|