void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* scale the x-axis to go from 0 to sensorRange, left to right */
int leftAxisX = (int)(((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2));
float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2);
Deadband(ref rightAxisX);
if (rightAxisX != 0)
{
_talon.Set(ControlMode.PercentOutput, rightAxisX);
}
else if (_talon.GetControlMode() == ControlMode.PercentOutput)
{
_targetPosition = _talon.GetSelectedSensorPosition(0);
/* 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.GetSelectedSensorPosition(0), _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 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);
}
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)
{
/* directly control the output */
_talon.Set(ControlMode.PercentOutput, filteredY);
}
else if (_talon.GetControlMode() == ControlMode.PercentOutput)
{
_targetPosition = _talon.GetSelectedSensorPosition(0);
/* 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.GetSelectedSensorPosition(0); /* current position */
EnableClosedLoop();
}
else if (_btns[4])
{
_targetPosition = +40960.0f; /* ten rotations forward */
EnableClosedLoop();
}
else if (_btns[2])
{
_targetPosition = -40960.0f; /* ten rotations reverse */
EnableClosedLoop();
}
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}