自学教程：C++ AHRS类代码示例

int main(int argc, char *argv[]){    QApplication a(argc, argv);    AHRS w;    w.show();        return a.exec();}

int main(int argc, char *argv[]){    //-------------------- IMU setup and main loop ----------------------------    imuSetup();    ros::init(argc, argv, "ros_erle_imu_euler");        ros::NodeHandle n;    ros::Publisher imu_euler_pub = n.advertise<ros_erle_imu_euler::euler>("euler", 1000);    ros::Rate loop_rate(50);    while (ros::ok()){        //----------------------- Calculate delta time ----------------------------        gettimeofday(&tv,NULL);        previoustime = currenttime;        currenttime = 1000000 * tv.tv_sec + tv.tv_usec;        dt = (currenttime - previoustime) / 1000000.0;        if(dt < 1/1300.0)             usleep((1/1300.0-dt)*1000000);        gettimeofday(&tv,NULL);        currenttime = 1000000 * tv.tv_sec + tv.tv_usec;        dt = (currenttime - previoustime) / 1000000.0;        //-------- Read raw measurements from the MPU and update AHRS --------------        // Accel + gyro.        imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);        ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);        //------------------------ Read Euler angles ------------------------------        ahrs.getEuler(&roll, &pitch, &yaw);        ros_erle_imu_euler::euler msg;        msg.roll = roll;        msg.pitch = pitch;        msg.yaw = yaw;        imu_euler_pub.publish(msg);        ros::spinOnce();        loop_rate.sleep();    }    return 0;}

int main(int argc, char *argv[]){    if (argc == 3) {        try {            AHRS *imu = new AHRS(std::string(argv[1]),getBaudrate(argv[2]));            char *buffer = NULL;            size_t bufsize;            // Connect to the AHRS.            imu->openDevice();            printf("Connected to %s./n", (imu->getInfo()).c_str());            // Prompt the user for a new baudrate.            printf("New speed? (baud) /n");            if (getline(&buffer, &bufsize, stdin) == -1)            {                fprintf(stderr, "Unable to read in new baudrate./n");                imu->closeDevice();                return -1;            }            // Attempt to read in a baudrate and change to it.            imu->setBaudrate(getBaudrate(buffer));            // Clean up and close resources.            free(buffer);            imu->closeDevice();            // Tell the user of the result.            fprintf(stderr, "Changed speed successfully./n");            exit(0);        } catch (std::exception& e) {            printError(e);        }    }    printUsage();}

	void RobotInit() override {		lw = LiveWindow::GetInstance();		drive->SetExpiration(20000);		drive->SetSafetyEnabled(false);		//Gyroscope stuff		try {			/* Communicate w/navX-MXP via the MXP SPI Bus.                                       */			/* Alternatively:  I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */			/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details.   */			ahrs = new AHRS(SPI::Port::kMXP);		} catch (std::exception ex) {			std::string err_string = "Error instantiating navX-MXP:  ";			err_string += ex.what();			//DriverStation::ReportError(err_string.c_str());		}		if (ahrs) {			LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);			ahrs->ZeroYaw();			// Kp	  Ki	 Kd		Kf    PIDSource PIDoutput			turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,					ahrs, this);			turnController->SetInputRange(-180.0f, 180.0f);			turnController->SetOutputRange(-1.0, 1.0);			turnController->SetAbsoluteTolerance(2); //tolerance in degrees			turnController->SetContinuous(true);		}		chooser.AddDefault("No Auto", new int(0));		chooser.AddObject("GyroTest Auto", new int(1));		chooser.AddObject("Spy Auto", new int(2));		chooser.AddObject("Low Bar Auto", new int(3));		chooser.AddObject("Straight Spy Auto", new int(4));		chooser.AddObject("Adjustable Straight Auto", new int(5));		SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);		SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);		SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);		SmartDashboard::PutData("Auto Modes", &chooser);		liftdown->Set(false);		comp->Start();	}

void imuLoop(){    //----------------------- Calculate delta time ----------------------------	gettimeofday(&tv,NULL);	previoustime = currenttime;	currenttime = 1000000 * tv.tv_sec + tv.tv_usec;	dt = (currenttime - previoustime) / 1000000.0;	if(dt < 1/1300.0) usleep((1/1300.0-dt)*1000000);        gettimeofday(&tv,NULL);        currenttime = 1000000 * tv.tv_sec + tv.tv_usec;	dt = (currenttime - previoustime) / 1000000.0;    //-------- Read raw measurements from the MPU and update AHRS --------------    // Accel + gyro.	/*    imu->update();    imu->read_accelerometer(&ax, &ay, &az);    imu->read_gyroscope(&gx, &gy, &gz);    ax /= G_SI;    ay /= G_SI;    az /= G_SI;    gx *= 180 / PI;    gy *= 180 / PI;    gz *= 180 / PI;    ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);    	*/    // Accel + gyro + mag.    // Soft and hard iron calibration required for proper function.        imu->update();    imu->read_accelerometer(&ay, &ax, &az);    imu->read_gyroscope(&gy, &gx, &gz);    imu->read_magnetometer(&my, &mx, &mz);    ax /= -G_SI;    ay /= -G_SI;    az /= G_SI;    gx *= 180 / PI;    gy *= 180 / PI;    gz *= -180 / PI;    ahrs.update(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, my, mx, -mz, dt);        //------------------------ Read Euler angles ------------------------------    //ahrs.getEuler(&pitch, &roll, &yaw);//roll and pitch inverted     ahrs.getEuler(&roll, &pitch, &yaw);    //------------------- Discard the time of the first cycle -----------------    if (!isFirst)    {    	if (dt > maxdt) maxdt = dt;    	if (dt < mindt) mindt = dt;    }    isFirst = 0;}

void imuLoop(){    float dtsum = 0.0f; //sum of delta t's    while(dtsum < 1.0f/freq) //run this loop at 1300 Hz (Max frequency gives best results for Mahony filter)    {		//----------------------- Calculate delta time ----------------------------		gettimeofday(&tv,NULL);		previoustime = currenttime;		currenttime = 1000000 * tv.tv_sec + tv.tv_usec;		dt = (currenttime - previoustime) / 1000000.0;		if(dt < 1/1300.0) usleep((1/1300.0-dt)*1000000);		gettimeofday(&tv,NULL);		currenttime = 1000000 * tv.tv_sec + tv.tv_usec;		dt = (currenttime - previoustime) / 1000000.0;	    //-------- Read raw measurements from the MPU and update AHRS --------------	    // Accel + gyro.		    imu->update();	    imu->read_accelerometer(&ax, &ay, &az);	    imu->read_gyroscope(&gx, &gy, &gz);	    ax /= G_SI;	    ay /= G_SI;	    az /= G_SI;	    gx *= 180 / PI;	    gy *= 180 / PI;	    gz *= 180 / PI;	    ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);	    		    // Accel + gyro + mag.	    // Soft and hard iron calibration required for proper function.	    /*	    imu->update();	    imu->read_accelerometer(&ay, &ax, &az);		az *= -1;	    imu->read_gyroscope(&gy, &gx, &gz);		gz *= -1;	    imu->read_magnetometer(&mx, &my, &mz);	    ax /= G_SI;	    ay /= G_SI;	    az /= G_SI;	    gx *= 180 / PI;	    gy *= 180 / PI;	    gz *= 180 / PI;	   ahrs.update(-ax, -ay, -az, gx*0.0175, gy*0.0175, gz*0.0175, mx, my, mz, dt);	    */	    //------------------------ Read Euler angles ------------------------------	    ahrs.getEuler(&pitch, &roll, &yaw);	    //------------------- Discard the time of the first cycle -----------------	    if (!isFirst)	    {	    	if (dt > maxdt) maxdt = dt;	    	if (dt < mindt) mindt = dt;	    }	    isFirst = 0;		dtsum += dt;    }    }

    void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update) {        /* Update base IMU class variables */        ahrs->yaw                    = ahrs_update.yaw;        ahrs->pitch                  = ahrs_update.pitch;        ahrs->roll                   = ahrs_update.roll;        ahrs->compass_heading        = ahrs_update.compass_heading;        ahrs->yaw_offset_tracker->UpdateHistory(ahrs_update.yaw);        /* Update AHRS class variables */        // 9-axis data        ahrs->fused_heading          = ahrs_update.fused_heading;        // Gravity-corrected linear acceleration (world-frame)        ahrs->world_linear_accel_x   = ahrs_update.linear_accel_x;        ahrs->world_linear_accel_y   = ahrs_update.linear_accel_y;        ahrs->world_linear_accel_z   = ahrs_update.linear_accel_z;        // Gyro/Accelerometer Die Temperature        ahrs->mpu_temp_c             = ahrs_update.mpu_temp;        // Barometric Pressure/Altitude        ahrs->altitude               = ahrs_update.altitude;        ahrs->baro_pressure          = ahrs_update.barometric_pressure;        // Status/Motion Detection        ahrs->is_moving              =                (((ahrs_update.sensor_status &                        NAVX_SENSOR_STATUS_MOVING) != 0)                        ? true : false);        ahrs->is_rotating                =                (((ahrs_update.sensor_status &                        NAVX_SENSOR_STATUS_YAW_STABLE) != 0)                        ? false : true);        ahrs->altitude_valid             =                (((ahrs_update.sensor_status &                        NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)                        ? true : false);        ahrs->is_magnetometer_calibrated =                (((ahrs_update.cal_status &                        NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)                        ? true : false);        ahrs->magnetic_disturbance       =                (((ahrs_update.sensor_status &                        NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)                        ? true : false);        ahrs->quaternionW                = ahrs_update.quat_w;        ahrs->quaternionX                = ahrs_update.quat_x;        ahrs->quaternionY                = ahrs_update.quat_y;        ahrs->quaternionZ                = ahrs_update.quat_z;        ahrs->velocity[0]     = ahrs_update.vel_x;        ahrs->velocity[1]     = ahrs_update.vel_y;        ahrs->velocity[2]     = ahrs_update.vel_z;        ahrs->displacement[0] = ahrs_update.disp_x;        ahrs->displacement[1] = ahrs_update.disp_y;        ahrs->displacement[2] = ahrs_update.disp_z;        ahrs->yaw_angle_tracker->NextAngle(ahrs->GetYaw());    }

void imuLoop ( void ) {	//Calculate delta time	gettimeofday( &tv , NULL );	previoustime = currenttime;	currenttime = 1000000 * tv.tv_sec + tv.tv_usec;	dt = ( currenttime - previoustime ) / 1000000.0;	if( dt < 1/1300.0 )  usleep( ( 1/1300.0 - dt ) * 1000000);	gettimeofday( &tv , NULL) ;	currenttime = 1000000 * tv.tv_sec + tv.tv_usec;	dt = ( currenttime - previoustime ) / 1000000.0;	//Read raw measurements from the MPU and update AHRS	// Accel + gyro + mag.	// Soft and hard iron calibration required for proper function.	imu -> update();	imu -> read_accelerometer( &ay , &ax , &az );	imu -> read_gyroscope( &gy , &gx , &gz );	imu -> read_magnetometer( &mx , &my , &mz );	ax /= G_SI;	ay /= G_SI;	az /= G_SI;	my = - my;	ahrs.update( ax , ay , az , gx , gy , gz , mx , my , mz , dt );	//Read Euler angles	ahrs.getEuler( &pitch , &roll , &yaw );	//Discard the time of the first cycle	if ( !isFirst )	{		if ( dt > maxdt ) maxdt = dt;		if ( dt < mindt ) mindt = dt;	}	isFirst = 0;	//Console and network output with a lowered rate	dtsumm += dt;	if( dtsumm > 0.05 ) {		//Console output		//printf("ROLL: %+05.2f PITCH: %+05.2f YAW: %+05.2f PERIOD %.4fs RATE %dHz /n", roll, pitch, gz/*yaw * -1*/, dt, int(1/dt));		// Network output		//sprintf( sendline , "%10f %10f %10f %10f %dHz/n" ,ahrs.getW() ,ahrs.getX() ,ahrs.getY() ,ahrs.getZ() ,int( 1/dt ) );		//sendto( sockfd , sendline , strlen( sendline ) , 0 ,  ( struct sockaddr * )&servaddr , sizeof( servaddr ) );		dtsumm = 0;	}}

int main(int argc, char *argv[]){   if (argc == 3) {      try {         AHRS *ahrs = new AHRS(std::string(argv[1]),getBaudrate(argv[2]));         AHRS::AcqConfig config;         AHRS::FilterData filters;         long unsigned int i;         ahrs->openDevice();         printf("Connected to %s./n", (ahrs->getInfo()).c_str());         ahrs->sendAHRSDataFormat();         printf("AHRS Configuration data:/n");         config = ahrs->getAcqConfig();         printf("Acqusition mode:   %d/n", config.poll_mode);         printf("Flush Filter:      %d/n", config.flush_filter);         printf("Sample delay:      %f/n", config.sample_delay);         filters = ahrs->getFIRFilters();         printf("Number of filters: %lu/n", filters.size());         for (i=0; i < filters.size(); i++)         printf("Filter #%.2lu:        %f/n",i, filters[i]);         printf("Mag Truth Method:  %d/n", ahrs->getMagTruthMethod());         printf("Functional mode:   %d/n", ahrs->getAHRSMode());         printf("Declination:       %f/n", ahrs->getDeclination());         printf("UserCalNumPoints:  %d/n", ahrs->getCalPoints());         printf("Mag Setting:       %d/n", ahrs->getMagCalID());         printf("Accel setting:     %d/n", ahrs->getAccelCalID());         printf("Mounting mode:     %d/n", ahrs->getMounting());         printf("Baudrate:          %d/n", ahrs->getBaudrate().baud);         printf("True north?        %d/n", ahrs->getTrueNorth());         printf("Big endian?        %d/n", ahrs->getBigEndian());         printf("Auto sampling?     %d/n", ahrs->getAutoCalibration());         printf("Mils/Degrees?      %d/n", ahrs->getMils());         printf("HPR During Cal?    %d/n", ahrs->getHPRCal());         ahrs->closeDevice();         exit(0);      }      catch (std::exception& e)      {         printError(e);      }   }    printUsage();}

	void UpdateDashboard() {		float r = 0.00001 * i;		SmartDashboard::PutNumber("State", currentState + r);		SmartDashboard::PutNumber("PID Turn Error",				turnController->GetError() + r);		SmartDashboard::PutNumber("PID Target",				turnController->GetSetpoint() + r);//		SmartDashboard::PutBoolean("Straight", straight);		SmartDashboard::PutData("test", turnController);		SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r);		SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r);		SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r);		SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r);		SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r);		SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r);		SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r);		SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r);		SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r);		SmartDashboard::PutNumber("Constant Lift", constantLift);		SmartDashboard::PutNumber("Rotate Rate", rotateRate + r);		i = (i + 1) % 2;		printf("2.1");//		.PutLong("test1.2", 1337);		printf("2.2");//		mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime());		printf("2.3");//		mqServer.PutString("test1.1","YOLO_SWAGINS");		printf("2.4");//		SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1"));//		SmartDashboard::PutNumber("test1", mqServer.GetDouble("test"));//		SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2"));//		SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime());	}

    /**     * Runs the motors with Mecanum drive.     */    void OperatorControl()    {        robotDrive.SetSafetyEnabled(false);        while (IsOperatorControl() && IsEnabled())        {            bool collisionDetected = false;            double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();            double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;            last_world_linear_accel_x = curr_world_linear_accel_x;            double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();            double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;            last_world_linear_accel_y = curr_world_linear_accel_y;            if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||                 ( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {                collisionDetected = true;            }            SmartDashboard::PutBoolean(  "CollisionDetected", collisionDetected);            try {                /* Use the joystick X axis for lateral movement,            */                /* Y axis for forward movement, and Z axis for rotation.    */                /* Use navX MXP yaw angle to define Field-centric transform */                robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),                                                  stick.GetZ(),ahrs->GetAngle());            } catch (std::exception ex ) {                std::string err_string = "Error communicating with Drive System:  ";                err_string += ex.what();                DriverStation::ReportError(err_string.c_str());            }            Wait(0.005); // wait 5ms to avoid hogging CPU cycles        }    }

	// Start auto mode	void AutonomousInit() override {		autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard		currentState = 1;		ahrs->ZeroYaw();		ahrs->GetFusedHeading();		autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);		autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);		autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);		liftdown->Set(false);	}

    /**     * Drive based upon joystick inputs, and automatically control     * motors if the robot begins tipping.     */    void OperatorControl()    {        robotDrive.SetSafetyEnabled(false);        while (IsOperatorControl() && IsEnabled()) {            double xAxisRate = stick.GetX();            double yAxisRate = stick.GetY();            double pitchAngleDegrees = ahrs->GetPitch();            double rollAngleDegrees = ahrs->GetRoll();            if ( !autoBalanceXMode &&                 (fabs(pitchAngleDegrees) >=                  fabs(kOffBalanceThresholdDegrees))) {                autoBalanceXMode = true;            }            else if ( autoBalanceXMode &&                      (fabs(pitchAngleDegrees) <=                       fabs(kOnBalanceThresholdDegrees))) {                autoBalanceXMode = false;            }            if ( !autoBalanceYMode &&                 (fabs(pitchAngleDegrees) >=                  fabs(kOffBalanceThresholdDegrees))) {                autoBalanceYMode = true;            }            else if ( autoBalanceYMode &&                      (fabs(pitchAngleDegrees) <=                       fabs(kOnBalanceThresholdDegrees))) {                autoBalanceYMode = false;            }            // Control drive system automatically,            // driving in reverse direction of pitch/roll angle,            // with a magnitude based upon the angle            if ( autoBalanceXMode ) {                double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);                xAxisRate = sin(pitchAngleRadians) * -1;            }            if ( autoBalanceYMode ) {                double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);                yAxisRate = sin(rollAngleRadians) * -1;            }            try {                // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.                robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());            } catch (std::exception ex ) {                std::string err_string = "Drive system error:  ";                err_string += ex.what();                DriverStation::ReportError(err_string.c_str());            }            Wait(0.005); // wait 5ms to avoid hogging CPU cycles        }    }

void imuSetup ( void ) {	//MPU initialization	imu->initialize();	printf( "Beginning Gyro calibration.../n" );	for( int i = 0 ; i < 100; i++ ) {		imu->update();		imu->read_gyroscope( &gx , &gy , &gz);		offset[0] += ( -gx );		offset[1] += ( -gy );		offset[2] += ( -gz );		usleep(10000);	}	offset[0] /= 100.0;	offset[1] /= 100.0;	offset[2] /= 100.0;	printf( "Offsets are: %f %f %f/n" ,offset[0] ,offset[1] ,offset[2] );	ahrs.setGyroOffset( offset[0] , offset[1] , offset[2] );}

void imuSetup(){    //----------------------- MPU initialization ------------------------------    imu->initialize();    //-------------------------------------------------------------------------	printf("Beginning Gyro calibration.../n");	for(int i = 0; i<100; i++)	{		imu->update();    		imu->read_gyroscope(&gx, &gy, &gz);		gx *= 180 / PI;		gy *= 180 / PI;		gz *= 180 / PI;		offset[0] += (-gx*0.0175);		offset[1] += (-gy*0.0175);		offset[2] += (-gz*0.0175);		usleep(10000);	}	offset[0]/=100.0;	offset[1]/=100.0;	offset[2]/=100.0;	printf("Offsets are: %f %f %f/n", offset[0], offset[1], offset[2]);	ahrs.setGyroOffset(offset[0], offset[1], offset[2]);}

 /**  * Runs the motors with Mecanum drive.  */ void OperatorControl() {     robotDrive.SetSafetyEnabled(false);     while (IsOperatorControl() && IsEnabled())     {         bool reset_yaw_button_pressed = stick.GetRawButton(1);         if ( reset_yaw_button_pressed ) {             ahrs->ZeroYaw();         }         bool rotateToAngle = false;         if ( stick.GetRawButton(2)) {             turnController->SetSetpoint(0.0f);             rotateToAngle = true;         } else if ( stick.GetRawButton(3)) {             turnController->SetSetpoint(90.0f);             rotateToAngle = true;         } else if ( stick.GetRawButton(4)) {             turnController->SetSetpoint(179.9f);             rotateToAngle = true;         } else if ( stick.GetRawButton(5)) {             turnController->SetSetpoint(-90.0f);             rotateToAngle = true;         }         double currentRotationRate;         if ( rotateToAngle ) {             turnController->Enable();             currentRotationRate = rotateToAngleRate;         } else {             turnController->Disable();             currentRotationRate = stick.GetTwist();         }         try {             /* Use the joystick X axis for lateral movement,          */             /* Y axis for forward movement, and the current           */             /* calculated rotation rate (or joystick Z axis),         */             /* depending upon whether "rotate to angle" is active.    */             robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),                                               currentRotationRate ,ahrs->GetAngle());         } catch (std::exception ex ) {             std::string err_string = "Error communicating with Drive System:  ";             err_string += ex.what();             DriverStation::ReportError(err_string.c_str());         }         Wait(0.005); // wait 5ms to avoid hogging CPU cycles     } }

	void AutonomousGyroTurn() {		switch (currentState) {		case 1:			timer->Reset();			timer->Start();			//State: Stopped			//Transition: Driving State			currentState = 2;			break;		case 2:			//State: Driving			//Stay in State until 2 seconds have passed--`			//Transition: Gyroturn State			drive->TankDrive(0.5, 0.5);			if (timer->Get() >= 1) {				drive->TankDrive(0.0, 0.0);				ahrs->ZeroYaw();				currentState = 3;				turnController->SetSetpoint(90);				turnController->Enable();			}			break;		case 3:			//State: Gyroturn			//Stay in state until navx yaw has reached 90 degrees			//Transition: Driving State			drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);//			if (ahrs->GetYaw() >= 90) {			if (turnController->OnTarget()) {				drive->TankDrive(0.0, 0.0);				currentState = 4;				timer->Reset();				timer->Start();			}			break;		case 4:			//State:Driving			//Stay in state until 2 seconds have passed			//Transition: State Stopped			drive->TankDrive(0.5, 0.5);			if (timer->Get() >= 1) {				currentState = 5;				timer->Stop();			}			break;		case 5:			//State: Stopped			drive->TankDrive(0.0, 0.0);			break;		}	}

    /**     * Runs the motors with Mecanum drive.     */    void OperatorControl()    {        robotDrive.SetSafetyEnabled(false);        while (IsOperatorControl() && IsEnabled())        {            bool motionDetected = ahrs->IsMoving();            SmartDashboard::PutBoolean("MotionDetected", motionDetected);            try {                /* Use the joystick X axis for lateral movement,            */                /* Y axis for forward movement, and Z axis for rotation.    */                /* Use navX MXP yaw angle to define Field-centric transform */                robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),                                                  stick.GetZ(),ahrs->GetAngle());            } catch (std::exception ex ) {                std::string err_string = "Error communicating with Drive System:  ";                err_string += ex.what();                DriverStation::ReportError(err_string.c_str());            }            Wait(0.005); // wait 5ms to avoid hogging CPU cycles        }    }

	void AutonomousAdjustableStraight() {		switch (currentState) {		case 1:			timer->Reset();			timer->Start();			turnController->Reset();			turnController->SetSetpoint(ahrs->GetYaw());			turnController->Enable();			currentState = 2;			break;		case 2:			intakeLever->Set(autoIntakeSpeed);			if (timer->Get() >= 1) {				intakeLever->Set(0);				currentState = 3;				timer->Reset();				timer->Start();			}			break;		case 3:			drive->TankDrive(autoSpeed, autoSpeed);			intakeLever->Set(-0.1);			if (timer->Get() >= autoLength) {				intakeLever->Set(0.0);				drive->TankDrive(0.0, 0.0);				currentState = 4;				timer->Reset();				timer->Start();			}			break;		case 4:			intake->Set(0.5);			shooter->Set(-0.5);			if (timer->Get() >= 2) {				currentState = 5;			}			break;		case 5:			intake->Set(0.0);			shooter->Set(0.0);			drive->TankDrive(0.0, 0.0);			break;		}	}

	void AutonomousStraightSpy() {		switch (currentState) {		case 1:			timer->Reset();			timer->Start();			turnController->Reset();			turnController->SetSetpoint(ahrs->GetYaw());			turnController->Enable();			currentState = 2;			break;		case 2:			intakeLever->Set(0.25);			if (timer->Get() >= 1) {				intakeLever->Set(0);				currentState = 3;				timer->Reset();				timer->Start();			}			break;		case 3:			drive->TankDrive(0.5, 0.5);			if (timer->Get() >= 5) {				drive->TankDrive(0.0, 0.0);				currentState = 4;				timer->Reset();				timer->Start();			}			break;		case 4:			intake->Set(0.5);			if (timer->Get() >= 2) {				currentState = 5;			}			break;		case 5:			intake->Set(0.0);			drive->TankDrive(0.0, 0.0);			break;		}	}

void imuSetup(){    //----------------------- MPU initialization ------------------------------    imu.initialize();    //-------------------------------------------------------------------------	printf("Beginning Gyro calibration.../n");	for(int i = 0; i<100; i++)	{		imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);		offset[0] += (-gx*0.0175);		offset[1] += (-gy*0.0175);		offset[2] += (-gz*0.0175);		usleep(10000);	}	offset[0]/=100.0;	offset[1]/=100.0;	offset[2]/=100.0;	printf("Offsets are: %f %f %f/n", offset[0], offset[1], offset[2]);	ahrs.setGyroOffset(offset[0], offset[1], offset[2]);}

	void AutonomousSpy() {//		Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal//		-------------------------------------------------------------------------------------------------------------------		switch (currentState) {		case 1:			//		-State: stopped			timer->Reset();			timer->Start();			ahrs->ZeroYaw();			currentState = 2;			break;//		--transition: state Driving Forward		case 2:			//		-State: Driving Forward			//		--wait until lined up with low goal			//		--transition: State stopped			drive->TankDrive(0.5, 0.5);			if (timer->Get() >= 1) { // NEEDS TO BE SET				//		-State: stopped				//		--wait until stopped				drive->TankDrive(0.0, 0.0);				currentState = 3;				timer->Reset();				timer->Start();			}			break;			//		--transition: State Shooting		case 3://		-State: Shooting//		--wait until shooting complete			intake->Set(-.5);			if (timer->Get() >= .7) { //Find Out Actual Time				intake->Set(0);				timer->Reset();				timer->Start();				currentState = 4;			}			break;			//		--transition: State Backing Up		case 4:			//		-State: Backing Up			//		--wait until off tower ramp			drive->TankDrive(-0.5, -0.5);			if (timer->Get() > 1) {				drive->TankDrive(0.0, 0.0);				ahrs->ZeroYaw();				ahrs->Reset();				currentState = 5;				turnController->SetSetpoint(-65.5);				turnController->Enable();			}			break;//		--transition: Turning		case 5:			//		-State: Turning Left			//		--wait until 65 degrees has been reached to line up with low bar			drive->TankDrive(-0.5, 0.5);			if (turnController->OnTarget()) {				drive->TankDrive(0.0, 0.0);				timer->Reset();				timer->Start();				currentState = 6;			}			break;//		--transition: Backing Up		case 6:			//		-State backing Up			//		--wait until near guard wall			drive->TankDrive(-0.5, -0.5);			if (timer->Get() >= 1) {				drive->TankDrive(0.0, 0.0);				ahrs->ZeroYaw();				ahrs->Reset();				currentState = 7;				turnController->SetSetpoint(-24.5);				turnController->Enable();			}			break;//		--transition: Turn Left		case 7://		-State: Turn Right//		--wait until 25 degree turn has been made to line with low bar			drive->TankDrive(-0.5, 0.5);			if (turnController->OnTarget()) {				drive->TankDrive(0.0, 0.0);				timer->Reset();				timer->Start();				currentState = 8;			}			break;//		--transition: Back Up		case 8://		-State: Backing Up//		--wait until backed through low bar			drive->TankDrive(-0.5, -0.5);			if (timer->Get() >= 1) { // NeedTo Update Value				timer->Stop();				currentState = 9;//.........这里部分代码省略.........

	void TeleopPeriodic() override {		float leftPower, rightPower; // Get the values for the main drive train joystick controllers		leftPower = -leftjoystick->GetY();		rightPower = -rightjoystick->GetY();		float multiplier; // TURBO mode		if (rightjoystick->GetRawButton(1))		{			multiplier = 1;		} else {			multiplier = 0.5;		}		// wtf is a setpoint - it's an angle to turn to		if (leftjoystick->GetRawButton(6)) {			turnController->Reset();			turnController->SetSetpoint(0);			turnController->Enable();			ahrs->ZeroYaw();			//ahrs->Reset();		}		// Press button to auto calculate angle to rotate bot to nearest ball//		if(leftjoystick->GetRawButton(99))//		{//			ahrs->ZeroYaw();//			turnController->Reset();//			turnController->SetSetpoint(mqServer.GetDouble("angle"));//			turnController->Enable();//			aimState = 1;//		}		switch(aimState)		{		default:		case 0: // No camera assisted turning			//Drive straight with one controller, else: drive with two controllers			if (leftjoystick->GetRawButton(1)) {				drive->TankDrive(leftPower * multiplier, leftPower * multiplier,						false);			} else if (leftjoystick->GetRawButton(2)) {				drive->TankDrive(leftPower * multiplier + rotateRate,						leftPower * multiplier + -rotateRate, false);			} else {				drive->TankDrive(leftPower * multiplier, rightPower * multiplier,						false);			}			break;		case 1: // Camera assisted turning, deny input from controllers			drive->TankDrive(rotateRate, -rotateRate, false);			if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {				aimState = 0; // Finished turning, auto assist off				turnController->Disable();				turnController->Reset();			}			break;		}		// That little flap at the bottom of the joystick		float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);		// Depending on the button, our intake will eat or shoot the ball		if (controlstick->GetRawButton(2)) {			intake->Set(-scaleIntake);			shooter->Set(scaleIntake);		} else if (controlstick->GetRawButton(1)) {			intake->Set(scaleIntake);			shooter->Set(-scaleIntake);		} else {			intake->Set(0);			shooter->Set(0);		}		// Control the motor that lifts and descends the intake bar		float intake_lever_power = 0;		if (controlstick->GetRawButton(6)) {			manual = true;			intake_lever_power = .3;//			intakeLever->Set(.30); // close		} else if (controlstick->GetRawButton(4)) {			manual = true;			intake_lever_power = -.4;//			intakeLever->Set(-.40); // open		} else if (controlstick->GetRawButton(3)){			manual = true;			intake_lever_power = -scaleIntake;//			intakeLever->Set(-scaleIntake);		} else if (controlstick->GetRawButton(5)) {			manual = true;			intake_lever_power = scaleIntake;//			intakeLever->Set(scaleIntake);		} else {			if (manual) {				manual = false;				lastLiftPos = intakeLever->GetEncPosition();				intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);				intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);				intakeLever->SetPID(1, 0.001, 0.0);				intakeLever->EnableControl();			}			intake_hold = true;//.........这里部分代码省略.........