C++ AnalogGyro类代码示例

/**
  * Gets calibrated parameters from previously calibrated gyro, allocates a new
  * gyro with the given parameters for center and offset, and re-runs tests on
  * the new gyro.
  */
void TiltPanCameraTest::GyroCalibratedParameters() {
  uint32_t cCenter = m_gyro->GetCenter();
  float cOffset = m_gyro->GetOffset();
  delete m_gyro;
  m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset);
  m_gyro->SetSensitivity(kSensitivity);

  // Default gyro angle test
  // Accumulator needs a small amount of time to reset before being tested
  m_gyro->Reset();
  Wait(.001);
  EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f);

  // Gyro angle test
  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
  m_pan->SetAngle(0.0);
  Wait(0.5);
  m_gyro->Reset();

  for (int i = 0; i < 600; i++) {
    m_pan->Set(i / 1000.0);
    Wait(0.001);
  }

  double gyroAngle = m_gyro->GetAngle();

  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
      << kTestAngle << " degrees";
}

 static void SetUpTestCase() {
   // The gyro object blocks for 5 seconds in the constructor, so only
   // construct it once for the whole test case
   m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel);
   m_gyro->SetSensitivity(kSensitivity);
 }

// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
    // Tank drive, both left and right joystick control their respective motor along the
    // joystick's 'y' axis
    //myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));
    //myRobot.TankDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));

    // Choose the teleop drive option
    DriverControl(driveOption);


    // Edits the gyro data to account for drift
    /*
    editedGyroRate = gyro.GetRate();
    if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) {
    	editedGyroRate = 0;
    } else {
    	editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE;
    }
    */

    editedGyroAngle = gyro.GetAngle();



    SmartDashboard::PutString("Operation Type:", "TeleOp");

    // Print gyro data
    if (showGyro == true) {
        SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR);
        SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR);
        SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
        SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
    }


    // Print out the encoder data
    // Raw encoder data
    if (showEncoderRaw == true) {
        SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
        SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());

    }

    // The delta encoder change
    if (showEncoderRate == true) {
        SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
        SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
    }

    // Print the encoder index
    if (showEncoderIndex == true) {
        SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
        SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
    }
}

// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
	// Tank drive, both left and right joystick control their respective motor along the
	// joystick's 'y' axis
	//myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));

	// Choose the teleop drive option
	for(int c=0;c<7;c++) {
		buttonDone[c] = false;
	}

	DriverControl(driveOption);

	if(fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE) {
	editedGyroRate = gyro.GetAngle();
	}
	else {
		editedGyroRate = 0;
		gyro.Reset();
	}





	SmartDashboard::PutString("Operation Type:", "TeleOp");

	// Print gyro data
	if (showGyro == true) {
		SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
	}


	// Print out the encoder data
	// Raw encoder data
	if (showEncoderRaw == true) {
		SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
		SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());

	}

	// The delta encoder change
	if (showEncoderRate == true) {
		SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
		SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
	}

	// Print the encoder index
	if (showEncoderIndex == true) {
		SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
		SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
	}
}

	// The initializer of dooooooom!!!!!
	Robot() :
		myRobot(0, 1),	// these must be initialized in the same order
		rStick(RIGHT_JOYSTICK_INPUT_CHANNEL),		// as they are declared above.
		lStick(LEFT_JOYSTICK_INPUT_CHANNEL),
		gamePad(GAMEPAD_INPUT_CHANNEL),
		lw(LiveWindow::GetInstance()),
		autoLoopCounter(0),
		gyro(GYRO_INPUT_CHANNEL),
		encoder1(ENCODER_CHANNEL_1A, ENCODER_CHANNEL_1B),
		encoder2(ENCODER_CHANNEL_2A, ENCODER_CHANNEL_2B),
		lifterEncoder(LIFTER_ENCODER_CHANNEL_1, LIFTER_ENCODER_CHANNEL_2),
		ballManipulatorEncoder(B_MANIPULATOR_ENCODER_CHANNEL_1, B_MANIPULATOR_ENCODER_CHANNEL_2),
		lifter(LIFTER_CHANNEL_LIFT, LIFTER_CHANNEL_TILT, lifterEncoder),
		ballManipulator(B_MANIPULATOR_CHANNEL_LIFT, B_MANIPULATOR_CHANNEL_PINCH, ballManipulatorEncoder)
	{
		myRobot.SetExpiration(0.1);

		driveOption = TANK_GAMEPAD;

		gyro.InitGyro();
		showGyro = true;

		showEncoderRaw = true;
		showEncoderRate = false;
		showEncoderIndex = false;

		autoTurn = 0;
	}

    void AutonomousInit()
    {
        gyro.Reset();

        if(Auto==1)
        {
            d1=84;
        }
        else if(Auto==2)
        {
            d1=36;
        }
        else if(Auto==3)
        {
            d1=12;
            ang=-ang;
        }
        else if(Auto==4)
        {
            d1=60;
            ang=-ang;
        }

        autoLoopCounter = 0;
        EncVal1=(d1/cir)*Rev;

    }

/**
 * Test if the servo turns 90 degrees and the gyroscope measures this angle
 * Note servo on TestBench is not the same type of servo that servo class
 * was designed for so setAngle is significantly off. This has been calibrated
 * for the servo on the rig.
 */
void TiltPanCameraTest::GyroAngle() {
  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
  m_pan->SetAngle(0.0);
  Wait(0.5);
  m_gyro->Reset();

  for (int i = 0; i < 600; i++) {
    m_pan->Set(i / 1000.0);
    Wait(0.001);
  }

  double gyroAngle = m_gyro->GetAngle();

  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
      << kTestAngle << " degrees";
}

// When the robot is on autonomous period, it will drive forwards at half speed for about two
// seconds, then stops
void Robot::AutonomousPeriodic() {
	/*
	if(autoLoopCounter < 100) { //Check if we've completed 100 loops (approximately 2 seconds)
		myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		autoLoopCounter++;
	} else {
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}
	*/



	// Edits the gyro angle to account for drift
	if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
		editedGyroAngle = gyro.GetAngle();
	else {
		gyro.Reset();
		editedGyroAngle = 0;
	}



	// Adjust course based on gyro data
	if (editedGyroAngle > 0)
		autoTurn = 0.3;
	else if (editedGyroAngle < 0)
		autoTurn = -0.3;
	else
		autoTurn = 0;

	// Go foward 3 feet at 1/3 speed, stop (adjusting for drift)
	if (encoder1.GetRaw() < ONE_FOOT_LEFT_ENCODER*3 || encoder2.GetRaw() < ONE_FOOT_RIGHT_ENCODER*3)
		myRobot.Drive(0.3, autoTurn);
	else
		myRobot.Drive(0.0, 0.0);


	// Print the raw encoder data
	SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
	SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}

  virtual void SetUp() override {
    m_tilt = new Servo(TestBench::kCameraTiltChannel);
    m_pan = new Servo(TestBench::kCameraPanChannel);
    m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0);

    m_tilt->Set(kTiltSetpoint45);
    m_pan->SetAngle(0.0f);

    Wait(kServoResetTime);

    m_gyro->Reset();
  }

    void AutonomousPeriodic()
    {
        if((Auto==1)||(Auto==2))
        {
            while(gyro.GetAngle()<ang)
            {
                motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
                myRobot.TankDrive(motorspeed/2,motorspeed/-2);
            }
        }
        if((Auto==3)||(Auto==4))
        {
            while(gyro.GetAngle()>ang)
            {
                motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
                myRobot.TankDrive(motorspeed/-2,motorspeed/2);
            }
        }
        while(EncDist.GetDistance()<EncVal1)
        {
            myRobot.Drive(-0.5, 0.0);
        }
        ang = -ang;
        if((Auto==1)||(Auto==2))
        {
            while(gyro.GetAngle()<ang)
            {
                motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
                myRobot.TankDrive(motorspeed/2,motorspeed/-2);
            }
        }
        if((Auto==3)||(Auto==4))
        {
            while(gyro.GetAngle()>ang)
            {
                motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
                myRobot.TankDrive(motorspeed/-2,motorspeed/2);
            }
        }
        launcher.Set(1.0);
        sonic.SetAutomaticMode(true);
        while(sonic.GetRangeInches()>DefToShot)
        {
            myRobot.Drive(-0.5,0.0);
        }
        myRobot.Drive(0.0,0.0);
        intake.Set(1.0);

    }

	// The initializer of dooooooom!!!!!
	Robot() :
		myRobot(0, 1),	// these must be initialized in the same order
		rStick(RIGHT_JOYSTICK_INPUT_CHANNEL),		// as they are declared above.
		lStick(LEFT_JOYSTICK_INPUT_CHANNEL),
		gamePad(GAMEPAD_INPUT_CHANNEL),
		lw(LiveWindow::GetInstance()),
		autoLoopCounter(0),
		gyro(GYRO_INPUT_CHANNEL),
		encoder1(ENCODER_CHANNEL_1A, ENCODER_CHANNEL_1B),
		encoder2(ENCODER_CHANNEL_2A, ENCODER_CHANNEL_2B)
	{
		myRobot.SetExpiration(0.1);

		driveOption = ARCADE_GAMEPAD_2;

		gyro.InitGyro();
		showGyro = true;

		showEncoderRaw = true;
		showEncoderRate = true;
		showEncoderIndex = false;
	}

/**
 * Test if the gyro angle defaults to 0 immediately after being reset.
 */
void TiltPanCameraTest::DefaultGyroAngle() {
  EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f);
}

// To determine which drive control type the robot to use
// In case user operator wants different controls
void Robot::DriverControl(int driveControl) {
	switch (driveControl) {
	// Arcade drive (think racing games) w/ 1 joysticks
	case ARCADE_1:
		// If the turn is outside the joystick's standard drift, robot will assume the robot is turning
		// If not, robot will assume it is pushed, or has naturally drifted
		if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
			gyro.Reset();
		} else {
			// The turn will be the opposite of what the gyro says the angle of unintentional drift is,
			// which will have the robot go straight
			myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Arcade drive w/ 2 joysticks
	case ARCADE_2:
		if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Arcade drive w/ left stick on gamepad (the knockoff xbox controller)
	case ARCADE_GAMEPAD_1:
		if (fabs(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Arcade drive w/ BOTH gamepad
	case ARCADE_GAMEPAD_2:
		if (fabs(gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Tank drive (requires 2 sticks; each stick controls its respective 'tread' or side;
	//			   Ex: moving the right stick moves only the wheels on the right) w/ gamepad
	case TANK_GAMEPAD:
		joystickDifference = gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) - gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y);
		joystickAverage = (gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) + gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y))/2;
		if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
			myRobot.TankDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(joystickAverage, editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Tank drive w/ joysticks
	case TANK_2:
		joystickDifference = lStick.GetRawAxis(LEFT_STICK_Y) - rStick.GetRawAxis(RIGHT_STICK_Y);
		joystickAverage = (lStick.GetRawAxis(LEFT_STICK_Y) + rStick.GetRawAxis(RIGHT_STICK_Y))/2;
		if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
			myRobot.TankDrive(lStick.GetRawAxis(LEFT_STICK_Y), rStick.GetRawAxis(RIGHT_STICK_Y));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(joystickAverage, -rStick.GetRawAxis(RIGHT_STICK_Y));
		}

		break;
	}
}

// Starts at the beginning of the teleop (user controlled) period
void Robot::TeleopInit() {

	gyro.Reset();
}