void ResetTargetPosition()
{
_targetPosition = _talon.GetPosition();
_talon.SetPosition(_targetPosition); //Sets current and desired positions to be equal so we don't move unexpectedly at startup
Thread.Sleep(100); //wait to make sure SetPosition takes effect
}
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);
}
void Instrument()
{
if (--_timeToColumns <= 0)
{
_timeToColumns = 400;
_sb.Clear();
_sb.Append("topCnt \t");
_sb.Append("btmCnt \t");
_sb.Append("setval \t");
_sb.Append("HasUndr\t");
_sb.Append("IsUnder\t");
_sb.Append(" IsVal \t");
_sb.Append(" IsLast\t");
_sb.Append("VelOnly\t");
_sb.Append(" TargetPos[AndVelocity] \t");
_sb.Append("Pos[AndVelocity]");
Debug.Print(_sb.ToString());
}
if (--_timeToPrint <= 0)
{
_timeToPrint = 40;
_sb.Clear();
_sb.Append(_motionProfileStatus.topBufferCnt);
_sb.Append("\t\t");
_sb.Append(_motionProfileStatus.btmBufferCnt);
_sb.Append("\t\t");
_sb.Append(_motionProfileStatus.outputEnable);
_sb.Append("\t\t");
_sb.Append(_motionProfileStatus.hasUnderrun ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.isUnderrun ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePointValid ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePoint.isLastPoint ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePoint.velocityOnly ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePoint.position);
_sb.Append("[");
_sb.Append(_motionProfileStatus.activePoint.velocity);
_sb.Append("]\t");
_sb.Append("\t\t\t");
_sb.Append(_talon.GetPosition());
_sb.Append("[");
_sb.Append(_talon.GetSpeed());
_sb.Append("]");
Debug.Print(_sb.ToString());
}
}