static void Run()
{
float x = _gamepad.GetAxis(1);
Deadband(ref x);
//Set the Maximum Current Limit for the Talon (in Amps)
talon.SetCurrentLimit(10);
//Enable the Current Limiting Feature.
talon.EnableCurrentLimit(true);
talon.Set(x);
float current = talon.GetOutputCurrent();
stringBuilder.Append("\t");
stringBuilder.Append(current);
stringBuilder.Append("\t");
}
public static void Main()
{
RCDGamepad gamepad = new RCDGamepad(new CTRE.UsbHostDevice());
TalonSrx driveTalon = new TalonSrx(1);
driveTalon.SetInverted(true);
driveTalon.SetCurrentLimit(1);
driveTalon.EnableCurrentLimit(true);
driveTalon.ConfigNeutralMode(TalonSrx.NeutralMode.Coast);
TalonSrx flywheelTalon = new TalonSrx(2);
flywheelTalon.SetCurrentLimit(40);
flywheelTalon.EnableCurrentLimit(true);
flywheelTalon.ConfigNeutralMode(TalonSrx.NeutralMode.Coast);
PigeonImu centerOfMassIMU = new PigeonImu(1);
PigeonImu headIMU = new PigeonImu(2);
uint pulsePeriod = 20000;
uint pulseDuration = 1500;
PWM pwm_tilt = new PWM(CTRE.HERO.IO.Port3.PWM_Pin9, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false);
PWM pwm_headRoll = new PWM(CTRE.HERO.IO.Port3.PWM_Pin7, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false);
PWM pwm_headPitch = new PWM(CTRE.HERO.IO.Port3.PWM_Pin6, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false);
PWM pwm_headSpin = new PWM(CTRE.HERO.IO.Port3.PWM_Pin8, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false);
PWM pwm_disco = new PWM(CTRE.HERO.IO.Port3.PWM_Pin4, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false);
pwm_tilt.Start();
pwm_headRoll.Start();
pwm_headPitch.Start();
pwm_headSpin.Start();
pwm_disco.Start();
RCDTalonSpeedController driveSpeedController = new RCDTalonSpeedController("Primary Drive");
driveSpeedController.loggingEnabled = false;
driveSpeedController.rampingEnabled = false;
bool previousButtonX = false;
bool previousButtonY = false;
bool previousButtonB = false;
bool previousButtonA = false;
bool previousStickButtonL = false;
bool previousStickButtonR = false;
bool staticOperationMode = false;
ConfigureBodyLightColors();
int orientationCompensation = -1;
while (true)
{
if (gamepad.GetConnectionStatus() != UsbDeviceConnection.Connected)
{
continue;
}
CTRE.Watchdog.Feed();
// Check for whether we need to flip orientation
bool currentButtonY = gamepad.buttonY;
if (!currentButtonY && previousButtonY)
{
orientationCompensation *= -1;
// No (working) interface for haptic feedback currently exists
// RumbleForDuration(gamepad, 0.25);
}
previousButtonY = currentButtonY;
// Check for whether we need to switch control modes between driving and static modes
bool currentButtonB = gamepad.buttonB;
if (!currentButtonB && previousButtonB)
{
staticOperationMode = !staticOperationMode;
// No (working) interface for haptic feedback currently exists
// RumbleForDuration(gamepad, 0.25);
}
previousButtonB = currentButtonB;
uint discoDuration = 0;
// Check for whether we need to send happy beeps
bool currentButtonA = gamepad.buttonA;
if (currentButtonA)
{
discoDuration = 1500;
}
previousButtonA = currentButtonA;
// Check if we need to send sad beeps
bool currentButtonX = gamepad.buttonX;
if (currentButtonX)
{
discoDuration = 2500;
}
previousButtonX = currentButtonX;
// Check if we need to send volume down
bool currentStickButtonL = gamepad.buttonLeftJoyClick;
if (currentStickButtonL)
{
discoDuration = 3500;
}
previousStickButtonL = currentStickButtonL;
// Check if we need to send volume up
bool currentStickButtonR = gamepad.buttonRightJoyClick;
if (currentStickButtonR)
{
discoDuration = 4500;
}
previousStickButtonR = currentStickButtonR;
pwm_disco.Duration = discoDuration;
/* Capture button states for gamepad */
Vector leftVec = gamepad.leftVector;
Vector rightVec = gamepad.rightVector;
bool leftBumper = gamepad.leftBumper;
bool rightBumper = gamepad.rightBumper;
bool leftTrigger = gamepad.leftTrigger;
bool rightTrigger = gamepad.rightTrigger;
uint servoZero = 1500;
pwm_headRoll.Duration = (uint)((rightVec.x * -600 * orientationCompensation) + servoZero);
pwm_headPitch.Duration = (uint)((rightVec.y * 600 * orientationCompensation) + servoZero);
if (staticOperationMode)
{
// Not driving or tilting while in static mode
driveTalon.Set(0);
pwm_tilt.Duration = servoZero;
pwm_headSpin.Duration = (uint)((leftVec.x * 600 * orientationCompensation) + servoZero);
}
else
{
// Drive forward/backward
float driveValue = leftVec.y * orientationCompensation;
driveValue = driveSpeedController.ComputeCurrentValue(driveValue);
driveTalon.Set(driveValue);
uint headSpinScalar = 500;
uint headSpinPosition = servoZero;
headSpinPosition += (uint)(leftTrigger ? -headSpinScalar * orientationCompensation : 0);
headSpinPosition += (uint)(rightTrigger ? headSpinScalar * orientationCompensation : 0);
pwm_headSpin.Duration = headSpinPosition;
pwm_tilt.Duration = (uint)((leftVec.x * 600 * orientationCompensation) + servoZero);
}
/* Throttle buttons are additive so they cancel if pressed simultaneously */
float flywheelThrottle = 0;
float flywheelMagnitude = 1.0F;
flywheelThrottle += leftBumper ? flywheelMagnitude : 0;
flywheelThrottle += rightBumper ? -flywheelMagnitude : 0;
flywheelTalon.Set(flywheelThrottle);
// No current (working) interface for Logitech remote's rumble feature.
// Commenting out so we don't waste cycles
// UpdateRumbleState(gamepad);
// Wait a bit
System.Threading.Thread.Sleep(5);
}
}