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(ControlMode.kPercentVbus);
_talon.Set(rightAxisX);
}
else if (_talon.GetControlMode() == 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();
}
}
void Drive()
{
FillBtns(ref _btns);
float y = -1 * _gamepad.GetAxis(1);
Deadband(ref y);
_talon.ProcessMotionProfileBuffer();
/* button handler, if btn5 pressed launch MP, if btn7 pressed, enter voltage mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.SetControlMode(ControlMode.kMotionProfile);
_talon.Set(0);
CTRE.Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
TalonSrx.TrajectoryPoint point = new TalonSrx.TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0];
point.velocity = (float)MotionProfile.Points[i][1];
point.timeDurMs = MotionProfile.kDurationMs;
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.velocityOnly = false;
point.profileSlotSelect = 0;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
CTRE.Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.SetControlMode(ControlMode.kVoltage);
}
/* if in voltage mode, update output voltage */
if (_talon.GetControlMode() == ControlMode.kVoltage)
{
_talon.Set(14.0f * y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* get the left y stick, invert so forward is positive */
float leftY = kJoystickScaler * _gamepad.GetAxis(1);
Deadband(ref leftY);
/* debounce the transition from nonzero => zero axis */
float filteredY = leftY;
if (filteredY != 0)
{
/* put in a ramp to prevent the user from flipping their mechanism */
_talon.SetVoltageRampRate(12.0f); /* V per sec */
/* directly control the output */
_talon.SetControlMode(ControlMode.kPercentVbus);
_talon.Set(filteredY);
}
else if (_talon.GetControlMode() == ControlMode.kPercentVbus)
{
_targetPosition = _talon.GetPosition();
/* user has let go of the stick, lets closed-loop whereever we happen to be */
EnableClosedLoop();
}
/* if a button is pressed while stick is let go, servo position */
if (filteredY == 0)
{
if (_btns[1])
{
_targetPosition = _talon.GetPosition(); /* twenty rotations forward */
EnableClosedLoop();
}
else if (_btns[4])
{
_targetPosition = +10.0f; /* twenty rotations forward */
EnableClosedLoop();
}
else if (_btns[2])
{
_targetPosition = -10.0f; /* twenty rotations reverese */
EnableClosedLoop();
}
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
