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 percent output mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.Set(ControlMode.MotionProfile, 0);
Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
TrajectoryPoint point = new TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0] * kTicksPerRotation; //convert from rotations to sensor units
point.velocity = (float)MotionProfile.Points[i][1] * kTicksPerRotation / 600; //convert from RPM to sensor units per 100 ms.
point.headingDeg = 0; //not used in this example
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.profileSlotSelect0 = 0;
point.profileSlotSelect1 = 0; //not used in this example
point.timeDur = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(ControlMode.MotionProfile, 1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.Set(ControlMode.PercentOutput, 0);
}
/* if not in motion profile mode, update output percent */
if (_talon.GetControlMode() != ControlMode.MotionProfile)
{
_talon.Set(ControlMode.PercentOutput, y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}