/**
* 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);
}
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);
}
}
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();
}
private void SetupPositionServo()
{
TalonSRX armTalon = (TalonSRX)_gearBox.MasterMotorController;
armTalon.ConfigNominalOutputForward(0);
armTalon.ConfigNominalOutputReverse(0);
armTalon.ConfigPeakOutputForward(1);
armTalon.ConfigPeakOutputReverse(-1);
armTalon.ConfigAllowableClosedloopError(0, Constants.TOLERANCE);
armTalon.Config_kP(Constants.KPARM);
armTalon.Config_kI(Constants.KIARM);
armTalon.Config_kD(Constants.KDARM);
_controlMode = ControlMode.MotionMagic;
armTalon.SelectProfileSlot(0);
armTalon.ConfigMotionAcceleration(60);
armTalon.ConfigMotionCruiseVelocity(22);
}
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();
}
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);
}
