public void Run()
{
//_talon.SetControlMode(TalonSRX.ControlMode.kVoltage);
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
_talon.SetSensorPhase(false);
_talon.Config_kP(0, 0.80f);
_talon.Config_kI(0, 0f);
_talon.Config_kD(0, 0f);
_talon.Config_kF(0, 0.09724488664269079041176191004297f);
_talon.SelectProfileSlot(0, 0);
_talon.ConfigNominalOutputForward(0f, 50);
_talon.ConfigNominalOutputReverse(0f, 50);
_talon.ConfigPeakOutputForward(+1.0f, 50);
_talon.ConfigPeakOutputReverse(-1.0f, 50);
_talon.ChangeMotionControlFramePeriod(5);
_talon.ConfigMotionProfileTrajectoryPeriod(0, 50);
/* loop forever */
while (true)
{
_talon.GetMotionProfileStatus(_motionProfileStatus);
Drive();
Watchdog.Feed();
Instrument();
Thread.Sleep(5);
}
}
/**
* Setup all of the configuration parameters.
*/
public void SetupConfig()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* specify sensor characteristics */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
_talon.SetSensorPhase(false); /* make sure positive motor output means sensor moves in position direction */
/* brake or coast during neutral */
_talon.SetNeutralMode(NeutralMode.Brake);
/* closed-loop and motion-magic parameters */
_talon.Config_kF(kSlotIdx, 0.1153f, kTimeoutMs); // 8874 native sensor units per 100ms at full motor output (+1023)
_talon.Config_kP(kSlotIdx, 2.00f, kTimeoutMs);
_talon.Config_kI(kSlotIdx, 0f, kTimeoutMs);
_talon.Config_kD(kSlotIdx, 20f, kTimeoutMs);
_talon.Config_IntegralZone(kSlotIdx, 0, kTimeoutMs);
_talon.SelectProfileSlot(kSlotIdx, 0); /* select this slot */
_talon.ConfigNominalOutputForward(0f, kTimeoutMs);
_talon.ConfigNominalOutputReverse(0f, kTimeoutMs);
_talon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
_talon.ConfigMotionCruiseVelocity(8000, kTimeoutMs); // 8000 native units
_talon.ConfigMotionAcceleration(16000, kTimeoutMs); // 16000 native units per sec, (0.5s to reach cruise velocity).
/* Home the relative sensor,
* alternatively you can throttle until limit switch,
* use an absolute signal like CtreMagEncoder_Absolute or analog sensor.
*/
_talon.SetSelectedSensorPosition(0, kTimeoutMs);
}
uint [] _debLeftY = { 0, 0 }; // _debLeftY[0] is how many times leftY is zero, _debLeftY[1] is how many times leftY is not zeero.
public void Run()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* first choose the sensor */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
_talon.SetSensorPhase(false);
/* set closed loop gains in slot0 */
_talon.Config_kP(0, 0.2f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
_talon.Config_kI(0, 0f, kTimeoutMs);
_talon.Config_kD(0, 0f, kTimeoutMs);
_talon.Config_kF(0, 0f, kTimeoutMs); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
_talon.ConfigNominalOutputForward(0.0f, kTimeoutMs);
_talon.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
_talon.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
_talon.ConfigAllowableClosedloopError(0, 0, kTimeoutMs);
/* put in a ramp to prevent the user from flipping their mechanism in open loop mode */
_talon.ConfigClosedloopRamp(0, kTimeoutMs);
_talon.ConfigOpenloopRamp(1, kTimeoutMs);
/* zero the sensor and throttle */
ZeroSensorAndThrottle();
/* loop forever */
while (true)
{
Loop10Ms();
//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();
}
/* print signals to Output window */
Instrument();
/* 10ms loop */
Thread.Sleep(10);
}
}
public void init()
{
/* first choose the sensor */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0);
_talon.SetSensorPhase(false);
/* set closed loop gains in slot0 */
_talon.Config_kP(0, 0.5f); /* tweak this first, a little bit of overshoot is okay */
_talon.Config_kI(0, 0f);
_talon.Config_kD(0, 0f);
_talon.Config_kF(0, 0f); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0, 0);
/*set target to the current position */
ResetTargetPosition();
}
public void Config(int talonId0, int talonId1)
{
talon0 = new TalonSRX(talonId0);
talon1 = new TalonSRX(talonId1);
talon0.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
talon0.SetSensorPhase(false);
talon0.SetNeutralMode(NeutralMode.Brake);
talon0.Config_kF(kSlotId, 0.1153f, kTimeout);
talon0.Config_kP(kSlotId, 2.00f, kTimeout);
talon0.Config_kI(kSlotId, 0f, kTimeout);
talon0.Config_kD(kSlotId, 20f, kTimeout);
talon0.Config_IntegralZone(kSlotId, 0, kTimeout);
talon0.SelectProfileSlot(kSlotId, 0);
talon0.ConfigNominalOutputForward(0f, kTimeout);
talon0.ConfigNominalOutputReverse(0f, kTimeout);
talon0.ConfigPeakOutputForward(1.0f, kTimeout);
talon0.ConfigPeakOutputReverse(-1.0f, kTimeout);
talon0.ConfigMotionCruiseVelocity(8000, kTimeout);
talon0.ConfigMotionAcceleration(16000, kTimeout);
}
public void init()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* nonzero to block the config until success, zero to skip checking */
const int kTimeoutMs = 30;
/* first choose the sensor */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, kTimeoutMs);
_talon.SetSensorPhase(false);
/* set closed loop gains in slot0 */
_talon.Config_kP(0, 0.5f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
_talon.Config_kI(0, 0f, kTimeoutMs);
_talon.Config_kD(0, 0f, kTimeoutMs);
_talon.Config_kF(0, 0f, kTimeoutMs); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0, 0);
/*set target to the current position */
ResetTargetPosition();
}
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);
}
}
static void Initialize()
{
// Serial port
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
_uart.Open();
// Victor SPX Slaves
// Left Slave
victor1.Set(ControlMode.Follower, 0);
// Right Slave
victor3.Set(ControlMode.Follower, 2);
// Talon SRX Slaves
// Feeder Slave
feederL.Set(ControlMode.Follower, 0);
feederL.SetInverted(true);
// Intake Slave
intakeLft.Set(ControlMode.Follower, 2);
intakeLft.SetInverted(true);
// Hood
hood.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog, 0, kTimeoutMs);
hood.SetSensorPhase(false);
hood.Config_kP(0, 30f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
hood.Config_kI(0, 0.0005f, kTimeoutMs);
hood.Config_kD(0, 0f, kTimeoutMs);
hood.Config_kF(0, 0f, kTimeoutMs);
/* use slot0 for closed-looping */
hood.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
hood.ConfigNominalOutputForward(0.0f, kTimeoutMs);
hood.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
hood.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
hood.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
hood.ConfigForwardSoftLimitThreshold(HOOD_LOWER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigReverseSoftLimitThreshold(HOOD_UPPER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
hood.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
hood.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
//***********************
// MAY NEED TUNING
//***********************
// Turret
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 1, kTimeoutMs);
int absPos = turret.GetSelectedSensorPosition(1);
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
turret.SetSelectedSensorPosition(0, 0, kTimeoutMs);
turret.SetSensorPhase(true);
turret.Config_IntegralZone(20);
turret.Config_kP(0, 2f, kTimeoutMs); // tweak this first, a little bit of overshoot is okay
turret.Config_kI(0, 0f, kTimeoutMs);
turret.Config_kD(0, 0f, kTimeoutMs);
turret.Config_kF(0, 0f, kTimeoutMs);
// use slot0 for closed-looping
turret.SelectProfileSlot(0, 0);
// set the peak and nominal outputs, 1.0 means full
turret.ConfigNominalOutputForward(0.0f, kTimeoutMs);
turret.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
turret.ConfigPeakOutputForward(+0.5f, kTimeoutMs);
turret.ConfigPeakOutputReverse(-0.5f, kTimeoutMs);
turret.ConfigReverseSoftLimitThreshold(TURRET_UPPER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigForwardSoftLimitThreshold(TURRET_LOWER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
turret.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
// how much error is allowed? This defaults to 0.
turret.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
// Shooter
shooterSensorTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
shooterSensorTalon.SetSensorPhase(false);
shooterSensorTalon.ConfigPeakOutputReverse(0.0f, kTimeoutMs);
shooterSensorTalon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
shooterVESC = new PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7);
shooterVESC.Set(0);
pcm.SetSolenoidOutput(shooterFeedbackLEDPort, false);
}