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

int main(int argc, char const *argv[])
{
    AccelVector gravity;
    MPU6050 accelgyro;
    accelgyro.initialize();
    accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    int16_t 
        x=-3940,
        y=-0110,
        z=00326;
    int16_t 
        gx,
        gy,
        gz;
    // accelgyro.getAcceleration(&x, &y,&z);
    gravity.x = x; gravity.y = y; gravity.z = z;
    float alpha = 0.5f, gyralpha = 0.9f;
    // for(int i=0; i<100; i++){
    //     accelgyro.getAcceleration(&x, &y,&z);
    //     gravity.x = gravity.x * alpha + x * (1 - alpha);
    //     gravity.y = gravity.y * alpha + y * (1 - alpha);
    //     gravity.z = gravity.z * alpha + z * (1 - alpha);
    //     usleep(50000);
    // }
    AccelVector vec, gyr;

        accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz);
    gyr.x = gx; gyr.y = gy; gyr.z = gz;
    while(1)
    {
        accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz);
        //accelgyro.getAcceleration(&x, &y,&z,);
        vec.x = vec.x * alpha + x * (1 - alpha);
        vec.y = vec.y * alpha + y * (1 - alpha);
        vec.z = vec.z * alpha + z * (1 - alpha);

        gyr.x = gyr.x * gyralpha + gx * (1 - gyralpha);
        gyr.y = gyr.y * gyralpha + gy * (1 - gyralpha);
        gyr.z = gyr.z * gyralpha + gz * (1 - gyralpha);
        
        printf("Total gforce now %.3f g's gyro len %.3f | Now: %05.0f %05.0f %05.0f Gyro: %05.0f %05.0f %05.0f\n", 
            vec.getMagnitude()/16000, gyr.getMagnitude(), vec.x, vec.y, vec.z, gyr.x, gyr.y, gyr.z);

        usleep(50000);
    }

        // MPU control/status vars
	/*bool dmpReady = false;  // set true if DMP init was successful
	uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
	uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
	uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
	uint16_t fifoCount;     // count of all bytes currently in FIFO
	uint8_t fifoBuffer[64]; // FIFO storage buffer

	// orientation/motion vars
	Quaternion q;           // [w, x, y, z]         quaternion container
	VectorInt16 aa;         // [x, y, z]            accel sensor measurements
	VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
	VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
	VectorFloat gravity;    // [x, y, z]            gravity vector
	float euler[3];         // [psi, theta, phi]    Euler angle container
	float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
	MPU6050 accelgyro;
    accelgyro.initialize();
    // load and configure the DMP
    printf("Initializing DMP...\n");
    devStatus = accelgyro.dmpInitialize();
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        printf("Enabling DMP...\n");
        accelgyro.setDMPEnabled(true);

        // enable Arduino interrupt detection
        //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = accelgyro.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        printf("DMP ready!\n");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = accelgyro.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        printf("DMP Initialization failed (code %d)\n", devStatus);
        return 0;
    }

    while(1){
        fifoCount = accelgyro.getFIFOCount();

        if (fifoCount == 1024) {
            // reset so we can continue cleanly
            accelgyro.resetFIFO();
//.........这里部分代码省略.........

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

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