summaryrefslogtreecommitdiffhomepage
path: root/facetracknoir/clientfiles/HAT_100/HAT_100.ino
diff options
context:
space:
mode:
Diffstat (limited to 'facetracknoir/clientfiles/HAT_100/HAT_100.ino')
-rw-r--r--facetracknoir/clientfiles/HAT_100/HAT_100.ino389
1 files changed, 389 insertions, 0 deletions
diff --git a/facetracknoir/clientfiles/HAT_100/HAT_100.ino b/facetracknoir/clientfiles/HAT_100/HAT_100.ino
new file mode 100644
index 00000000..f8644ffe
--- /dev/null
+++ b/facetracknoir/clientfiles/HAT_100/HAT_100.ino
@@ -0,0 +1,389 @@
+// Arduino sketch for MPU6050 on NanoWII using DMP MotionApps v4.1
+// HAT 14/04/2013 by FuraX49
+//
+// Head Arduino Tracker for FaceTrackNoIR
+// http://facetracknoir.sourceforge.net/home/default.htm
+// I2C device class (I2Cdev)
+// https://github.com/jrowberg/i2cdevlib
+
+
+#include <avr/eeprom.h>
+#include <Wire.h>
+#include "I2Cdev.h"
+#include "MPU6050_9Axis_MotionApps41.h"
+
+
+MPU6050 mpu;
+
+
+typedef struct {
+ int16_t Begin ; // 2 Debut
+ uint16_t Cpt ; // 2 Compteur trame or Code info or error
+ float gyro[3]; // 12 [Y, P, R] gyro
+ float acc[3]; // 12 [x, y, z] Acc
+ int16_t End ; // 2 Fin
+} _hatire;
+
+typedef struct {
+ int16_t Begin ; // 2 Debut
+ uint16_t Code ; // 2 Code info
+ char Msg[24]; // 24 Message
+ int16_t End ; // 2 Fin
+} _msginfo;
+
+typedef struct
+{
+ byte rate;
+ double gyro_offset[3] ;
+ double acc_offset[3] ;
+} _eprom_save;
+
+
+// MPU control/status vars
+bool dmpReady = false; // set true if DMP init was successful
+bool dmpLoaded = false; // set true if DMP loaded successfuly
+uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
+uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount; // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+char Commande;
+char Version[] = "HAT V 1.00";
+
+// orientation/motion vars
+Quaternion q; // [w, x, y, z] quaternion container
+VectorInt16 aa; // [x, y, z] accel sensor measurements
+VectorFloat gravity; // [x, y, z] gravity vector
+
+float Rad2Deg = (180/M_PI) ;
+
+// trame for message
+_hatire hatire;
+_msginfo msginfo;
+_eprom_save eprom_save;
+
+
+bool AskCalibrate = false; // set true when calibrating is ask
+int CptCal = 0;
+const int NbCal = 5;
+
+
+
+
+// ================================================================
+// === INTERRUPT DETECTION ROUTINE ===
+// ================================================================
+volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
+
+void dmpDataReady() {
+ mpuInterrupt = true;
+}
+
+// ================================================================
+// === PRINT SERIAL FORMATTE ===
+// ================================================================
+void PrintCodeSerial(uint16_t code,char Msg[24],bool EOL ) {
+ msginfo.Code=code;
+ memset(msginfo.Msg,0x00,24);
+ strcpy(msginfo.Msg,Msg);
+ if (EOL) msginfo.Msg[23]=0x0A;
+ // Send HATIRE message to PC
+ Serial.write((byte*)&msginfo,30);
+}
+
+
+// ================================================================
+// === INITIAL SETUP ===
+// ================================================================
+
+void setup() {
+ // join I2C bus (I2Cdev library doesn't do this automatically)
+ Wire.begin();
+
+ // initialize serial communication
+ while (!Serial); // wait for Leonardo enumeration, others continue immediately
+
+ Serial.begin(115200);
+ PrintCodeSerial(2000,Version,true);
+
+ hatire.Begin=0xAAAA;
+ hatire.Cpt=0;
+ hatire.End=0x5555;
+
+ msginfo.Begin=0xAAAA;
+ msginfo.Code=0;
+ msginfo.End=0x5555;
+
+ // initialize device
+ PrintCodeSerial(3001,"Initializing I2C",true);
+ mpu.initialize();
+
+ // verify connection
+ PrintCodeSerial(3002,"Testing connections",true);
+
+ if (mpu.testConnection()){
+ PrintCodeSerial(3003,"MPU6050 connection OK",true);
+ } else {
+ PrintCodeSerial(9007,"MPU6050 ERRROR CNX",true);
+ }
+
+ while (Serial.available() && Serial.read()); // empty buffer
+
+ // load and configure the DMP
+ PrintCodeSerial(3004,"Initializing DMP...",true);
+ devStatus = mpu.dmpInitialize();
+
+ // make sure it worked (returns 0 if so)
+ if (devStatus == 0) {
+ dmpLoaded=true;
+
+ // Read Epprom saved params
+ PrintCodeSerial(3005,"Reading saved params...",true);
+ ReadParams();
+
+ // turn on the DMP, now that it's ready
+ PrintCodeSerial(3006,"Enabling DMP...",true);
+ mpu.setDMPEnabled(true);
+
+ // enable Arduino interrupt detection
+ PrintCodeSerial(3007,"Enabling interrupt",true);
+ attachInterrupt(0, dmpDataReady, RISING);
+ mpuIntStatus = mpu.getIntStatus();
+
+ // set our DMP Ready flag so the main loop() function knows it's okay to use it
+ PrintCodeSerial(5000,"HAT BEGIN",true);
+ dmpReady = true;
+ // get expected DMP packet size for later comparison
+ packetSize = mpu.dmpGetFIFOPacketSize();
+ // Empty FIFO
+ fifoCount = mpu.getFIFOCount();
+ while (fifoCount > packetSize) {
+ fifoCount = mpu.getFIFOCount();
+ mpu.getFIFOBytes(fifoBuffer, fifoCount);
+ }
+ }
+ else {
+ // ERROR!
+ // 1 = initial memory load failed
+ // 2 = DMP configuration updates failed
+ // (if it's going to break, usually the code will be 1)
+ dmpLoaded=false;
+ PrintCodeSerial(9000+devStatus,"DMP Initialization failed",true);
+ }
+}
+
+
+// ================================================================
+// === RAZ OFFSET ===
+// ================================================================
+void razoffset() {
+ eprom_save.gyro_offset[0] = 0;
+ eprom_save.gyro_offset[1] = 0;
+ eprom_save.gyro_offset[2] = 0;
+ eprom_save.acc_offset[0] = 0;
+ eprom_save.acc_offset[1] = 0;
+ eprom_save.acc_offset[2] = 0;
+}
+
+
+// ================================================================
+// === SAVE PARAMS ===
+// ================================================================
+void SaveParams() {
+ eeprom_write_block((const void*)&eprom_save, (void*) 0, sizeof(eprom_save));
+}
+
+
+
+// ================================================================
+// === READ PARAMS ===
+// ================================================================
+void ReadParams() {
+ eeprom_read_block( (void*)&eprom_save, (void*) 0, sizeof(eprom_save));
+}
+
+
+// ================================================================
+// === Serial Command ===
+// ================================================================
+void serialEvent(){
+ Commande = (char)Serial.read();
+ switch (Commande) {
+ case 'S':
+ PrintCodeSerial(5001,"HAT START",true);
+ if (dmpLoaded==true) {
+ mpu.resetFIFO();
+ hatire.Cpt=0;
+ attachInterrupt(0, dmpDataReady, RISING);
+ mpu.setDMPEnabled(true);
+ dmpReady = true;
+ }
+ else {
+ PrintCodeSerial(9011,"Error DMP not loaded",true);
+ }
+ break;
+
+ case 's':
+ PrintCodeSerial(5002,"HAT STOP",true);
+ if (dmpReady==true) {
+ mpu.setDMPEnabled(false);
+ detachInterrupt(0);
+ dmpReady = false;
+ }
+ break;
+
+ case 'R':
+ PrintCodeSerial(5003,"HAT RESET",true);
+ if (dmpLoaded==true) {
+ mpu.setDMPEnabled(false);
+ detachInterrupt(0);
+ mpu.resetFIFO();
+ hatire.Cpt=0;
+ dmpReady = false;
+ setup();
+ }
+ else {
+ PrintCodeSerial(9011,"Error DMP not loaded",true);
+ }
+ break;
+
+
+ case 'C':
+ CptCal=0;
+ razoffset();
+ AskCalibrate=true;
+ break;
+
+ case 'V':
+ PrintCodeSerial(2000,Version,true);
+ break;
+
+ case 'I':
+ Serial.println();
+ Serial.print("Version : \t");
+ Serial.println(Version);
+ Serial.println("Gyroscopes offsets");
+ for (int i=0; i <= 2; i++) {
+ Serial.print(i);
+ Serial.print(" : ");
+ Serial.print(eprom_save.gyro_offset[i]);
+ Serial.println();
+ }
+ Serial.println("Accelerometers offsets");
+ for (int i=0; i <= 2; i++) {
+ Serial.print(i);
+ Serial.print(" : ");
+ Serial.print(eprom_save.acc_offset[i]);
+ Serial.println();
+ }
+ break;
+
+
+ default:
+ break;
+ }
+}
+
+
+// ================================================================
+// === MAIN PROGRAM LOOP ===
+// ================================================================
+void loop() {
+ // Leonardo BUG (simul Serial Event)
+ if(Serial.available() > 0) serialEvent();
+
+
+ // if programming failed, don't try to do anything
+ if (dmpReady) {
+
+
+ while (!mpuInterrupt && fifoCount < packetSize) ;
+
+ // reset interrupt flag and get INT_STATUS byte
+ mpuInterrupt = false;
+ mpuIntStatus = mpu.getIntStatus();
+
+ // get current FIFO count
+ fifoCount = mpu.getFIFOCount();
+
+ // check for overflow (this should never happen unless our code is too inefficient)
+ if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+ // reset so we can continue cleanly
+ mpu.resetFIFO();
+ PrintCodeSerial(9010,"Overflow FIFO DMP",true);
+ hatire.Cpt=0;
+
+ // otherwise, check for DMP data ready interrupt (this should happen frequently)
+ }
+ else if (mpuIntStatus & 0x02) {
+ // wait for correct available data length, should be a VERY short wait
+ while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+ // read a packet from FIFO
+ mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+ // track FIFO count here in case there is > 1 packet available
+ // (this lets us immediately read more without waiting for an interrupt)
+ fifoCount -= packetSize;
+
+ // Get Euler angles in degrees
+ mpu.dmpGetQuaternion(&q, fifoBuffer);
+ mpu.dmpGetGravity(&gravity, &q);
+ mpu.dmpGetYawPitchRoll(hatire.gyro, &q, &gravity);
+
+ // Get real acceleration, adjusted to remove gravity
+ // not used in this script
+ // mpu.dmpGetAccel(&aa, fifoBuffer);
+ // mpu.dmpGetLinearAccel(&hatire.acc, &aa, &gravity);
+
+ // Calibration sur X mesures
+ if (AskCalibrate) {
+ if ( CptCal>=NbCal) {
+ CptCal=0;
+ eprom_save.gyro_offset[0] = eprom_save.gyro_offset[0] / NbCal ;
+ eprom_save.gyro_offset[1] = eprom_save.gyro_offset[1] / NbCal ;
+ eprom_save.gyro_offset[2] = eprom_save.gyro_offset[2] / NbCal ;
+ AskCalibrate=false;
+ SaveParams();
+ }
+ else {
+ eprom_save.gyro_offset[0] += (float) hatire.gyro[0];
+ eprom_save.gyro_offset[1] += (float) hatire.gyro[1];
+ eprom_save.gyro_offset[2] += (float) hatire.gyro[2];
+
+ CptCal++;
+ }
+ }
+
+
+ // Conversion angles Euler en +-180 Degr�es
+ for (int i=0; i <= 2; i++) {
+ hatire.gyro[i]= (hatire.gyro[i] - eprom_save.gyro_offset[i] ) * Rad2Deg;
+ if (hatire.gyro[i]>180) {
+ hatire.gyro[i] = hatire.gyro[i] - 360;
+ }
+ }
+
+ if (AskCalibrate) {
+ hatire.gyro[0] = 0;
+ hatire.gyro[1] = 0;
+ hatire.gyro[2] = 0;
+ hatire.acc[0]= 0;
+ hatire.acc[1] = 0;
+ hatire.acc[2] = 0;
+ }
+
+ // Send Trame to HATIRE PC
+ Serial.write((byte*)&hatire,30);
+
+ hatire.Cpt++;
+ if (hatire.Cpt>999) {
+ hatire.Cpt=0;
+ }
+ }
+ }
+ delay(1);
+}
+
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-14 13:56:37 +0000