Blender-EduTech/blender-edutech-tutoriels
Blender-EduTech/blender-edutech-tutoriels
1
1
Fork 0
mirror of https://forge.apps.education.fr/blender-edutech/blender-edutech-tutoriels.git synced 2024-01-27 09:42:33 +01:00
Code Issues Projects Releases Wiki Activity
blender-edutech-tutoriels/labyrinthe/3-arduino/Test/imu-test/imu-test.ino

314 lines
12 KiB
C++
Raw Blame History

#include "Wire.h"
/******************************************************************************
* imu-test.ino
* @title: Test du capteur IMU (liaison série)
* @project: Blender-EduTech - Tutoriel : Tutoriel 3 Labyrinthe à bille - Interfacer la scène 3D avec une carte Arduino
* @lang: fr
* @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
* @copyright: Copyright (C) 2023 Philippe Roy
* @license: GNU GPL
*
******************************************************************************/
/******************************************************************************
* I2C
******************************************************************************/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
I2Cdev I2C_M;
uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
float heading;
float tiltheading;
float Axyz[3];
float roll;
float pitch;
float roll_deg;
float pitch_deg;
String roll_txt;
String pitch_txt;
/******************************************************************************
* Pupitre
******************************************************************************/
// Adressage Led Arduino
const int led = 13; // Led de mouvement (onboard)
const int led_com = 10; // Led de communication modele 3d-> arduino
// Adressage Entrees Arduino
// const int bt_a_m = A1; // Bouton A+ (verin droit)
// const int bt_a_d = A0; // Bouton A- (verin droit)
// const int bt_b_m = A3; // Bouton B+ (verin gauche arrière)
// const int bt_b_d = A2; // Bouton B- (verin gauche arrière)
// const int bt_c_m = 3; // Bouton C+ (verin gauche avant)
// const int bt_c_d = 2; // Bouton C- (verin gauche avant)
// Entrees numeriques (modele 3D)
// bool bt_a_m_num=false ; // Bouton A+ (verin droit)
// bool bt_a_d_num=false; // Bouton A- (verin droit)
// bool bt_b_m_num=false; // Bouton B+ (verin gauche arrière)
// bool bt_b_d_num=false; // Bouton B- (verin gauche arrière)
// bool bt_c_m_num=false; // Bouton C+ (verin gauche avant)
// bool bt_c_d_num=false; // Bouton C- (verin gauche avant)
// // Adressage Sorties Arduino
// const int v_a_v = 5; // Mouvement A (verin droit) : Vitesse (grove fil blanc)
// const int v_a_s = 4; // Mouvement A (verin droit) : Sens (grove fil jaune)
// const int v_b_v = 7; // Mouvement B (verin gauche arriere) : Vitesse (grove fil blanc)
// const int v_b_s = 6; // Mouvement B (verin gauche arriere) : Sens (grove fil jaune)
// const int v_c_v = 9; // Mouvement C (verin gauche avant) : Vitesse (grove fil blanc)
// const int v_c_s = 8; // Mouvement C (verin gauche avant) : Sens (grove fil jaune)
/******************************************************************************
* Communication serie
******************************************************************************/
String serial_msg = ""; // Message
bool serial_msg_complet = false; // Flag de message complet
/******************************************************************************
* Initialisation
******************************************************************************/
void setup() {
// Configure les broches des entrees
// pinMode(bt_a_m, INPUT);
// pinMode(bt_a_d, INPUT);
// pinMode(bt_b_m, INPUT);
// pinMode(bt_b_d, INPUT);
// pinMode(bt_c_m, INPUT);
// pinMode(bt_c_d, INPUT);
// // Configure les broches des sorties
// pinMode(v_a_v, OUTPUT);
// pinMode(v_a_s, OUTPUT);
// pinMode(v_b_v, OUTPUT);
// pinMode(v_b_s, OUTPUT);
// pinMode(v_c_v, OUTPUT);
// pinMode(v_c_s, OUTPUT);
pinMode(led, OUTPUT); // Led de mouvement
pinMode(led_com, OUTPUT); // Led de communication modele 3d-> arduino
digitalWrite(led, LOW);
digitalWrite(led_com, LOW);
// Moniteur serie
Serial.begin(115200); // 7 fps
/* Serial.begin(38400); */ // 6 fps
/* Serial.begin(9600); */ // trop lent 2fps
// I2C
Wire.begin();
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
}
/******************************************************************************
* Boucle principale
******************************************************************************/
void loop() {
// /*****
// * Communication : modele 3d -> arduino
// *****/
// if (serial_msg_complet) {
// if (serial_msg =="Bp Am R\n") bt_a_m_num=false;
// if (serial_msg =="Bp Am\n") bt_a_m_num=true;
// if (serial_msg =="Bp Ad R\n") bt_a_d_num=false;
// if (serial_msg =="Bp Ad\n") bt_a_d_num=true;
// if (serial_msg =="Bp Bm R\n") bt_b_m_num=false;
// if (serial_msg =="Bp Bm\n") bt_b_m_num=true;
// if (serial_msg =="Bp Bd R\n") bt_b_d_num=false;
// if (serial_msg =="Bp Bd\n") bt_b_d_num=true;
// if (serial_msg =="Bp Cm R\n") bt_c_m_num=false;
// if (serial_msg =="Bp Cm\n") bt_c_m_num=true;
// if (serial_msg =="Bp Cd R\n") bt_c_d_num=false;
// if (serial_msg =="Bp Cd\n") bt_c_d_num=true;
// /* Serial.println("Echo : "+serial_msg); */
// serial_msg = "";
// serial_msg_complet = false;
// }
// /*****
// * Verin A (verin droit)
// *****/
// // Bouton physique : LOW = actif et HIGH = pas actif
// // Bouton numérique (modele 3d) : true = actif et false = pas actif
// // A+ (sortie de tige)
// if ((digitalRead(bt_a_m) == LOW || bt_a_m_num) && digitalRead(bt_a_d) == HIGH && !bt_a_d_num) {
// /* Serial.println("A+"); */
// digitalWrite(v_a_v, HIGH); // Mouvement A : Vitesse (fil blanc)
// digitalWrite(v_a_s, LOW); // Mouvement A : Sens trigo (fil jaune)
// digitalWrite(led, HIGH);
// }
// // A- (rentrée de tige)
// if ((digitalRead(bt_a_d) == LOW || bt_a_d_num) && digitalRead(bt_a_m) == HIGH && !bt_a_m_num) {
// /* Serial.println("A-"); */
// digitalWrite(v_a_v, HIGH); // Mouvement A : Vitesse (fil blanc)
// digitalWrite(v_a_s, HIGH); // Mouvement A : Sens horaire (fil jaune)
// digitalWrite(led, HIGH);
// }
// // Stop A
// if (digitalRead(bt_a_m) == LOW && (digitalRead(bt_a_d) == LOW || bt_a_d_num)){ // Ordres contradictoires
// /* Serial.println("Stop A"); */
// digitalWrite(v_a_v, LOW); // Mouvement A : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// if (digitalRead(bt_a_d) == LOW && (digitalRead(bt_a_m) == LOW || bt_a_m_num)){ // Ordres contradictoires
// /* Serial.println("Stop A"); */
// digitalWrite(v_a_v, LOW); // Mouvement A : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// if (digitalRead(bt_a_m) == HIGH && digitalRead(bt_a_d) == HIGH && !bt_a_m_num && !bt_a_d_num){ // Aucun ordre
// /* Serial.println("Stop A"); */
// digitalWrite(v_a_v, LOW); // Mouvement A : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// /*****
// * Verin B (verin gauche arriere)
// *****/
// // B+ (sortie de tige)
// if ((digitalRead(bt_b_m) == LOW || bt_b_m_num) && digitalRead(bt_b_d) == HIGH && !bt_b_d_num) {
// /* Serial.println("B+"); */
// digitalWrite(v_b_v, HIGH); // Mouvement B : Vitesse (fil blanc)
// digitalWrite(v_b_s, LOW); // Mouvement B : Sens trigo (fil jaune)
// digitalWrite(led, HIGH);
// }
// // B- (rentrée de tige)
// if ((digitalRead(bt_b_d) == LOW || bt_b_d_num) && digitalRead(bt_b_m) == HIGH && !bt_b_m_num) {
// /* Serial.println("B-"); */
// digitalWrite(v_b_v, HIGH); // Mouvement B : Vitesse (fil blanc)
// digitalWrite(v_b_s, HIGH); // Mouvement B : Sens horaire (fil jaune)
// digitalWrite(led, HIGH);
// }
// // Stop B
// if (digitalRead(bt_b_m) == LOW && (digitalRead(bt_b_d) == LOW || bt_b_d_num)){ // Ordres contradictoires
// /* Serial.println("Stop B"); */
// digitalWrite(v_b_v, LOW); // Mouvement B : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// if (digitalRead(bt_b_d) == LOW && (digitalRead(bt_b_m) == LOW || bt_b_m_num)){ // Ordres contradictoires
// /* Serial.println("Stop B"); */
// digitalWrite(v_b_v, LOW); // Mouvement B : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// if (digitalRead(bt_b_m) == HIGH && digitalRead(bt_b_d) == HIGH && !bt_b_m_num && !bt_b_d_num){ // Aucun ordre
// /* Serial.println("Stop B"); */
// digitalWrite(v_b_v, LOW); // Mouvement B : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// /*****
// * Verin C (verin gauche avant)
// *****/
// // C+ (sortie de tige)
// if ((digitalRead(bt_c_m) == LOW || bt_c_m_num) && digitalRead(bt_c_d) == HIGH && !bt_c_d_num) {
// /* Serial.println("C+"); */
// digitalWrite(v_c_v, HIGH); // Mouvement C : Vitesse (fil blanc)
// digitalWrite(v_c_s, LOW); // Mouvement C : Sens trigo (fil jaune)
// digitalWrite(led, HIGH);
// }
// // C- (rentrée de tige)
// if ((digitalRead(bt_c_d) == LOW || bt_c_d_num) && digitalRead(bt_c_m) == HIGH && !bt_c_m_num) {
// /* Serial.println("C-"); */
// digitalWrite(v_c_v, HIGH); // Mouvement C : Vitesse (fil blanc)
// digitalWrite(v_c_s, HIGH); // Mouvement C : Sens horaire (fil jaune)
// digitalWrite(led, HIGH);
// }
// // Stop C
// if (digitalRead(bt_c_m) == LOW && (digitalRead(bt_c_d) == LOW || bt_c_d_num)){ // Ordres contradictoires
// /* Serial.println("Stop C"); */
// digitalWrite(v_c_v, LOW); // Mouvement C : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// if (digitalRead(bt_c_d) == LOW && (digitalRead(bt_c_m) == LOW || bt_c_m_num)){ // Ordres contradictoires
// /* Serial.println("Stop C"); */
// digitalWrite(v_c_v, LOW); // Mouvement C : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
// if (digitalRead(bt_c_m) == HIGH && digitalRead(bt_c_d) == HIGH && !bt_c_m_num && !bt_c_d_num){ // Aucun ordre
// /* Serial.println("Stop C"); */
// digitalWrite(v_c_v, LOW); // Mouvement C : Vitesse nulle (fil blanc)
// digitalWrite(led, LOW);
// }
/*****
* Lecture des accelerations
*****/
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)); */
pitch = -asin(Axyz[1]/cos(roll)); // dépend du positionnement du capteur (X vers la droite, Y vers l'arriere, Z vers le haut)
pitch_deg = pitch*57.3;
pitch_txt = String(pitch_deg);
/*****
* Communication : arduino -> modele 3d
*****/
/* Serial.println("Roll (Rx): "+String(roll*57.3) + " Pitch (Ry): " + String(pitch*57.3) + */
/* " bt_a_m: " + String(digitalRead(bt_a_m)) + " bt_a_d: " + String(digitalRead(bt_a_d)) + */
/* " bt_b_m: " + String(digitalRead(bt_b_m)) + " bt_b_d: " + String(digitalRead(bt_b_d)) + */
/* " bt_c_m: " + String(digitalRead(bt_c_m)) + " bt_c_d: " + String(digitalRead(bt_c_d))); */
// Serial.println("Roll (Rx): "+ roll_txt + " Pitch (Ry): " + pitch_txt +
// " bt_a_m: " + digitalRead(bt_a_m) + " bt_a_d: " + digitalRead(bt_a_d) +
// " bt_b_m: " + digitalRead(bt_b_m) + " bt_b_d: " + digitalRead(bt_b_d) +
// " bt_c_m: " + digitalRead(bt_c_m) + " bt_c_d: " + digitalRead(bt_c_d));
Serial.println("Roll (Rx): "+ roll_txt + " Pitch (Ry): " + pitch_txt);
/* delay(300); */
}
/******************************************************************************
* Evenements provoques par la communication serie
******************************************************************************/
// void serialEvent() {
// while (Serial.available()) {
// char inChar = (char)Serial.read();
// serial_msg += inChar;
// if (inChar == '\n') {
// serial_msg_complet = true;
// }
// }
// }
Reference in New Issue View Git Blame Copy Permalink