/*
* 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;
