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 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);
}
}
/**
* Setup all of the configuration parameters.
*/
public int SetupConfig()
{
/* binary OR all the return values so we can make a quick decision if our init was successful */
int status = 0;
/* specify sensor characteristics */
_talon.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_talon.SetSensorDirection(false); /* make sure positive motor output means sensor moves in position direction */
// call ConfigEncoderCodesPerRev or ConfigPotentiometerTurns for Quadrature or Analog sensor types.
/* brake or coast during neutral */
status |= _talon.ConfigNeutralMode(TalonSrx.NeutralMode.Brake);
/* closed-loop and motion-magic parameters */
status |= _talon.SetF(kSlotIdx, 0.1153f, kTimeoutMs); // 1300RPM (8874 native sensor units per 100ms) at full motor output (+1023)
status |= _talon.SetP(kSlotIdx, 2.00f, kTimeoutMs);
status |= _talon.SetI(kSlotIdx, 0f, kTimeoutMs);
status |= _talon.SetD(kSlotIdx, 20f, kTimeoutMs);
status |= _talon.SetIzone(kSlotIdx, 0, kTimeoutMs);
status |= _talon.SelectProfileSlot(kSlotIdx); /* select this slot */
status |= _talon.ConfigNominalOutputVoltage(0f, 0f, kTimeoutMs);
status |= _talon.ConfigPeakOutputVoltage(+12f, -12f, kTimeoutMs);
status |= _talon.SetMotionMagicCruiseVelocity(1000f, kTimeoutMs); // 1000 RPM
status |= _talon.SetMotionMagicAcceleration(2000f, kTimeoutMs); // 2000 RPM 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.
*/
status |= _talon.SetPosition(0, kTimeoutMs);
return(status);
}
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()
{
/* first choose the sensor */
_talon.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_talon.SetSensorDirection(false);
//_talon.ConfigEncoderCodesPerRev(XXX), // if using CTRE.TalonSrx.FeedbackDevice.QuadEncoder
//_talon.ConfigPotentiometerTurns(XXX), // if using CTRE.TalonSrx.FeedbackDevice.AnalogEncoder or CTRE.TalonSrx.FeedbackDevice.AnalogPot
/* set closed loop gains in slot0 */
_talon.SetP(0, 0.2f); /* tweak this first, a little bit of overshoot is okay */
_talon.SetI(0, 0f);
_talon.SetD(0, 0f);
_talon.SetF(0, 0f); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0);
/* set the peak and nominal outputs, 12V means full */
_talon.ConfigNominalOutputVoltage(+0.0f, -0.0f);
_talon.ConfigPeakOutputVoltage(+3.0f, -3.0f);
/* how much error is allowed? This defaults to 0. */
_talon.SetAllowableClosedLoopErr(0, 0);
/* 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*/
CTRE.Watchdog.Feed();
}
/* print signals to Output window */
Instrument();
/* 10ms loop */
Thread.Sleep(10);
}
}
private void SetupPositionServo()
{
TalonSrx armTalon = (TalonSrx)_gearBox.GetMaster();
armTalon.ConfigNominalOutputVoltage(0, 0);
armTalon.ConfigPeakOutputVoltage(Constants.MAX_VOLTAGE, -Constants.MAX_VOLTAGE);
armTalon.SetAllowableClosedLoopErr(0, Constants.TOLERANCE);
armTalon.SetPID(0, Constants.KPARM, Constants.KIARM, Constants.KDARM);
_controlMode = ControlMode.kPosition;
armTalon.SelectProfileSlot(0);
armTalon.SetMotionMagicAcceleration(60.0f);
armTalon.SetMotionMagicCruiseVelocity(22.0f);
armTalon.SetVoltageRampRate(0f);
}
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);
}