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

80 lines
2.1 KiB
C++

#include "servo.h"
#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
*
******************************************************************************/
/******************************************************************************
* 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;
/******************************************************************************
* Initialisation
******************************************************************************/
void setup() {
// Liaison série
Serial.begin(115200);
// IMU
Wire.begin();
accelgyro.initialize();
}
/******************************************************************************
* 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);
/*****
* Console
*****/
Serial.print(roll_txt);
Serial.print(",");
Serial.print(pitch_txt);
Serial.println();
}