/**
* Start the device through address with offset's informed
* @param address
* @param xGyroOffset
* @param yGyroOffset
* @param zGyroOffset
* @return boolean
*/
bool MPU6050::dmpStartDevice(uint8_t address, int xGyroOffset, int yGyroOffset, int zGyroOffset) {
setAddress(address);
initialize();
setXGyroOffset(xGyroOffset);
setYGyroOffset(yGyroOffset);
setZGyroOffset(zGyroOffset);
if (testConnection())
{
// load and configure the DMP
if (dmpInitialize() == 0)
{
// turn on the DMP, now that it's ready
setDMPEnabled(true);
dmpStatus = getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
// get expected DMP packet size for later comparison
dmpPacketSize = dmpGetFIFOPacketSize();
timeReset();
resetFIFO();
return true;
}
}
return false;
}
void loop() {
// ---------- Main loop ------------
fraiseService(); // listen to Fraise events
analogService(); // analog management routine
if(delayFinished(mainDelay)) // when mainDelay triggers :
{
delayStart(mainDelay, 5000); // re-init mainDelay
analogSend(); // send analog channels that changed
rampCompute(&rampeVitesseA);
rampCompute(&rampeVitesseB);
DCMOTOR(A).Vars.PWMConsign = rampGetPos(&rampeVitesseA);
DCMOTOR(B).Vars.PWMConsign = rampGetPos(&rampeVitesseB);
if(analogGet(0) > COURANT_MAX) { if(countA < COUNT_MAX) countA++; }
else { if(countA > 0) countA--; }
if(analogGet(1) > COURANT_MAX) { if(countB < COUNT_MAX) countB++; }
else { if(countB > 0) countB--; }
if(countA == 0) { digitalSet(MAEN); digitalSet(MAEN2); }
else if(countA == COUNT_MAX) { digitalClear(MAEN); digitalClear(MAEN2); }
if(countB == 0) { digitalSet(MBEN); digitalSet(MBEN2); }
else if(countB == COUNT_MAX) { digitalClear(MBEN); digitalClear(MBEN2); }
if(digitalRead(ONOFF)) {
rampGoto(&rampeVitesseA, 0) ;
rampGoto(&rampeVitesseB, 0) ;
timeReset();
}
DCMOTOR_COMPUTE(A,SYM);
DCMOTOR_COMPUTE(B,SYM);
if(i++ == 10) {
i = 0;
printf("C P %d %d %d\n",DCMOTOR(A).Vars.PWMConsign, DCMOTOR(B).Vars.PWMConsign, digitalRead(ONOFF));
}
}
if(delayFinished(secondesDelay)) // when secondesDelay triggers :
{
delayStart(secondesDelay, 1000000); // init the secondesDelay to 1 s
Seconds++;
if(++secondes >= 60) {
secondes = 0;
if(++minutes >= 60) {
minutes = 0;
heures++;
}
}
sequenceCompute();
}
}
void timeDecrement(TTime *time)
{
if ((--time->t00_0x) < 10) return; // because of overflow (unsigned 0-1 = 255)
time->t00_0x = 9;
if ((--time->t00_x0) < 6) return;
time->t00_x0 = 5;
if ((--time->t0x_00) < 10) return;
time->t0x_00 = 9;
if ((--time->tx0_00) < 10) return;
timeReset(time);
}
void timeIncrement(TTime *time)
{
if ((++time->t00_0x) < 10) return;
time->t00_0x = 0;
if ((++time->t00_x0) < 6) return;
time->t00_x0 = 0;
if ((++time->t0x_00) < 10) return;
time->t0x_00 = 0;
if ((++time->tx0_00) < 10) return;
timeReset(time);
}
void fraiseReceiveChar() // receive text
{
unsigned char c;
c=fraiseGetChar();
if(c=='L'){ //switch LED on/off
c=fraiseGetChar();
digitalWrite(LED, c!='0');
}
else if(c=='E') { // echo text (send it back to host)
printf("C");
c = fraiseGetLen(); // get length of current packet
while(c--) printf("%c",fraiseGetChar());// send each received byte
putchar('\n'); // end of line
}
else if(c=='S') { // save EEPROM
EEwriteMain();
}
else if(c=='R') { // save EEPROM
timeReset();
}
}
void sequenceCompute()
{
printf("C T %d %d %d %ld\n",heures, minutes, secondes, Seconds);
//rampGoto(&rampeVitesseA, vitesseA);
//return;
ONTIMERAMP(0 , 0 , 0 , 0 )
ONTIMERAMP(0 , 5 , 200 , 0 )
ONTIMERAMP(0 , 15 , 200 , 200 )
ONTIMERAMP(0 , 20 , 0 , 200 )
ONTIMERAMP(0 , 30 , -100 , 0 )
ONTIMERAMP(0 , 40 , -150 , 100 )
ONTIMERAMP(0 , 55 , -250 , 250 )
ONTIMERAMP(1 , 10 , 450 , 0 )
ONTIMERAMP(1 , 30 , 0 , 450 )
ONTIMERAMP(1 , 50 , 0 , 0 )
ONTIMERAMP(2 , 20 , -1023 , 1023 )
ONTIMERAMP(2 , 35 , 1023 , -1023 )
ONTIMERAMP(2 , 50 , 0 , 0 )
IFTIME(3, 20) timeReset(); else
void initTime(TTime *time)
{
timeReset(time);
}
float MPU6050::dmpGetFirstYPRData()
{
long long time = timeCheck();
if(dmpReady && time > 0)
{
// get current FIFO count
dmpFifoCount = getFIFOCount();
if (dmpFifoCount == 1024) {
// reset so we can continue cleanly
resetFIFO();
timeReset();
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (dmpFifoCount >= 42) {
timeCount(time);
// read a packet from FIFO
getFIFOBytes(dmpFifoBuffer, dmpPacketSize);
// display quaternion values in easy matrix form: w x y z
dmpGetQuaternion(&dmpQuat, dmpFifoBuffer);
// if(dmpDebug) printf("quat %7.2f %7.2f %7.2f %7.2f ", dmpQuat.w,dmpQuat.x,dmpQuat.y,dmpQuat.z);
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
dmpGetEuler(dmpEuler, &dmpQuat);
dmpEuler[0] = dmpEuler[0] * 180/M_PI;
dmpEuler[1] = dmpEuler[1] * 180/M_PI;
dmpEuler[2] = dmpEuler[2] * 180/M_PI;
// if(dmpDebug) printf("euler %7.2f %7.2f %7.2f ", dmpEuler[0], dmpEuler[1], dmpEuler[2]);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
dmpGetGravity(&dmpGravity, &dmpQuat);
dmpGetYawPitchRoll(dmpYawPitchRoll, &dmpQuat, &dmpGravity);
dmpYawPitchRoll[0] = dmpYawPitchRoll[0] * 180/M_PI;
dmpYawPitchRoll[1] = dmpYawPitchRoll[1] * 180/M_PI;
dmpYawPitchRoll[2] = dmpYawPitchRoll[2] * 180/M_PI;
// if(dmpDebug) printf("ypr %7.2f %7.2f %7.2f ", dmpYawPitchRoll[0], dmpYawPitchRoll[1], dmpYawPitchRoll[2]);
if(dmpDebug)
return dmpYawPitchRoll[0];
#endif
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
dmpGetAccel(&dmpAccel, dmpFifoBuffer);
dmpGetGravity(&dmpGravity, &dmpQuat);
dmpGetLinearAccel(&dmpAccelReal, &dmpAccel, &dmpGravity);
// if(dmpDebug) printf("areal %6d %6d %6d ", dmpAccelReal.x, dmpAccelReal.y, dmpAccelReal.z);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
dmpGetAccel(&dmpAccel, dmpFifoBuffer);
dmpGetGravity(&dmpGravity, &dmpQuat);
dmpGetLinearAccelInWorld(&dmpAccelWorld, &dmpAccelReal, &dmpQuat);
// if(dmpDebug) printf("aworld %6d %6d %6d ", dmpAccelWorld.x, dmpAccelWorld.y, dmpAccelWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
// display quaternion values in InvenSense Teapot demo format:
dmpTeapotPacket[2] = dmpFifoBuffer[0];
dmpTeapotPacket[3] = dmpFifoBuffer[1];
dmpTeapotPacket[4] = dmpFifoBuffer[4];
dmpTeapotPacket[5] = dmpFifoBuffer[5];
dmpTeapotPacket[6] = dmpFifoBuffer[8];
dmpTeapotPacket[7] = dmpFifoBuffer[9];
dmpTeapotPacket[8] = dmpFifoBuffer[12];
dmpTeapotPacket[9] = dmpFifoBuffer[13];
//Serial.write(dmpTeapotPacket, 14);
dmpTeapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
return -255;
}
}
return -255;
}
