/// <summary>
/// Drives Kiwi around
/// </summary>
static void Drive()
{
if (EStop)
{
return;
}
float aSpeed, bSpeed, cSpeed;
if (null == _gamepad)
{
_gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance());
}
float x = _gamepad.GetAxis(0);
float y = -1 * _gamepad.GetAxis(1);
float twist = _gamepad.GetAxis(2);
float adjustmentAngle = 0;
if (FieldOriented1)
{
TryFieldOriented(out adjustmentAngle);
}
else
{
adjustmentAngle = Orientation;
}
//Always using radial coordinates allows for easy orientation adjustment outside of field oriented
float magnitude = (float)System.Math.Sqrt(x * x + y * y),
directionRadians = (float)System.Math.Atan(x / y);
x = magnitude * (float)System.Math.Cos(directionRadians - adjustmentAngle);
y = magnitude * (float)System.Math.Sin(directionRadians - adjustmentAngle);
Deadband(ref x, .05f);
Deadband(ref y, .05f);
Deadband(ref twist, .05f);
aSpeed = x / 2 - (float)System.Math.Sqrt(3) / 2 * y + twist / (2);
bSpeed = x / 2 + (float)System.Math.Sqrt(3) / 2 * y + twist / (2);
cSpeed = x * -1 + twist / 2;
Deadband(ref aSpeed, .07f);
Deadband(ref bSpeed, .07f);
Deadband(ref cSpeed, .07f);
motorA.Set(aSpeed);
motorB.Set(bSpeed);
motorC.Set(cSpeed);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}