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();
}
}
/**
* Mecanum Drive that is inverted on the left side and decreases output when low battery
*
* @param Forward Forward/Backward drive of mecanum drive
* @param Strafe Left/Right drive of mecanum drive
* @param Twist Turn left/Right of mecanum drive
*/
private static void MecanumDrive(float Forward, float Strafe, float Twist)
{
float leftFrnt = (Forward + Strafe + Twist); // left front moves positive for forward, strafe-right, turn-right
float leftRear = (Forward - Strafe + Twist); // left rear moves positive for forward, strafe-left, turn-right
float rghtFrnt = (Forward - Strafe - Twist); // right front moves positive for forward, strafe-left, turn-left
float rghtRear = (Forward + Strafe - Twist); // right rear moves positive for forward, strafe-right, turn-left
/* Invert left sided motors */
leftFrnt *= -1;
leftRear *= -1;
/* If battery is lower than 10% scale down output */
if (_Battery.IsLow())
{
leftFrnt *= 0.5f;
leftRear *= 0.5f;
rghtFrnt *= 0.5f;
rghtRear *= 0.5f;
}
/* Feed Talons */
RightFront.Set(rghtFrnt);
RightRear.Set(rghtRear);
LeftFront.Set(leftFrnt);
LeftRear.Set(leftRear);
}
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);
}
/**
* 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 */
}
void Drive()
{
FillBtns(ref _btns);
float y = -1 * _gamepad.GetAxis(1);
Deadband(ref y);
_talon.ProcessMotionProfileBuffer();
/* button handler, if btn5 pressed launch MP, if btn7 pressed, enter voltage mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.SetControlMode(ControlMode.kMotionProfile);
_talon.Set(0);
CTRE.Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
TalonSrx.TrajectoryPoint point = new TalonSrx.TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0];
point.velocity = (float)MotionProfile.Points[i][1];
point.timeDurMs = MotionProfile.kDurationMs;
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.velocityOnly = false;
point.profileSlotSelect = 0;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
CTRE.Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.SetControlMode(ControlMode.kVoltage);
}
/* if in voltage mode, update output voltage */
if (_talon.GetControlMode() == ControlMode.kVoltage)
{
_talon.Set(14.0f * y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
/** 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);
}
}
public void ServoToSpeed(float speedRPM)
{
/* close loop constants */
_ServoParameters.P = 0.01f;
_ServoParameters.I = 0 * 0.0001f;
_ServoParameters.F = 6f / 2000f; // about 6V for 2000 RPM
/* save the target */
_targetSpeedRPM = speedRPM;
/* get measured speed */
float measuredSpeedRpm = MeasuredSpeed;
/* robot controller level closed loop, replace with firmware close loop later */
float output = _ServoParameters.PID(_targetSpeedRPM, measuredSpeedRpm, 0);
_tal.Set(output);
}
public static void Main()
{
TalonSrx test = new TalonSrx(0);
GameController stick = new GameController(UsbHostDevice.GetInstance(0), 0);
/* loop forever */
while (true)
{
if (stick.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
//This call is redundant but you can un-comment to guarantee limit switches will work or change the mode.
//test.ConfigLimitMode(TalonSrx.LimitMode.kLimitMode_SwitchInputsOnly);
Debug.Print("Rev: " + test.IsRevLimitSwitchClosed() + " | Fwd: " + test.IsFwdLimitSwitchClosed());
Debug.Print("" + test.GetFaults());
if (stick.GetButton(1))
{
test.ClearStickyFaults();
}
test.Set(stick.GetAxis(1));
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
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);
}
static void drive(float x, float y, float turn)
{
float[] ypr = new float[3];
pigeon.GetYawPitchRoll(ypr);
float turnPower = turnPID(turn - ypr[0]);
leftF.Set((y - x + turnPower));
leftB.Set((y + x + turnPower));
rightB.Set(-(y - x - turnPower));
rightF.Set(-(y + x - turnPower));
}
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);
}
}
public static void Main()
{
Arduino arduino = new Arduino(CTRE.HERO.IO.Port1.UART, 9600);
TalonSrx talon = new TalonSrx(1);
TalonSrx talon2 = new TalonSrx(2);
OI controller = new OI(new Gamepad(new UsbHostDevice()));
int motorSpeed = 0;
int motorStoredSpeed = 700;
float rMotorSpeed = 0f;
float rMotorStoreSpeed = .7f;
bool isPressed = false;
byte tData;
int tMotorSpeed;
int counter;
while (!arduino.Status())
{
Debug.Print("Waiting for arduino");
}
while (true)
{
if (controller.GetButton(OI.Button.a) && !isPressed)
{
motorSpeed += 50;
rMotorSpeed += .05f;
}
else if (controller.GetButton(OI.Button.b) && !isPressed)
{
motorSpeed -= 50;
rMotorSpeed -= .05f;
}
if (controller.GetButton(OI.Button.x) && !isPressed)
{
motorSpeed += 10;
rMotorSpeed += .01f;
}
else if (controller.GetButton(OI.Button.y) && !isPressed)
{
motorSpeed -= 10;
rMotorSpeed -= .01f;
}
if (controller.GetButton(OI.Button.a) || controller.GetButton(OI.Button.b) || controller.GetButton(OI.Button.x) || controller.GetButton(OI.Button.y))
{
isPressed = true;
}
else
{
isPressed = false;
}
if (controller.GetAxis(OI.Axis.left_y) == -1)
{
motorSpeed = 0;
rMotorSpeed = 0;
}
else if (controller.GetAxis(OI.Axis.left_y) == 1)
{
motorSpeed = motorStoredSpeed;
rMotorSpeed = rMotorStoreSpeed;
}
tMotorSpeed = motorSpeed / 10;
tData = (byte)(tMotorSpeed - (tMotorSpeed % 10)); //Outputs 100 place of motor speed
tData += (byte)(tMotorSpeed - tData);
if (controller.GetAxis(OI.Axis.right_y) == -1 && (counter % 1000) == 0)
{
arduino.sendCommand('U', 'P', tData, talon.GetOutputVoltage(), (byte)talon.GetOutputCurrent());
}
else if (controller.GetAxis(OI.Axis.right_y) == 1 && (counter % 1000) == 0)
{
arduino.sendCommand('D', 'O', tData, talon.GetOutputVoltage(), (byte)talon.GetOutputCurrent());
}
else if ((counter % 1000) == 0)
{
arduino.sendCommand('0', '0', tData, talon.GetOutputVoltage(), (byte)talon.GetOutputCurrent());
}
talon.Set(rMotorSpeed);
talon2.Set(rMotorSpeed);
CTRE.Watchdog.Feed();
Debug.Print("Motor Speed = " + motorSpeed);
Debug.Print("tData = " + tData);
counter++;
}
}
public void RunForever()
{
/* enable XInput, if gamepad is in DInput it will disable robot. This way you can
* use X mode for drive, and D mode for disable (instead of vice versa as the
* stock HERO implementation traditionally does). */
while (true)
{
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* get buttons */
bool[] btns = new bool[_buttons.Length];
for (uint i = 1; i < 20; ++i)
{
btns[i] = _gamepad.GetButton(i);
}
/* get sticks */
for (uint i = 0; i < _sticks.Length; ++i)
{
_sticks[i] = _gamepad.GetAxis(i);
}
/* yield for a bit, and track timeouts */
System.Threading.Thread.Sleep(10);
if (_rumblingTimeMs < 5000)
{
_rumblingTimeMs += 10;
}
/* update the Talon using the shoulder analog triggers */
_tal.Set((_sticks[5] - _sticks[4]) * 0.60f);
/* fire some solenoids based on buttons */
_driver.Set(1, _buttons[1]);
_driver.Set(2, _buttons[2]);
_driver.Set(3, _buttons[3]);
_driver.Set(4, _buttons[4]);
/* rumble state machine */
switch (_rumblinSt)
{
/* rumbling is disabled, require some off time to save battery */
case 0:
_gamepad.SetLeftRumble(0);
_gamepad.SetRightRumble(0);
if (_rumblingTimeMs < 100)
{
/* waiting for off-time */
}
else if ((btns[1] && !_buttons[1])) /* button off => on */
{
/* off time long enough, user pressed btn */
_rumblingTimeMs = 0;
_rumblinSt = 1;
_gamepad.SetLeftRumble(0xFF);
}
else if ((btns[2] && !_buttons[2])) /* button off => on */
{
/* off time long enough, user pressed btn */
_rumblingTimeMs = 0;
_rumblinSt = 1;
_gamepad.SetRightRumble(0xFF);
}
break;
/* already vibrating, track the time */
case 1:
if (_rumblingTimeMs > 500)
{
/* vibrating too long, turn off now */
_rumblingTimeMs = 0;
_rumblinSt = 0;
_gamepad.SetLeftRumble(0);
_gamepad.SetRightRumble(0);
}
else if ((btns[3] && !_buttons[3])) /* button off => on */
{
/* immedietely turn off */
_rumblingTimeMs = 0;
_rumblinSt = 0;
_gamepad.SetLeftRumble(0);
_gamepad.SetRightRumble(0);
}
else if ((btns[1] && !_buttons[1])) /* button off => on */
{
_gamepad.SetLeftRumble(0xFF);
}
else if ((btns[2] && !_buttons[2])) /* button off => on */
{
_gamepad.SetRightRumble(0xFF);
}
break;
}
/* this will likley be replaced with a strongly typed interface,
* control the LEDs on the center XBOX emblem. */
if (btns[5] && !_buttons[5])
{
_gamepad.SetLEDCode(6);
}
if (btns[6] && !_buttons[6])
{
_gamepad.SetLEDCode(7);
}
if (btns[7] && !_buttons[7])
{
_gamepad.SetLEDCode(8);
}
if (btns[8] && !_buttons[8])
{
_gamepad.SetLEDCode(9);
}
/* build line to print */
StringBuilder sb = new StringBuilder();
foreach (float stick in _sticks)
{
sb.Append(Format(stick));
sb.Append(",");
}
sb.Append("-");
for (uint i = 1; i < _buttons.Length; ++i)
{
if (_buttons[i])
{
sb.Append("b" + i + ",");
}
}
/* print useful info */
sb.AppendLine();
Debug.Print(sb.ToString());
/* save button states for button-change states */
_buttons = btns;
}
}
