public void Run()
{
_talon.SetControlMode(ControlMode.kVoltage);
_talon.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_talon.SetSensorDirection(false);
_talon.SetVoltageRampRate(0.0f);
_talon.SetP(0, 0.80f);
_talon.SetI(0, 0f);
_talon.SetD(0, 0f);
_talon.SetF(0, 0.09724488664269079041176191004297f);
_talon.SelectProfileSlot(0);
_talon.ConfigNominalOutputVoltage(0f, 0f);
_talon.ConfigPeakOutputVoltage(+12.0f, -12.0f);
_talon.ChangeMotionControlFramePeriod(5);
/* loop forever */
while (true)
{
_talon.GetMotionProfileStatus(out _motionProfileStatus);
Drive();
CTRE.Watchdog.Feed();
Instrument();
Thread.Sleep(5);
}
}
void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* scale the x-axis to go from 0 to sensorRange, left to right */
float leftAxisX = (((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2));
float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2);
Deadband(ref rightAxisX);
if (rightAxisX != 0)
{
_talon.SetControlMode(ControlMode.kPercentVbus);
_talon.Set(rightAxisX);
}
else if (_talon.GetControlMode() == ControlMode.kPercentVbus)
{
_targetPosition = _talon.GetPosition();
/* user has let go of the stick, lets closed-loop whereever we happen to be */
EnableClosedLoop();
}
/* When you press the 'A' button on a Logitech Gamepad
* and the enable button is pressed */
if (_btns[2] && !_btnsLast[2] && _gamepad.GetButton(kEnableButton))
{
_targetPosition = servo(leftAxisX, _talon.GetPosition(), _sensorRange);
EnableClosedLoop();
}
}
/**
* Zero the sensor and zero the throttle.
*/
void ZeroSensorAndThrottle()
{
_talon.SetPosition(0); /* start our position at zero, this example uses relative positions */
_targetPosition = 0;
/* zero throttle */
_talon.SetControlMode(ControlMode.kPercentVbus);
_talon.Set(0);
Thread.Sleep(100); /* wait a bit to make sure the Setposition() above takes effect before sampling */
}
/** spin in this routine forever */
public void RunForever()
{
/* config our talon, don't continue until it's successful */
int initStatus = SetupConfig(); /* configuration */
while (initStatus != 0)
{
Instrument.PrintConfigError();
initStatus = SetupConfig(); /* (re)config*/
}
/* robot loop */
while (true)
{
/* get joystick params */
float leftY = -1f * _gamepad.GetAxis(1);
bool btnTopLeftShoulder = _gamepad.GetButton(5);
bool btnBtmLeftShoulder = _gamepad.GetButton(7);
Deadband(ref leftY);
/* keep robot enabled if gamepad is connected and in 'D' mode */
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* set the control mode based on button pressed */
if (btnTopLeftShoulder)
{
_mode = ControlMode.kPercentVbus;
}
if (btnBtmLeftShoulder)
{
_mode = ControlMode.kMotionMagic;
}
/* calc the Talon output based on mode */
if (_mode == ControlMode.kPercentVbus)
{
float output = leftY; // [-1, +1] percent duty cycle
_talon.SetControlMode(_mode);
_talon.Set(output);
}
else if (_mode == ControlMode.kMotionMagic)
{
float servoToRotation = leftY * 10;// [-10, +10] rotations
_talon.SetControlMode(_mode);
_talon.Set(servoToRotation);
}
/* instrumentation */
Instrument.Process(_talon);
/* wait a bit */
System.Threading.Thread.Sleep(5);
}
}
void initTurret()
{
/* first choose the sensor */
_turret.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_turret.SetSensorDirection(false);
_turret.ConfigEncoderCodesPerRev(4096); // if using CTRE.TalonSrx.FeedbackDevice.QuadEncoder
_turret.SetP(0, TURRET_P); /* tweak this first, a little bit of overshoot is okay */
_turret.SetI(0, TURRET_I);
_turret.SetD(0, TURRET_D);
_turret.SetF(0, TURRET_F);
/* use slot0 for closed-looping */
_turret.SelectProfileSlot(0);
/* set the peak and nominal outputs, 12V means full */
_turret.ConfigNominalOutputVoltage(MINIMUM_TURRET_VOLTAGE, -1 * MINIMUM_TURRET_VOLTAGE); //The minimum voltage that will be applied to the turret.
_turret.ConfigPeakOutputVoltage(+3.0f, -3.0f); //THe maximum voltage that will be applied to the turret.
/* how much error is allowed? This defaults to 0. */
_turret.SetAllowableClosedLoopErr(0, 0);
_turret.SetPosition(0); /* start our position at zero, this example uses relative positions */
_turret.SetVoltageRampRate(0); /* V per sec */
_turret.SetControlMode(ControlMode.kPosition);
_turret.Set(angleSetpoint);
_turret.SetEncPosition(0);
}
public void Setup()
{
_tal.ConfigFwdLimitSwitchNormallyOpen(true);
_tal.ConfigRevLimitSwitchNormallyOpen(true);
_tal.SetControlMode(ControlMode.kVoltage); //voltage control mode
_tal.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative); //sensor type
_tal.SetStatusFrameRateMs(TalonSrx.StatusFrameRate.StatusFrameRatePulseWidthMeas, 1); //feedback to 1ms
}
void initShooter()
{
/* first choose the sensor */
_shooter.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_shooter.SetSensorDirection(false);
_shooter.SetControlMode(ControlMode.kSpeed);
_shooter.ConfigNominalOutputVoltage(+0.65f, -0.65f); //The minimum voltage that will be applied to the shooter.
_shooter.ConfigPeakOutputVoltage(+12.0f, -12.0f); //THe maximum voltage that will be applied to the shooter.
_shooter.Set(0);
_shooter.SetP(0, SHOOTER_P); /* tweak this first, a little bit of overshoot is okay */
_shooter.SetI(0, SHOOTER_I);
_shooter.SetD(0, SHOOTER_D);
_shooter.SetF(0, SHOOTER_F);
/* use slot0 for closed-looping */
_shooter.SelectProfileSlot(0);
}