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