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 }
int main() { // MPU6050 object "MPU6050.h" MPU6050 accelgyro; // Kalman filter Object "Kalman.h" Kalman kalman; // Xbee serial baudrate xBee.baud(38400); pc.baud(9600); // Initialize timers Timer timer, code, encTimer; // Start timers timer.start(); code.start(); encTimer.start(); // Reset timer values timer.reset(); code.reset(); encTimer.reset(); //Encoder 1 interrupts enc1a.rise(&incrementEnc1a); enc1b.rise(&incrementEnc1b); enc1a.fall(&setEnc1aFall); enc1b.fall(&setEnc1bFall); enc2a.rise(&incrementEnc2a); enc2b.rise(&incrementEnc2b); enc2a.fall(&setEnc2aFall); enc2b.fall(&setEnc2bFall); // Debug leds myled1 = 0; myled2 = 0; myled3 = 0; // Encoder 1 initializations newTime =0; oldTime = 0; lastEncTime = 0; enc1Speed = 0; enc2Speed = 0; // MPU6050 initializations accelgyro.initialize(); accelgyro.setFullScaleGyroRange(0); accelgyro.setFullScaleAccelRange(0); // Initialize MotorShield object shield.init(); float measuredTheta , measuredRate, newTheta,calcRate; float position= 0, velocity = 0, lastPosition = 0, lastVelocity =0; float angleOffset = 0, positionOffset = 0,lastAngleoffset = 0; // Position control constants, if necessary float zoneA = 16000.0f, zoneB = 8000.0f, zoneC = 2000.0f; float multiplier = 1.0f; float zoneAscale = 600*2*multiplier, zoneBscale = 800*2*multiplier, zoneCscale = 1000*2*multiplier, zoneDscale = 1500*2*multiplier, velocityScale = 60*2*multiplier; // Serial communication buffer char buffer[40]; int i, strSize; // Control parameters float k0,k1,k2,k3,tref; float x, dotx, deltaX, deltaT, lastX = 0; // Helper variables float waittime , u, diff, dt; float error = 0,lastError = 0; int counter = 0; float pTerm = 0,dTerm = 0,iTerm = 0; while(1) { /////////////////////////////////////////// // Read serial data and update constants // /////////////////////////////////////////// i = 0; char a; while(pc.readable()){ a = pc.getc(); buffer[i++] = a; printf("%c\n", a ); myled1 = 1; wait(0.001); } strSize = i; string receive(buffer,strSize); // Changes from char to string if(strSize > 0){ printf("%s\n", receive); assignGainsFromString(&k[0], &k[1], &k[2], &k[3], &k[4],receive); } // Below is an attempt to control the robot position, // by dividing it into "zones" and changing the angle accordingly. // ///////////////////////////// // Generate encoder offset // ///////////////////////////// // position = (float)enc2total; // positionOffset = position; // //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{ // Define error term error = newTheta - tref; // Proportional term pTerm = kp*error; //Integral term iTerm += ki*error*dt*100.0f; // Prevent integration windup: if(iTerm >= 100) iTerm = 100; else if (iTerm <= -100) iTerm = -100; u = -(iTerm + pTerm); // Calculated derivative based on smoothened angle. // There are two other sources for the angle here: kalman.getRate(); and measuredRate. calcRate = -(error-lastError)/dt; // Derivative term if(kd != 0) dTerm = kd*calcRate/100.0f; lastError = error; u += dTerm; // Correct for dead zone --- Did not have successful results but I'll leave it here. // int deadzone = 20; // if(u < -deadzone) u = u + deadzone; // else if(u > deadzone) u = u - deadzone; // else u = 0; // // Include saturation u = constrain(u,-400,400); // Flash LED to indicate loop if(counter % 50 == 0){ myled3 = !myled3; counter = 0; } counter++; } lastError = error; // update angle if(counter % 50 == 0){ myled3 = !myled3; // xBee.printf("%f,%f\n", newTheta,u); counter = 0; } // Set motor speed shield.setM1Speed(-(int)u); shield.setM2Speed((int)u); // DEBUG over serial port. Use with MATLAB program "serialPlot.m" to plot these results. printf("%f,%f,%f\n", measuredTheta, newTheta , u); // Hold the timer until desired sampling time is reached. while(timer.read_us() < waittime*1000); // Turn off serial transmission LED myled1 = 0; } }
// 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"); } #ifdef MANUAL_TAP else if(gotTaps(rot[0],rot[1],rot[2],sFIFO_Y,sFIFO_P,sFIFO_R)){ //Same from gotTap() function, if returns true Serial.print(", 1"); } #endif #endif */ //Return error code, to indicate type of problem if any return errorCode; }
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; }