/* * Updates function * */ extern "C" void sf_MPU6050_2xDriver_GxAyz_Update_wrapper(const int16_T *x_vel, const int16_T *y_acc, const int16_T *z_acc, const int16_T *x_vel_2, const int16_T *y_acc_2, const int16_T *z_acc_2, real_T *xD) { /* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */ if(xD[0] != 1){ # ifndef MATLAB_MEX_FILE Wire.begin(); accelgyro.initialize(); accelgyro.setDLPFMode(MPU6050_DLPF_BW_42); accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); accelgyro2.initialize(); accelgyro2.setDLPFMode(MPU6050_DLPF_BW_42); accelgyro2.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); #endif //done with initialization xD[0] = 1; } /* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */ }
int main() { MPU6050 *mpu = new MPU6050(); mpu->setDebug(true); mpu->reset(); if (mpu->whoAmI()) { printf("WhoAmI was okay\n"); // i2c bypass enabled mpu->setBypassEnable(true); printf("Set and Get BypassEnable to true - %s\n", mpu->getBypassEnable() ? "SUCCESS" : "FAILED"); mpu->setBypassEnable(false); printf("Set and Get BypassEnable to false - %s\n", !mpu->getBypassEnable() ? "SUCCESS" : "FAILED"); // gyro ranges mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_250DEG_S); printf("Set and Get FullScaleGyroRange to 250deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_250DEG_S) ? "SUCCESS" : "FAILED"); mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_500DEG_S); printf("Set and Get FullScaleGyroRange to 500deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_500DEG_S) ? "SUCCESS" : "FAILED"); mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_1000DEG_S); printf("Set and Get FullScaleGyroRange to 1000deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_1000DEG_S) ? "SUCCESS" : "FAILED"); mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_2000DEG_S); printf("Set and Get FullScaleGyroRange to 2000deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_2000DEG_S) ? "SUCCESS" : "FAILED"); // accelerometer ranges mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_2G); printf("Set and Get FullScaleAccRange to 2G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_2G) ? "SUCCESS" : "FAILED"); mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_4G); printf("Set and Get FullScaleAccRange to 4G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_4G) ? "SUCCESS" : "FAILED"); mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_8G); printf("Set and Get FullScaleAccRange to 8G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_8G) ? "SUCCESS" : "FAILED"); mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_16G); printf("Set and Get FullScaleAccRange to 16G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_16G) ? "SUCCESS" : "FAILED"); return 1; } return 0; }
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; } }
void initialize_imu() { // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); // ************************************************************** // It is best to configure I2C to 400 kHz. // If you are using an Arduino DUE, modify the variable TWI_CLOCK to 400000, defined in the file: // c:/Program Files/Arduino/hardware/arduino/sam/libraries/Wire/Wire.h // If you are using any other Arduino instead of the DUE, uncomment the following line: //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) //This line should be commented if you are using Arduino DUE // ************************************************************** #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialize serial communication Serial.begin(250000); // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // TODO: Compute these parameters // mpu.setXAccelOffset(-1600); // mpu.setYAccelOffset(-180); // mpu.setZAccelOffset(650); // mpu.setXGyroOffset(0); // mpu.setYGyroOffset(0); // mpu.setZGyroOffset(0); mpu.setFullScaleGyroRange(0); calibrate_imu(); // Magnetometer configuration mpu.setI2CMasterModeEnabled(0); mpu.setI2CBypassEnabled(1); Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS); Wire.write(0x02); Wire.write(0x00); // Set continuous mode Wire.endTransmission(); delay(5); Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS); Wire.write(0x00); Wire.write(B00011000); // 75Hz Wire.endTransmission(); delay(5); mpu.setI2CBypassEnabled(0); // X axis word mpu.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction mpu.setSlaveRegister(0, HMC5883L_RA_DATAX_H); mpu.setSlaveEnabled(0, true); mpu.setSlaveWordByteSwap(0, false); mpu.setSlaveWriteMode(0, false); mpu.setSlaveWordGroupOffset(0, false); mpu.setSlaveDataLength(0, 2); // Y axis word mpu.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80); mpu.setSlaveRegister(1, HMC5883L_RA_DATAY_H); mpu.setSlaveEnabled(1, true); mpu.setSlaveWordByteSwap(1, false); mpu.setSlaveWriteMode(1, false); mpu.setSlaveWordGroupOffset(1, false); mpu.setSlaveDataLength(1, 2); // Z axis word mpu.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80); mpu.setSlaveRegister(2, HMC5883L_RA_DATAZ_H); mpu.setSlaveEnabled(2, true); mpu.setSlaveWordByteSwap(2, false); mpu.setSlaveWriteMode(2, false); mpu.setSlaveWordGroupOffset(2, false); mpu.setSlaveDataLength(2, 2); mpu.setI2CMasterModeEnabled(1); mpu.setDLPFMode(6); }
//PROGRAM FUNCTIONS void setup_mpu6050(){ clear_i2c(); Wire.begin(); SERIAL_OUT.println("Initializing gyro..."); accelgyro.initialize(); //accelgyro.reset(); accelgyro.setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! // verify connection SERIAL_OUT.println("Testing device connections..."); SERIAL_OUT.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); SERIAL_OUT.println(F("Setting clock source to Z Gyro...")); accelgyro.setClockSource(MPU6050_CLOCK_PLL_ZGYRO); //SERIAL_OUT.println(accelgyro.getClockSource(MPU6050_CLOCK_PLL_ZGYRO); SERIAL_OUT.println(F("Setting sample rate to 200Hz...")); accelgyro.setRate(0); // 1khz / (1 + 4) = 200 Hz // * | ACCELEROMETER | GYROSCOPE // * DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate // * ---------+-----------+--------+-----------+--------+------------- // * 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz // * 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz // * 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz // * 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz // * 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz // * 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz // * 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz // * 7 | -- Reserved -- | -- Reserved -- | Reserved SERIAL_OUT.println(F("Setting DLPF bandwidth")); accelgyro.setDLPFMode(MPU6050_DLPF_BW_42); SERIAL_OUT.println(F("Setting gyro sensitivity to +/- 250 deg/sec...")); accelgyro.setFullScaleGyroRange(0); //accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //accelgyro.setFullScaleGyroRange(0); // 0=250, 1=500, 2=1000, 3=2000 deg/sec //SERIAL_OUT.println(F("Resetting FIFO...")); //accelgyro.resetFIFO(); // use the code below to change accel/gyro offset values accelgyro.setXGyroOffset(XGYROOFFSET); accelgyro.setYGyroOffset(YGYROOFFSET); accelgyro.setZGyroOffset(ZGYROOFFSET); SERIAL_OUT.print(accelgyro.getXAccelOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getYAccelOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getZAccelOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getXGyroOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getYGyroOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getZGyroOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print("\n"); SERIAL_OUT.println(F("Enabling FIFO...")); accelgyro.setFIFOEnabled(true); accelgyro.setZGyroFIFOEnabled(true); accelgyro.setXGyroFIFOEnabled(false); accelgyro.setYGyroFIFOEnabled(false); accelgyro.setAccelFIFOEnabled(false); SERIAL_OUT.print("Z axis enabled?\t"); SERIAL_OUT.println(accelgyro.getZGyroFIFOEnabled()); SERIAL_OUT.print("x axis enabled?\t"); SERIAL_OUT.println(accelgyro.getXGyroFIFOEnabled()); SERIAL_OUT.print("y axis enabled?\t"); SERIAL_OUT.println(accelgyro.getYGyroFIFOEnabled()); SERIAL_OUT.print("accel enabled?\t"); SERIAL_OUT.println(accelgyro.getAccelFIFOEnabled()); accelgyro.resetFIFO(); return ; }
// // main task // int main(void) { #ifdef DEBUG #if __USE_USB usbCDC ser; ser.connect(); #else CSerial ser; ser.settings(115200); #endif CDebug dbg(ser); dbg.start(); #endif /************************************************************************* * * your setup code here * **************************************************************************/ // // Load Configuration // EEPROM::read(0, &config, sizeof(config)); if ( config.length!=sizeof(config) ) { setDefault(); } // class default I2C address is 0x68 // specific I2C addresses may be passed as a parameter here // AD0 low = 0x68 (default for InvenSense evaluation board) // AD0 high = 0x69 MPU6050 mpu; // initialize device mpu.initialize(); mpu.setRate(7); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // // check device // if (mpu.testConnection()) { } // // H-Bridge // CPwm::frequency(KHZ(20)); HBridge left(PWM1, P18, P19); HBridge right(PWM2, P22, P23); left.enable(); right.enable(); BalanceRobot robot(mpu, left, right); robot.start("Robot", 168, PRI_HIGH); #ifndef DEBUG myMenu menu(mpu, robot); menu.start(); #endif while (1) { /********************************************************************** * * your loop code here * **********************************************************************/ LEDs[0] = !LEDs[0]; sleep(500); } return 0;