/** 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); } }