/**
* Set the gyro type based on the sensitivity.
* This takes the number of volts/degree/second sensitivity of the gyro and uses it in subsequent
* calculations to allow the code to work with multiple gyros.
*
* @param slot The slot the analog module is connected to
* @param channel The analog channel the gyro is plugged into
* @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second.
*/
void SetGyroSensitivity(UINT32 slot, UINT32 channel,
float voltsPerDegreePerSecond)
{
Gyro *gyro = AllocateGyro(slot, channel);
if (gyro)
gyro->SetSensitivity(voltsPerDegreePerSecond);
}
void OperatorControl()
{
float xJoy, yJoy, gyroVal, angle = 0, turn = 0, angleDiff, turnPower;
gyro.Reset();
gyro.SetSensitivity(9.7);
while (IsEnabled() && IsOperatorControl()) // loop as long as the robot is running
{
yJoy = xbox.getAxis(Xbox::L_STICK_V);
xJoy = xbox.getAxis(Xbox::R_STICK_H);
gyroVal = gyro.GetAngle()/0.242*360;
turn = 0.15;
angle = angle - xJoy * xJoy * xJoy * turn;
angleDiff = mod(angle - gyroVal, 360);
turnPower = - mod(angleDiff / 180.0 + 1.0, 2) + 1.0;
SmartDashboard::PutString("Joy1", vectorToString(xJoy, yJoy));
SmartDashboard::PutNumber("Heading", mod(gyroVal, 360));
SmartDashboard::PutNumber("Turn Power", turnPower);
SmartDashboard::PutBoolean("Switch is ON:", dumperSwitch.Get());
if (!xbox.isButtonPressed(Xbox::R))
{
drive.ArcadeDrive(yJoy * yJoy * yJoy, turnPower * fabs(turnPower), false);
}
}
}
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 Gyro(TestBench::kCameraGyroChannel);
m_gyro->SetSensitivity(0.013);
}
