mirror of
https://forge.apps.education.fr/blender-edutech/blender-edutech-tutoriels.git
synced 2024-01-27 09:42:33 +01:00
150 lines
4.6 KiB
C++
150 lines
4.6 KiB
C++
#include "Wire.h"
|
|
#include "Grove_LED_Matrix_Driver_HT16K33.h"
|
|
#include "MPU6050.h"
|
|
|
|
/******************************************************************************
|
|
* 4-labyrinthe-imu.ino
|
|
* @title: Programme pour la carte Arduino de gestion de centrale inertielle (capteur IMU)
|
|
* @project: Blender-EduTech - Tutoriel 3 : Labyrinthe à bille - Interfacer avec une carte Arduino par la liaision série
|
|
* @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;
|
|
|
|
/******************************************************************************
|
|
* Led Matrix - I2C
|
|
******************************************************************************/
|
|
|
|
Matrix_8x8 matrix;
|
|
|
|
/******************************************************************************
|
|
* Communication serie
|
|
******************************************************************************/
|
|
|
|
String serial_msg = ""; // Message
|
|
uint64_t serial_msg_int;
|
|
String serial_msg_old = ""; // Message
|
|
bool serial_msg_complet = false; // Flag de message complet
|
|
|
|
/******************************************************************************
|
|
* Initialisation
|
|
******************************************************************************/
|
|
|
|
void setup() {
|
|
|
|
// Liaison série
|
|
Serial.begin(115200); // 7 fps
|
|
/* Serial.begin(38400); */ // 6 fps
|
|
/* Serial.begin(9600); */ // trop lent 2fps
|
|
|
|
// IMU
|
|
Wire.begin();
|
|
accelgyro.initialize();
|
|
|
|
// Led Matrix
|
|
matrix.init();
|
|
matrix.setBrightness(0);
|
|
matrix.setBlinkRate(BLINK_OFF);
|
|
matrix.clear();
|
|
matrix.display();
|
|
|
|
// Ok
|
|
// Serial.println("Initialisation des composants I2C.");
|
|
}
|
|
|
|
/******************************************************************************
|
|
* Boucle principale
|
|
******************************************************************************/
|
|
|
|
void loop() {
|
|
|
|
/*****
|
|
* 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)); // position capteur (X vers la gauche, Y vers l'arriere, Z vers le haut)
|
|
pitch_deg = pitch*57.3;
|
|
pitch_txt = String(pitch_deg);
|
|
|
|
/*****
|
|
* IMU : Arduino -> UPBGE
|
|
*****/
|
|
|
|
//Serial.println("Roll (Rx): "+ roll_txt + " Pitch (Ry): " + pitch_txt);
|
|
Serial.print(roll_txt);
|
|
Serial.print(",");
|
|
Serial.print(pitch_txt);
|
|
Serial.println();
|
|
|
|
/*****
|
|
* Led Matrix : UPBGE -> Arduino
|
|
* https://xantorohara.github.io/led-matrix-editor
|
|
* labyrinte : 0xff90b2a2a6a4ae82
|
|
*****/
|
|
|
|
if (serial_msg_complet) {
|
|
/* matrix.writeString(serial_msg, 1000, ACTION_SCROLLING); */
|
|
/* matrix.display(); */
|
|
if (serial_msg.length() ==2) {
|
|
if (serial_msg[0] == 'X') matrix.clear();
|
|
if (serial_msg[0] == 'N') matrix.writeOnePicture(0x00000000000000ff);
|
|
if (serial_msg[0] == 'E') matrix.writeOnePicture(0x8080808080808080);
|
|
if (serial_msg[0] == 'S') matrix.writeOnePicture(0xff00000000000000);
|
|
if (serial_msg[0] == 'O') matrix.writeOnePicture(0x0101010101010101);
|
|
}
|
|
if (serial_msg.length() ==3) {
|
|
if ((serial_msg[0] == 'N') and (serial_msg[1] == 'E')) matrix.writeOnePicture(0x00000000808080f0);
|
|
if ((serial_msg[0] == 'S') and (serial_msg[1] == 'E')) matrix.writeOnePicture(0xf080808000000000);
|
|
if ((serial_msg[0] == 'S') and (serial_msg[1] == 'O')) matrix.writeOnePicture(0x0f01010100000000);
|
|
if ((serial_msg[0] == 'N') and (serial_msg[1] == 'O')) matrix.writeOnePicture(0x000000000101010f);
|
|
}
|
|
matrix.display();
|
|
serial_msg = "";
|
|
serial_msg_complet = false;
|
|
}
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* Évenement provoqué UPBGE
|
|
******************************************************************************/
|
|
|
|
void serialEvent() {
|
|
while (Serial.available()) {
|
|
char inChar = (char)Serial.read();
|
|
serial_msg += inChar;
|
|
if (inChar == '\n') {
|
|
serial_msg_complet = true;
|
|
}
|
|
}
|
|
}
|
|
|