From 5f17327c0d9d5bd2da69139a324434d461519403 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 27 Nov 2013 01:33:53 +0100 Subject: initial import of hatire by Furax49 Signed-off-by: Stanislaw Halik --- facetracknoir/clientfiles/HAT_100/HAT_100.ino | 389 ++++++++++++++++++++++++++ facetracknoir/clientfiles/HAT_100/README | 35 +++ 2 files changed, 424 insertions(+) create mode 100644 facetracknoir/clientfiles/HAT_100/HAT_100.ino create mode 100644 facetracknoir/clientfiles/HAT_100/README (limited to 'facetracknoir/clientfiles') 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 +#include +#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); +} + + diff --git a/facetracknoir/clientfiles/HAT_100/README b/facetracknoir/clientfiles/HAT_100/README new file mode 100644 index 00000000..1e4f2926 --- /dev/null +++ b/facetracknoir/clientfiles/HAT_100/README @@ -0,0 +1,35 @@ + Frame exchange protocol fixed size of 30 bytes like this : + + typedef struct { + int16_t Begin ; // 2 Debut + uint16_t Cpt ; // 2 Compteur trame or Code + float gyro[3]; // 12 [Y, P, R] gyro + float acc[3]; // 12 [x, y, z] Acc + int16_t End ; // 2 Fin +} _hatire; +_hat hat; + + +void setup() { + Serial.begin(115200); + // header frame + hatire.Begin=0xAAAA; + // Frame Number or Error code + hat.Cpt=0; + // footer frame + hat.End=0x5555; +} + + + void loop() { + mpu.dmpGetYawPitchRoll(hatire.gyro); + mpu.dmpAccXYZ(hatire.acc); + // Send Trame to HATIRE PC + Serial.write((byte*)&hatire,30); + hatire.Cpt++; + if (hatire.Cpt>999) { + hatire.Cpt=0; + } + delay(1); +} + -- cgit v1.2.3