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);
}