void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* scale the x-axis to go from 0 to sensorRange, left to right */
float leftAxisX = (((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2));
float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2);
Deadband(ref rightAxisX);
if (rightAxisX != 0)
{
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
_talon.Set(rightAxisX);
}
else if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kPercentVbus)
{
_targetPosition = _talon.GetPosition();
/* user has let go of the stick, lets closed-loop whereever we happen to be */
EnableClosedLoop();
}
/* When you press the 'A' button on a Logitech Gamepad
* and the enable button is pressed */
if (_btns[2] && !_btnsLast[2] && _gamepad.GetButton(kEnableButton))
{
_targetPosition = servo(leftAxisX, _talon.GetPosition(), _sensorRange);
EnableClosedLoop();
}
}
/// <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);
}