void setup() {
Wire.begin();
Serial.begin(57600);
I2Cdev::writeByte(0x68, 0x6B, 0x01); //PWR_MGMT1 for clock source as X-gyro
uint8_t temp[8] = {8, 0};//GYRO range:250, ACCEL range:2g | Refer the Datasheet if you want to change these.
I2Cdev::writeBytes(0x68, 0x1B, 2, temp); //GYRO_CFG and ACCEL_CFG
accelgyro.setRate(4);
accelgyro.setDLPFMode(0x03);
accelgyro.setFIFOEnabled(true);
I2Cdev::writeByte(0x68, 0x23, 0x78); //FIFO_EN for ACCEL,GYRO
accelgyro.setDMPEnabled(false);
I2Cdev::writeByte(0x68, 0x38, 0x11); //INT_EN
attachInterrupt(0, mpu_interrupt, RISING);
attachInterrupt(1, compass_interrupt, FALLING);
//Put the HMC5883 IC into the correct operating mode
Wire.beginTransmission(0x1E); //open communication with HMC5883
Wire.write(0x00); //select Config_Register_A:
Wire.write(0x58); //4-point avg. and 75Hz rate
Wire.write(0x02); //In Config_Register_B: +-1.9G (820LSB/G)
Wire.write(0x00); //In Mode_Register: continuous measurement mode
Wire.endTransmission();
pinMode(13, INPUT);
#ifdef CAL_DEBUG
Serial.print("Calibrating Gyros and Accel! Hold Still and Level!");
#endif
calibrate_gyros();
calibrate_accel();
accelgyro.resetFIFO();
}
//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;
