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);
}
}
private void SetupMotorOutput()
{
TalonSrx armTalon = (TalonSrx)_gearBox.GetMaster();
//_gearBox.SetControlMode(ControlMode.kPercentVbus);
_controlMode = ControlMode.kPercentVbus;
armTalon.SetVoltageRampRate(16.0f);
}
public void Setup()
{
TalonSrx armTalon = (TalonSrx)_gearBox.GetMaster();
armTalon.SetStatusFrameRateMs(TalonSrx.StatusFrameRate.StatusFrameRateGeneral, 10); //Send updates every 10ms instead of 10ms
armTalon.SetStatusFrameRateMs(TalonSrx.StatusFrameRate.StatusFrameRatePulseWidthMeas, 1); //1ms update insead of 100ms for encoder
armTalon.SetSensorDirection(true); //reversed sensor
armTalon.ConfigFwdLimitSwitchNormallyOpen(false);
armTalon.ConfigRevLimitSwitchNormallyOpen(false);
armTalon.EnableZeroSensorPositionOnReverseLimit(false); //enable on reverse limit
armTalon.EnableZeroSensorPositionOnForwardLimit(false);
}
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);
}
public static void Offset(TalonSrx talon, bool sensorIsReversed, int offset)
{
/* read the talon's absolute pulse wid */
int pos = Platform.Hardware.armTalon.GetPulseWidthPosition();
/* keep bottom 12bits */
pos &= 0xFFF;
/* offset */
{
pos -= offset;
if (pos < 0)
{
pos += 4096;
}
}
/* redo negative */
if (sensorIsReversed)
{
pos *= -1;
}
/* set it back */
Platform.Hardware.armTalon.SetPosition(pos);
}
/**
* @param ratioBetweenSensorAndGearedOutput If there is a redunction between sensor
* slice and geared output, enter the ratio (or product of all slices).
*
* @param talon that is connected to VP.
*/
public VersaPlanetaryWithMagEnc(float ratioBetweenSensorAndGearedOutput, TalonSrx talon)
: base(ratioBetweenSensorAndGearedOutput, talon, TalonSrx.FeedbackDevice.CtreMagEncoder_Relative)
{
}
/**
* @param talon that is connected to VP.
*/
public VersaPlanetaryWithMagEnc(TalonSrx talon)
: base(4096f, talon, SmartMotorController.FeedbackDevice.QuadEncoder)
{
}
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++;
}
}
