C++ MPU6050::getAcceleration方法代码示例

void calibrate_accel(){
  xoff = accelgyro.getXAccelOffset();
  yoff = accelgyro.getYAccelOffset();
  zoff = accelgyro.getZAccelOffset();
  byte i=0;
  while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
    accelgyro.getAcceleration(&ax, &ay, &az);
    if (count == SAMPLE_COUNT){
      xoff += int(axm/-6);
      yoff += int(aym/-6);
      zoff += int((azm+16384)/-6);
      accelgyro.setXAccelOffset(xoff);
      accelgyro.setYAccelOffset(yoff);
      accelgyro.setZAccelOffset(zoff);
      #ifdef CAL_DEBUG
        Serial.print(axm); Serial.print(" ");
        Serial.print(aym); Serial.print(" ");
        Serial.println(azm);
        Serial.print(xoff); Serial.print(" ");
        Serial.print(yoff); Serial.print(" ");
        Serial.println(zoff);
        Serial.println("*********************");
      #endif
      count = 0;
      i++; //iteration++
      #ifdef CAL_DEBUG
        Serial.print(".");
      #endif
    }
    else{
      axm = (axm*count + ax)/(count+1.0);
      aym = (aym*count + ay)/(count+1.0);
      azm = (azm*count + az)/(count+1.0);
      count++;
    }
  }
  #ifdef CAL_DEBUG
    Serial.println(" Done.");
  #endif
}

//.........这里部分代码省略.........

        // //if((encTimer.read_us() - lastEncTime) > 0.0f ) {   // every 100 ms
            
        //     float sign = 0;
        //     if(positionOffset > 0) sign= 1;
        //     else sign = -1;


        //     if(abs(positionOffset) > zoneA) angleOffset += 1.0f/zoneAscale*positionOffset;
        //     else if(abs(positionOffset) > zoneB) angleOffset += 1.0f/zoneBscale*positionOffset;
        //     else if(abs(positionOffset) > zoneC) angleOffset += 1.0f/zoneCscale*positionOffset;
        //     else angleOffset += positionOffset/zoneDscale;

        //     printf("angleOffset: %f, positionoffset: %f\n", angleOffset, positionOffset );
        //     // Estimate velocity
        //     // 
        //     velocity = (position - lastPosition);
        //     lastPosition = position;

        //     angleOffset += velocity/velocityScale;

        //     angleOffset = constrain(angleOffset,-10, 10);
            
        //     lastAngleoffset = angleOffset;
        // //}

        // angleOffset = constrain(angleOffset,lastAngleoffset - 1, lastAngleoffset + 1);
        
        timer.reset();
        ///////////////////////////
        // Get gyro/accel values //
        ///////////////////////////
        
        accelgyro.getAcceleration(&ax,&ay,&az);
        measuredRate = accelgyro.getRotationX()*1.0/131.0f;  // Units depend on config. Right now 250o/s
        measuredTheta = -atan(1.0*ay/az);   
        measuredTheta = measuredTheta*180/PI;  // measured in degrees
        
        ///////////////////
        // Kalman Filter //
        ///////////////////
        dt = (double)(code.read_us() - oldTime)/1000000.0f;
        newTheta = kalman.getAngle(measuredTheta,
            -measuredRate, dt);
        
        //DEBUG: printf("%g \n",  (double)(code.read_us() - oldTime)/1000000.0f);
        oldTime = code.read_us();

        //////////////////
        // Control loop //
        //////////////////

        // Set control variable to zero
        u = 0;
        
        // Extract constants from k vector, which has the serial readings.
        float kp = k[0];
        float ki = k[1];
        float kd = k[2];
        tref = k[3] - angleOffset;
        waittime = k[4];

        if(newTheta >= 50 || newTheta <= -50){
            u = 0;
        }else{

// PUBLIC METHOD TO PROVIDE DATA ACQUIRED FROM MPU TO DEPENDENT SYSTEMS
uint8_t mpuAcquire(int16_t *accelOffsetX, int16_t *accelOffsetY,int16_t *accelOffsetZ, int16_t *currAccelX, int16_t *currAccelY, int16_t *currAccelZ, int16_t *currYaw, int16_t *currPitch, int16_t *currRoll){ /*Changed to int16*/

int16_t rot[3]; //Equivalent to uint8_t yaw; uint8_t pitch; uint8_t roll;
int16_t accel[3];//Equivalent to uint8_t accelX; uint8_t accelY; uint8_t accelZ;
//Error flag
uint8_t errorCode;

//DEFAULT SEND 0's
for(uint8_t j=0; j<4;j++){
  accel[j]=0;
}

for(uint8_t k=0; k<4;k++){
  rot[k]=0;
}

//Monitor MPU
errorCode=mpuMonitor(currAccelX,currAccelY,currAccelZ);

//IF PROBLEM, SEND PREVIOUS VALUES
    if(errorCode!=0){
       accel[0]=*currAccelX;
       accel[1]=*currAccelY;
       accel[2]=*currAccelZ;

       rot[0]=*currYaw;
       rot[1]=*currPitch;
       rot[2]=*currRoll;
    }

//IF GOT VALUES, SUPPLY NEW VALUES
    else if(errorCode==0){

  // display raw acceleration values
    // Chosen getAcceleration method because only one showing approximately independent dimensions
    // ALL others having problems; linearAccelInWorld in particular only giving zeroes, and linearAccel giving all ~same
    #ifdef MPU_RAW_ACCEL
    mpu.getAcceleration(accel,accel+1,accel+2);
    accel[0]=(accel[0] - *accelOffsetX); // Components with offset applied
    accel[1]=(accel[1] - *accelOffsetY); // No scaling factor 2048, to maximize resolution in integer rep.
    accel[2]=(accel[2] - *accelOffsetZ);
    #endif

  // TO DO: WILL DETERMINE AXIS OF MAX DIFFERENCE BETWEEN accel AND currAccel VALUES...
  // AND DIVIDE OTHER AXES' VALUES BY DERIVATIVE (USING TIMER COUNT)
  
  // -OR- TAKE TIME AVERAGE OF BUFFERED VALUES AND DIVIDE OTHERS BY AXIS OF MAX RATE OF CHANGE
  
  // -OR- MAX/MIN HOLD VALUES ON AXIS AND FIND RATE OF CHANGE OF PEAKS. DIVIDE OTHER AXES BY MAX RATE OF CHANGE
  
  
    // display raw gyroscope values
    //Took raw values because all others not independent (and dmpGetGyro in particular not sensitive)
    #ifdef MPU_RAW_GYRO 
    mpu.getRotation(rot,rot+1,rot+2);
    /* rot[0]=(rot[0]/16.4); // No scaling of rotation, so as maximize resolution with integer
    rot[1]=(rot[1]/16.4);
    rot[2]=(rot[2]/16.4); */

      //Return absolute angle values, if desired
      /* #ifdef ABSOLUTE_ANGLE
      //Apply complementary filter to get
      Complementary_Filter(accel[0],accel[1],accel[2],rot[0],rot[1],rot[2]);
      #endif */
    
    #endif

    }


//Print out and update "previous" values to current ones
#ifdef MPU_RAW_ACCEL

/*  FOR DEBUG, SERIAL PRINT
  #ifdef accelAsComponents
    Serial.print(accel[0] + ", " + accel[1] + ", " + accel[2]));
  #endif
  #ifdef accelAsMag
        Serial.print(sqrt((abs(accel[0]))^2+(abs(accel[1]))^2+(abs(accel[2]))^2));
  #endif
*/

  *currAccelX=accel[0];
  *currAccelY=accel[1];
  *currAccelZ=accel[2];
#endif


#ifdef MPU_RAW_GYRO
  *currYaw=rot[0];
  *currPitch=rot[1];
  *currRoll=rot[2];
#endif


/* #ifdef GET_TAPS
    if(&(devAddr+TAPXYZ)){ //If tap received register goes high, send 1 tap indication
      Serial.print(", 1");
    }
//.........这里部分代码省略.........

void system_init()
{
    DEBUG_PRINTF(V_MESSAGE, "Initializing WirePi.\n");
    wiringPiSetup() ;
    pinMode(LED, OUTPUT);
    LED_ON;     // Turn red led ON on initialization.

    DEBUG_PRINTF(V_MESSAGE, "Initializing timers.\n");
    init_photo_timer();

    if((operation_type == OPERATION_A)
    || (operation_type == OPERATION_B))
    {
        init_accelgyro_timer();
    }
    init_operation_timer();
    init_led_timer();

    DEBUG_PRINTF(V_MESSAGE, "Initializing accelgyro.\n");
    accelgyro.initialize();
    
    if(operation_type == OPERATION_A) // PLAN A
    {
        accelgyro_handler = &accelgyro_handler_planA;
        accelgyro.getAcceleration(&x, &y,&z);
        accel_low.x = accel_low.x * gravity_alpha + x * (1 - gravity_alpha);
        accel_low.y = accel_low.y * gravity_alpha + y * (1 - gravity_alpha);
        accel_low.z = accel_low.z * gravity_alpha + z * (1 - gravity_alpha);
        last_angle = accel_low.getAngleTo(gravity);
        max_angle = last_angle;
        start_accelgyro_timer(0, ACCELGYRO_TICK);
    }
    else if(operation_type == OPERATION_B) // PLAN B
    {
        accelgyro_handler = &accelgyro_handler_planB;
        start_accelgyro_timer(0, ACCELGYRO_TICK);
    }

    DEBUG_PRINTF(V_MESSAGE, "Initializing TCP socket.\n");
    socket_tcp = TCP_Socket(host_ip, socket_port);

    DEBUG_PRINTF(V_MESSAGE, "Starting Timers.\n");
    start_led_timer(TIMER_LED_TICK, 0);

    DEBUG_PRINTF(V_MESSAGE, "Initialization sleep!\n");
    sleep(INITIAL_DELAY);

    DEBUG_PRINTF(V_MESSAGE, "System initialized!\n");

    if((operation_test == TEST_NO_TCP) || (operation_test == FULL_TEST))
    {
        DEBUG_PRINTF(V_MESSAGE, "System test!\n");
        if(system_test() == false)
        {
            signalize(ERROR);
            DEBUG_PRINTF(V_ERROR, "Failed on System Test!\n");
            while(1);
        }
        else
        {
            DEBUG_PRINTF(V_MESSAGE, "Passed on system test!\n");
        }
    }
    return;
}

uint8_t mpuMonitor(int16_t *currAccelX,int16_t *currAccelY,int16_t *currAccelZ){

uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint16_t fifoCount;     // count of all bytes currently in FIFO
/*MUST DECIDE WHETHER TO USE*/
/*ERROR CODES:
1: DMP not ready
2: No interrupt received
3: FIFO OFLOW
4: Other (unknown)
*/
uint8_t monitorErrorCode=0;


  
  //PROGRAMMING FAILURE CHECK
  if (!dmpReady){
    monitorErrorCode=1;
    return monitorErrorCode;
  }
  
  //NO-INTERRUPT CHECK
  // If fails, must wait for MPU interrupt or extra packet(s) to become available
  // Also catches if interrupt line disconnected, or other hardware issues (e.g. power loss)
  if(!mpuInterrupt && (fifoCount < packetSize)){
    monitorErrorCode=2;
    return monitorErrorCode;
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // FIFO OVERFLOW CHECK
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // mpu FIFO OFLOW flag is raised or fifoCount has max of 1024 (max # of bytes in buffer)
   monitorErrorCode=3; 
  // reset so we can continue cleanly
    mpu.resetFIFO();
    return monitorErrorCode;
  }
  
  
  // GOT MPU DATA READY INTERRUPT WITH SUFFICIENT SIZE!
  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;
  
  //mpuMONITOR gets values of acceleration, too...
  //to provide for offset calculation in calibration (redundancy - to improve)
  mpu.getAcceleration(currAccelX,currAccelY,currAccelZ);

    return monitorErrorCode;
  }

  //Unknown error
  monitorErrorCode=4;
  return monitorErrorCode;
}