public void Drive(double speed, double turn, bool isQuickTurn, bool isHighGear)
{
if (DriverStation.Instance.Autonomous)
{
return;
}
double wheelNonLinearity;
turn = HandleDeadband(turn, m_speedDeadzone);
speed = HandleDeadband(speed, m_turnDeadzone);
//double negInertia = turn - m_oldTurn;
//m_oldTurn = turn;
//If throttle == 0 and quickturn is off, things get weird. Trust me, I tried.
isQuickTurn |= speed == 0;
if (isHighGear)
{
wheelNonLinearity = m_highNonLinearity;
// Apply a sin function that's scaled to make it feel better.
turn = Math.Sin(Math.PI / 2.0 * wheelNonLinearity * turn)
/ Math.Sin(Math.PI / 2.0 * wheelNonLinearity);
turn = Math.Sin(Math.PI / 2.0 * wheelNonLinearity * turn)
/ Math.Sin(Math.PI / 2.0 * wheelNonLinearity);
}
else
{
wheelNonLinearity = m_lowNonLinearity;
// Apply a sin function that's scaled to make it feel better.
turn = Math.Sin(Math.PI / 2.0 * wheelNonLinearity * turn)
/ Math.Sin(Math.PI / 2.0 * wheelNonLinearity);
turn = Math.Sin(Math.PI / 2.0 * wheelNonLinearity * turn)
/ Math.Sin(Math.PI / 2.0 * wheelNonLinearity);
turn = Math.Sin(Math.PI / 2.0 * wheelNonLinearity * turn)
/ Math.Sin(Math.PI / 2.0 * wheelNonLinearity);
}
double leftPwm, rightPwm, overPower;
double sensitivity;
double angularPower;
double linearPower;
// Negative inertia!
// Currently not enabled
//double negInertiaScalar;
if (isHighGear)
{
//negInertiaScalar = 5.0;
sensitivity = m_highSensitivity;
}
else
{
/*
if (turn * negInertia > 0)
{
//negInertiaScalar = 2.5;
}
else
{
if (Math.Abs(turn) > 0.65)
{
//negInertiaScalar = 5.0;
}
else
{
//negInertiaScalar = 3.0;
}
}
*/
sensitivity = m_lowSensitivity;
}
//double negInertiaPower = negInertia * negInertiaScalar;
//m_negInertiaAccumulator += negInertiaPower;
/*
turn = turn + m_negInertiaAccumulator;
if (m_negInertiaAccumulator > 1)
{
m_negInertiaAccumulator -= 1;
}
else if (m_negInertiaAccumulator < -1)
{
m_negInertiaAccumulator += 1;
}
else
{
m_negInertiaAccumulator = 0;
}
*/
linearPower = speed;
// Quickturn!
if (isQuickTurn)
{
if (Math.Abs(linearPower) < 0.2)
{
double alpha = 0.1;
m_quickStopAccumulator = (1 - alpha) * m_quickStopAccumulator + alpha * ((Math.Abs(turn) < 1.0) ? turn : Math.Sign(turn)) * 5;
}
overPower = 1.0;
angularPower = turn;
}
else
{
overPower = 0.0;
angularPower = Math.Abs(speed) * turn * sensitivity - m_quickStopAccumulator;
if (m_quickStopAccumulator > 1)
{
m_quickStopAccumulator -= 1;
}
else if (m_quickStopAccumulator < -1)
{
m_quickStopAccumulator += 1;
}
else
{
m_quickStopAccumulator = 0.0;
}
}
rightPwm = leftPwm = linearPower;
leftPwm += angularPower;
rightPwm -= angularPower;
if (leftPwm > 1.0)
{
rightPwm -= overPower * (leftPwm - 1.0);
leftPwm = 1.0;
}
else if (rightPwm > 1.0)
{
leftPwm -= overPower * (rightPwm - 1.0);
rightPwm = 1.0;
}
else if (leftPwm < -1.0)
{
rightPwm += overPower * (-1.0 - leftPwm);
leftPwm = -1.0;
}
else if (rightPwm < -1.0)
{
leftPwm += overPower * (-1.0 - rightPwm);
rightPwm = -1.0;
}
m_drive.SetPowers(leftPwm, rightPwm);
}