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

/*
  * 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){
//.........这里部分代码省略.........

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;