// Arcade Drive Controller
static void ArcadeDrive()
{
// Set slaves and inversion
m_masterRight.SetInverted(true);
m_slaveRight.SetInverted(true);
m_slaveRight.Follow(m_masterRight);
m_masterLeft.SetInverted(false);
m_slaveLeft.SetInverted(false);
m_slaveLeft.Follow(m_masterLeft);
// Get Axis Value (-1.0 to 1.0)
float y = m_controller.GetAxis(LEFT_JOYSTICK_Y);
float x = m_controller.GetAxis(RIGHT_JOYSTICK_X);
// Set speed values
double throttle = Deadband(y); // Throttle equals y axis value of left joystick with deadband
double steer = Deadband(x); // Steer equals x axis value of right joystick with deadband
throttle = System.Math.Pow(throttle, 3.0);
steer = System.Math.Pow(steer, 3.0);
double rightSpeed = throttle + steer;
double leftSpeed = throttle - steer;
// If either right or left speed are out of range (-1.0 to 1.0) Scale both until in range
double maxValue = 0;
if (System.Math.Abs(leftSpeed) > maxValue)
{
maxValue = System.Math.Abs(leftSpeed);
}
if (System.Math.Abs(rightSpeed) > maxValue)
{
maxValue = System.Math.Abs(rightSpeed);
}
//Scale down all values if max > 1.0
if (maxValue > 1.0)
{
leftSpeed /= maxValue;
rightSpeed /= maxValue;
}
// Set outputs
m_masterRight.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, rightSpeed);
m_masterLeft.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, leftSpeed);
}