//reads the talons and updates the data
public void updateStatusData()
{
talonCurrent = (talon.GetOutputCurrent());
talonTemperature = (talon.GetTemperature());
talonVoltage = (talon.GetOutputVoltage());
talonSpeed = (talon.GetSpeed());
talonPosition = (talon.GetPosition());
talonSetpoint = (talon.GetSetpoint());
talonForwardLimitReached = talon.IsFwdLimitSwitchClosed() ? 1 : 0;
talonReverseLimitReached = talon.IsRevLimitSwitchClosed() ? 1 : 0;
controlMode = talon.GetControlMode();
}
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());
}
}
/** occasionally builds a line and prints to output window */
void Instrument()
{
if (--_timeToPrint <= 0)
{
_timeToPrint = 20;
_sb.Clear();
_sb.Append("pos=");
_sb.Append(_talon.GetPosition());
_sb.Append(" vel=");
_sb.Append(_talon.GetSpeed());
_sb.Append(" err=");
_sb.Append(_talon.GetClosedLoopError());
_sb.Append(" out%=");
_sb.Append(_talon.GetOutputVoltage() * 100.0f / 12.0f);
Debug.Print(_sb.ToString());
}
}
public static void Process(CTRE.TalonSrx talon)
{
/* simple timeout to reduce printed lines */
if (++_instrumLoops1 > 10)
{
_instrumLoops1 = 0;
/* get closed loop info */
float pos = talon.GetPosition();
float spd = talon.GetSpeed();
int err = talon.GetClosedLoopError();
/* build the string */
_sb.Clear();
_sb.Append(pos);
if (_sb.Length < 16)
{
_sb.Append(' ', 16 - _sb.Length);
}
_sb.Append(spd);
if (_sb.Length < 32)
{
_sb.Append(' ', 32 - _sb.Length);
}
_sb.Append(err);
if (_sb.Length < 48)
{
_sb.Append(' ', 48 - _sb.Length);
}
Debug.Print(_sb.ToString()); /* print data row */
if (++_instrumLoops2 > 8)
{
_instrumLoops2 = 0;
_sb.Clear();
_sb.Append("Position");
if (_sb.Length < 16)
{
_sb.Append(' ', 16 - _sb.Length);
}
_sb.Append("Velocity");
if (_sb.Length < 32)
{
_sb.Append(' ', 32 - _sb.Length);
}
_sb.Append("Error");
if (_sb.Length < 48)
{
_sb.Append(' ', 48 - _sb.Length);
}
Debug.Print(_sb.ToString()); /* print columns */
}
}
}
