/** occasionally builds a line and prints to output window */
void Instrument()
{
if (--_timeToPrint <= 0)
{
_timeToPrint = 20;
_sb.Clear();
_sb.Append("pos=");
_sb.Append(_talon.GetSelectedSensorPosition(0));
_sb.Append(" vel=");
_sb.Append(_talon.GetSelectedSensorVelocity(0));
_sb.Append(" err=");
_sb.Append(_talon.GetClosedLoopError(0));
_sb.Append(" out%=");
_sb.Append(_talon.GetMotorOutputPercent());
Debug.Print(_sb.ToString());
}
}
public static void Process(TalonSRX talon)
{
/* simple timeout to reduce printed lines */
if (++_instrumLoops1 > 10)
{
_instrumLoops1 = 0;
/* get closed loop info */
float pos = talon.GetSelectedSensorPosition(0);
float spd = talon.GetSelectedSensorVelocity(0);
int err = talon.GetClosedLoopError(0);
/* 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 */
}
}
}
public static void Main()
{
/* Hardware */
TalonSRX _talon = new TalonSRX(1);
/** Use a USB gamepad plugged into the HERO */
GameController _gamepad = new GameController(UsbHostDevice.GetInstance());
/* String for output */
StringBuilder _sb = new StringBuilder();
/** hold bottom left shoulder button to enable motors */
const uint kEnableButton = 7;
/* Loop tracker for prints */
int _loops = 0;
/* Initialization */
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* Config sensor used for Primary PID [Velocity] */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
Constants.kPIDLoopIdx,
Constants.kTimeoutMs);
/**
* Phase sensor accordingly.
* Positive Sensor Reading should match Green (blinking) Leds on Talon
*/
_talon.SetSensorPhase(false);
/* Config the peak and nominal outputs */
_talon.ConfigNominalOutputForward(0, Constants.kTimeoutMs);
_talon.ConfigNominalOutputReverse(0, Constants.kTimeoutMs);
_talon.ConfigPeakOutputForward(1, Constants.kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1, Constants.kTimeoutMs);
/* Config the Velocity closed loop gains in slot0 */
_talon.Config_kF(Constants.kPIDLoopIdx, Constants.kF, Constants.kTimeoutMs);
_talon.Config_kP(Constants.kPIDLoopIdx, Constants.kP, Constants.kTimeoutMs);
_talon.Config_kI(Constants.kPIDLoopIdx, Constants.kI, Constants.kTimeoutMs);
_talon.Config_kD(Constants.kPIDLoopIdx, Constants.kD, Constants.kTimeoutMs);
/* loop forever */
while (true)
{
/* Get gamepad axis */
double leftYstick = -1 * _gamepad.GetAxis(1);
/* Get Talon/Victor's current output percentage */
double motorOutput = _talon.GetMotorOutputPercent();
/* Prepare line to print */
_sb.Append("\tout:");
/* Cast to int to remove decimal places */
_sb.Append((int)(motorOutput * 100));
_sb.Append("%"); // Percent
_sb.Append("\tspd:");
_sb.Append(_talon.GetSelectedSensorVelocity(Constants.kPIDLoopIdx));
_sb.Append("u"); // Native units
/**
* When button 1 is held, start and run Velocity Closed loop.
* Velocity Closed Loop is controlled by joystick position x2000 RPM, [-2000, 2000] RPM
*/
if (_gamepad.GetButton(1))
{
/* Velocity Closed Loop */
/**
* Convert 2000 RPM to units / 100ms.
* 4096 Units/Rev * 2000 RPM / 600 100ms/min in either direction:
* velocity setpoint is in units/100ms
*/
double targetVelocity_UnitsPer100ms = leftYstick * 2000.0 * 4096 / 600;
/* 2000 RPM in either direction */
_talon.Set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);
/* Append more signals to print when in speed mode. */
_sb.Append("\terr:");
_sb.Append(_talon.GetClosedLoopError(Constants.kPIDLoopIdx));
_sb.Append("\ttrg:");
_sb.Append(targetVelocity_UnitsPer100ms);
}
else
{
/* Percent Output */
_talon.Set(ControlMode.PercentOutput, leftYstick);
}
/* Print built string every 10 loops */
if (++_loops >= 10)
{
_loops = 0;
Debug.Print(_sb.ToString());
}
/* Reset built string */
_sb.Clear();
//if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) // check if gamepad is plugged in OR....
if (_gamepad.GetButton(kEnableButton)) // check if bottom left shoulder buttom is held down.
{
/* then enable motor outputs*/
Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(20);
}
}
