public void Test()
{
int count = 0;
Watchdog dog = new Watchdog(100d);
dog.Bark += delegate
{
count++;
};
dog.Enabled = true;
dog.KeepBarking = false;
dog.Feed();
dog.Feed();
dog.Feed();
dog.Feed();
Thread.Sleep(200);
Assert.AreEqual(1, count);
Thread.Sleep(120);
dog.Enabled = false;
Assert.IsFalse(dog.Enabled);
Assert.IsFalse(dog.KeepBarking);
// TODO: 100% coverage in this way?
dog.Feed();
Assert.AreEqual(1, count);
}
public void Run()
{
//_talon.SetControlMode(TalonSRX.ControlMode.kVoltage);
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
_talon.SetSensorPhase(false);
_talon.Config_kP(0, 0.80f);
_talon.Config_kI(0, 0f);
_talon.Config_kD(0, 0f);
_talon.Config_kF(0, 0.09724488664269079041176191004297f);
_talon.SelectProfileSlot(0, 0);
_talon.ConfigNominalOutputForward(0f, 50);
_talon.ConfigNominalOutputReverse(0f, 50);
_talon.ConfigPeakOutputForward(+1.0f, 50);
_talon.ConfigPeakOutputReverse(-1.0f, 50);
_talon.ChangeMotionControlFramePeriod(5);
_talon.ConfigMotionProfileTrajectoryPeriod(0, 50);
/* loop forever */
while (true)
{
_talon.GetMotionProfileStatus(_motionProfileStatus);
Drive();
Watchdog.Feed();
Instrument();
Thread.Sleep(5);
}
}
public void Run()
{
/* loop forever */
while (true)
{
Loop10Ms();
//if (_gamepad.GetButton(kEnableButton)) // check if bottom left shoulder buttom is held down.
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected) // check if gamepad is plugged in OR....
{
/* then enable motor outputs*/
//Debug.Print("Watchdog fed");
Watchdog.Feed();
//Start the Compressor if the Pressure switch is triggered, GetPressureSwitchValue returns true if pressure if low
if (_PCM.GetPressureSwitchValue())
{
_PCM.StartCompressor();
Debug.Print("Compressor Current: " + _PCM.GetCompressorCurrent());
}
else
{
_PCM.StopCompressor();
Debug.Print("Compressor Current: " + _PCM.GetCompressorCurrent());
}
}
/* 10ms loop */
Thread.Sleep(10);
}
}
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 percent output mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.Set(ControlMode.MotionProfile, 0);
Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
TrajectoryPoint point = new TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0] * kTicksPerRotation; //convert from rotations to sensor units
point.velocity = (float)MotionProfile.Points[i][1] * kTicksPerRotation / 600; //convert from RPM to sensor units per 100 ms.
point.headingDeg = 0; //not used in this example
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.profileSlotSelect0 = 0;
point.profileSlotSelect1 = 0; //not used in this example
point.timeDur = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(ControlMode.MotionProfile, 1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.Set(ControlMode.PercentOutput, 0);
}
/* if not in motion profile mode, update output percent */
if (_talon.GetControlMode() != ControlMode.MotionProfile)
{
_talon.Set(ControlMode.PercentOutput, y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
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()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* first choose the sensor */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
_talon.SetSensorPhase(false);
/* set closed loop gains in slot0 */
_talon.Config_kP(0, 0.2f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
_talon.Config_kI(0, 0f, kTimeoutMs);
_talon.Config_kD(0, 0f, kTimeoutMs);
_talon.Config_kF(0, 0f, kTimeoutMs); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
_talon.ConfigNominalOutputForward(0.0f, kTimeoutMs);
_talon.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
_talon.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
_talon.ConfigAllowableClosedloopError(0, 0, kTimeoutMs);
/* put in a ramp to prevent the user from flipping their mechanism in open loop mode */
_talon.ConfigClosedloopRamp(0, kTimeoutMs);
_talon.ConfigOpenloopRamp(1, kTimeoutMs);
/* 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*/
Watchdog.Feed();
}
/* print signals to Output window */
Instrument();
/* 10ms loop */
Thread.Sleep(10);
}
}
public void run()
{
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*/
Watchdog.Feed();
}
}
public override sealed void RunForever()
{
/*first repeat call init until true */
bool flag = false;
while (!flag)
{
flag = RobotInit();
System.Threading.Thread.Sleep(kLoopTimeMs);
}
/* now the loops */
while (true)
{
bool en = IsEnabled();
bool au = IsAuton();
if (!en)
{
DisabledInit();
while (!IsEnabled())
{
DisabledPeriodic();
System.Threading.Thread.Sleep(kLoopTimeMs);
}
}
else if (au)
{
AutonInit();
while (IsEnabled() && IsAuton())
{
AutonPeriodic();
System.Threading.Thread.Sleep(kLoopTimeMs);
if (IsConnected())
{
Watchdog.Feed();
}
}
}
else
{
TeleopInit();
while (IsEnabled() && !IsAuton())
{
TeleopPeriodic();
System.Threading.Thread.Sleep(kLoopTimeMs);
if (IsConnected())
{
Watchdog.Feed();
}
}
}
}
}
public static void Main()
{
/* loop forever */
while (true)
{
/* keep feeding watchdog to enable motors */
Watchdog.Feed();
Drive();
Thread.Sleep(20);
}
}
/** spin in this routine forever */
public void RunForever()
{
SetupConfig(); /* configuration */
/* 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() == UsbDeviceConnection.Connected)
{
Watchdog.Feed();
}
/* set the control mode based on button pressed */
if (btnTopLeftShoulder)
{
_mode = ControlMode.PercentOutput;
}
if (btnBtmLeftShoulder)
{
_mode = ControlMode.MotionMagic;
}
/* calc the Talon output based on mode */
if (_mode == ControlMode.PercentOutput)
{
float output = leftY; // [-1, +1] percent duty cycle
_talon.Set(_mode, output);
}
else if (_mode == ControlMode.MotionMagic)
{
float servoToRotation = leftY * 10;// [-10, +10] rotations
_talon.Set(_mode, servoToRotation);
}
/* instrumentation */
Instrument.Process(_talon);
/* wait a bit */
System.Threading.Thread.Sleep(5);
}
}
public static void Main()
{
/* Factory Default all hardware to prevent unexpected behaviour */
leftFrnt.ConfigFactoryDefault();
leftRear.ConfigFactoryDefault();
rghtFrnt.ConfigFactoryDefault();
rghtRear.ConfigFactoryDefault();
/* loop forever */
while (true)
{
/* keep feeding watchdog to enable motors */
Watchdog.Feed();
Drive();
Thread.Sleep(20);
}
}
public void RunForever()
{
/* Pick your motor controller ID */
const UInt32 deviceNumber = 23; //must be 0 - 62
/* This is the frame which is used for percent output. */
const UInt32 CONTROL = 0x040080;
while (true)
{
/* get gamepad/joystick stick value, which is within [-1,+1] */
float gamepadValue = _gamepad.GetAxis(0);
/* converts gamepad value [-1,+1] to [-1023,+1023].
* Talon/Victor takes a demand value where 1023 is full, 0 is neutral,
* anything in the middle is partial output. This is how PercentOutput mode works. */
float outputAsFloat = gamepadValue * 1023;
/* convert the output [-1023,+1023] from floating point to int.
* Basically throw away the fractional part. */
int outputAsInt = (int)outputAsFloat;
/* encode output into bytes */
byte first_byte = (byte)(outputAsInt >> 0x10);
byte second_byte = (byte)(outputAsInt >> 0x08);
byte third_byte = (byte)(outputAsInt);
/* build the control CANbus frame. The first three bytes is the demand value,
* which typically is the output value [-1023,+1023] */
ulong frame = 0;
/*
* This assumes we want PercentOutput mode. See Phoenix netmf repository for full implementation (https://github.com/CrossTheRoadElec).
*/
frame |= (UInt64)(first_byte);
frame |= (UInt64)(second_byte) << 0x08;
frame |= (UInt64)(third_byte) << 0x10;
/* transmit the CAN bus frame once per loop,
* if you stop sending this then Talon/Victor will disable (blink orange) all the time.
*/
CTRE.Native.CAN.Send(CONTROL | deviceNumber | 0x02040000, frame, 8, 0); /* use 0x01040000 for Victor SPX */
/*
* CTRE Motor Controllers also need a global enable frame.
* We will let HERO send this automatically by calling Feed().
* On other platforms you must transmit the (nonFRC) enable frame...
* - ArbID is $401BF
* - first data byte is 0 for disable, 1 for enable,
* - other bytes are zero, length = 8;
* - use 29bit ID
*
* For safety reasons, only send the enable frame if Gamepad is plugged into HERO (D-mode).
*/
if (_gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected) /* is gamepad USB connected? */
{
Watchdog.Feed(); //watch dog times out after 100 ms
}
/* wait 10 milliseconds and do it all over again */
Thread.Sleep(10);
}
}
private void OnChanged(object sender, FileSystemEventArgs e)
{
_dog.Feed();
}
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). */
UsbHostDevice.GetInstance(0).SetSelectableXInputFilter(UsbHostDevice.SelectableXInputFilter.XInputDevices);
while (true)
{
if (_gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
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(ControlMode.PercentOutput, (_sticks[5] - _sticks[4]) * 0.60f);
/* fire some solenoids based on buttons */
_driver.Set(0, _buttons[1]);
_driver.Set(1, _buttons[2]);
_driver.Set(2, _buttons[3]);
_driver.Set(3, _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;
}
}
public static void Main()
{
TeleopControl Telecontrol = new TeleopControl();
AutonControl Autocontrol = new AutonControl();
int AutonLoops = LoopSec * AutonTime;
GameController controller = Telecontrol.GetController();
bool okToRun = false; // Can we run code or not
while (true) // loop forever
{
// If the USB controller isn't plugged in or if the stop button is pressed
// it isn't ok to run the motors.
// If the USB is plugged in, use the start button to activate (it will stay
// that way unless the stop button is pressed or the USB becomes dislodged.
if (controller.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
if (controller.GetButton(Telecontrol.GetStartButton()))
{
okToRun = true;
}
else if (controller.GetButton(Telecontrol.GetStopButton()))
{
okToRun = false;
}
}
else
{
okToRun = false;
}
if (okToRun)
{
Watchdog.Feed();
bool runauton = controller.GetButton(Telecontrol.GetAutonButton());
if ((runauton || runningAuton) && AutonLoopCount < AutonLoops)
{
runningAuton = !Autocontrol.Run();
ranAuto = true;
AutonLoopCount++;
}
else if (ranAuto && AutonLoopCount < AutonLoops)
{
Deliver del = Deliver.GetInstance();
del.Run();
AutonLoopCount++;
}
else
{
Telecontrol.Run();
}
Thread.Sleep(20);
}
else
{
Telecontrol.CheckSensors();
}
}
}
public static void Main()
{
var casterDrive = new CasterDrive();
// uncomment this if you need to zero the swerves
/*while (true)
* {
* Debug.Print("Angle: " + casterDrive.casters[0].Angle + "\t" + casterDrive.casters[1].Angle + "\t" + casterDrive.casters[2].Angle);
* }*/
var cannon = new Cannon();
// TODO: Move controller stuff to operator input class
var controller = new GameController(UsbHostDevice.GetInstance());
Debug.Print("Program started");
var controllerValues = new GameControllerValues();
var stopwatch = new Stopwatch();
var watchdogListener = new Listener(false);
var shootModeToggler = new Toggler(true);
var shootListener = new Listener(true);
var zeroListener = new Listener(false);
while (true)
{
double dt = stopwatch.Duration;
stopwatch.Start();
controller.GetAllValues(ref controllerValues);
bool isEnabled = controller.GetButton(5);
if (isEnabled && controller.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
Watchdog.Feed();
}
double turn = -controller.GetAxis(2);
casterDrive.Drive(new Vector2(-controller.GetAxis(0), controller.GetAxis(1)), turn);
bool shootMode = shootModeToggler.Get(isEnabled && controller.GetButton(1));
if (zeroListener.Get(controller.GetButton(3)))
{
Debug.Print("Zeroing Gyro");
casterDrive.ZeroGyro();
}
bool firing = isEnabled && shootListener.Get(controller.GetButton(6));
if (watchdogListener.Get(Watchdog.IsEnabled()))
{
cannon.Setpoint = cannon.Angle;
}
double adjust;
if (controllerValues.pov == 0)
{
adjust = 30.0 * dt;
}
else if (controllerValues.pov == 4)
{
adjust = -30.0 * dt;
}
else
{
adjust = 0.0;
}
cannon.Update(shootMode, firing, adjust);
//Debug.Print(controllerValues.pov +"");
//Debug.Print(dt +"");
//cannon.DebugPID();
/*
* leftMax = Math.Max(leftMax, casterDrive.casters[0].TurnCurrent);
* rightMax = Math.Max(rightMax, casterDrive.casters[1].TurnCurrent);
* backMax = Math.Max(backMax, casterDrive.casters[2].TurnCurrent);
* Debug.Print("Left: " + leftMax + "\tRight: " + rightMax + "\tBack: " + backMax);
*/
}
}
static void Main(string[] args)
{
Hardware.I2C.OpenBus("/dev/i2c-2");
// var dog = new Watchdog();
//ThreadStart doggy = dog.Watch;
//hread watchdog = new Thread(doggy);
//watchdog.Start();
var dog = new Watchdog();
dog.Feed();
TcpListener server = new TcpListener(IPAddress.Any, 2543);
server.Start();
Console.WriteLine("Server started!");
Hardware.Leds.SetFrontRight(
63, 0, 0
);
Hardware.Leds.SetFrontLeft(
0, 63, 0
);
Hardware.Leds.SetBack(
0, 0, 63
);
while (true)
{
var client = server.AcceptTcpClient();
var stream = client.GetStream();
//var buffer = new byte[1000];
while (client.Connected)
{
var buffer = new byte[1024];
stream.Read(buffer, 0, buffer.Length);
//decode packet
var packet = JsonConvert.DeserializeObject <Packet>(Encoding.ASCII.GetString(buffer));
// Console.WriteLine($"Opcode: {packet.Opcode}");
stream.Flush();
switch (packet.Opcode)
{
case Opcodes.MotorSetSpeed:
var motorSpeed = JsonConvert.DeserializeObject <Registers.MotorSpeed>(JsonConvert.SerializeObject(packet.Data));
/*Console.WriteLine("--------------------");
* Console.WriteLine("Setting motor speeds");
* Console.WriteLine($"Left: {motorSpeed.Left}");
* Console.WriteLine($"Right: {motorSpeed.Right}");*/
Hardware.Motors.SetSpeed(motorSpeed.Left, motorSpeed.Right);
dog.Feed();
break;
case Opcodes.SetLeds:
var leds = JsonConvert.DeserializeObject <Registers.Leds>(JsonConvert.SerializeObject(packet.Data));
/*Console.WriteLine("------------");
* Console.WriteLine("Setting leds");
* Console.WriteLine($"Back R:{leds.Back.Red} G:{leds.Back.Green} B:{leds.Back.Red}");
* Console.WriteLine($"FrontR R:{leds.FrontLeft.Red} G:{leds.FrontLeft.Green} B:{leds.FrontLeft.Blue}");
* Console.WriteLine($"FrontR R:{leds.FrontRight.Red} G:{leds.FrontRight.Green} B:{leds.FrontRight.Red}");
*/
Hardware.Leds.SetFrontRight(
leds.FrontRight.Red,
leds.FrontRight.Green,
leds.FrontRight.Blue
);
Hardware.Leds.SetFrontLeft(
leds.FrontLeft.Red,
leds.FrontLeft.Green,
leds.FrontLeft.Blue
);
Hardware.Leds.SetBack(
leds.Back.Red,
leds.Back.Green,
leds.Back.Blue
);
break;
case Opcodes.IrProximity:
Hardware.Infrared.Proximity.ReloadValues();
var json = JsonConvert.SerializeObject(
new Registers.Infrared(
Hardware.Infrared.Proximity.BackLeft,
Hardware.Infrared.Proximity.Left,
Hardware.Infrared.Proximity.FrontLeft,
Hardware.Infrared.Proximity.Front,
Hardware.Infrared.Proximity.FrontRight,
Hardware.Infrared.Proximity.Right,
Hardware.Infrared.Proximity.BackRight,
Hardware.Infrared.Proximity.Back,
Hardware.Infrared.Proximity.GroundLeft,
Hardware.Infrared.Proximity.GroundFrontLeft,
Hardware.Infrared.Proximity.GroundFrontRight,
Hardware.Infrared.Proximity.GroundRight
));
stream.Write(Encoding.ASCII.GetBytes(json), 0, json.Length);
break;
}
client.Close();
}
}
}
public static void Main()
{
/* Hardware */
TalonSRX _talon = new TalonSRX(1);
/** Use a USB gamepad plugged into the HERO */
GameController _gamepad = new GameController(UsbHostDevice.GetInstance());
/* String for output */
StringBuilder _sb = new StringBuilder();
/** hold bottom left shoulder button to enable motors */
const uint kEnableButton = 7;
/* Loop tracker for prints */
int _loops = 0;
/* Initialization */
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* Config sensor used for Primary PID [Velocity] */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
Constants.kPIDLoopIdx,
Constants.kTimeoutMs);
/**
* Phase sensor accordingly.
* Positive Sensor Reading should match Green (blinking) Leds on Talon
*/
_talon.SetSensorPhase(false);
/* Config the peak and nominal outputs */
_talon.ConfigNominalOutputForward(0, Constants.kTimeoutMs);
_talon.ConfigNominalOutputReverse(0, Constants.kTimeoutMs);
_talon.ConfigPeakOutputForward(1, Constants.kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1, Constants.kTimeoutMs);
/* Config the Velocity closed loop gains in slot0 */
_talon.Config_kF(Constants.kPIDLoopIdx, Constants.kF, Constants.kTimeoutMs);
_talon.Config_kP(Constants.kPIDLoopIdx, Constants.kP, Constants.kTimeoutMs);
_talon.Config_kI(Constants.kPIDLoopIdx, Constants.kI, Constants.kTimeoutMs);
_talon.Config_kD(Constants.kPIDLoopIdx, Constants.kD, Constants.kTimeoutMs);
/* loop forever */
while (true)
{
/* Get gamepad axis */
double leftYstick = -1 * _gamepad.GetAxis(1);
/* Get Talon/Victor's current output percentage */
double motorOutput = _talon.GetMotorOutputPercent();
/* Prepare line to print */
_sb.Append("\tout:");
/* Cast to int to remove decimal places */
_sb.Append((int)(motorOutput * 100));
_sb.Append("%"); // Percent
_sb.Append("\tspd:");
_sb.Append(_talon.GetSelectedSensorVelocity(Constants.kPIDLoopIdx));
_sb.Append("u"); // Native units
/**
* When button 1 is held, start and run Velocity Closed loop.
* Velocity Closed Loop is controlled by joystick position x2000 RPM, [-2000, 2000] RPM
*/
if (_gamepad.GetButton(1))
{
/* Velocity Closed Loop */
/**
* Convert 2000 RPM to units / 100ms.
* 4096 Units/Rev * 2000 RPM / 600 100ms/min in either direction:
* velocity setpoint is in units/100ms
*/
double targetVelocity_UnitsPer100ms = leftYstick * 2000.0 * 4096 / 600;
/* 2000 RPM in either direction */
_talon.Set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);
/* Append more signals to print when in speed mode. */
_sb.Append("\terr:");
_sb.Append(_talon.GetClosedLoopError(Constants.kPIDLoopIdx));
_sb.Append("\ttrg:");
_sb.Append(targetVelocity_UnitsPer100ms);
}
else
{
/* Percent Output */
_talon.Set(ControlMode.PercentOutput, leftYstick);
}
/* Print built string every 10 loops */
if (++_loops >= 10)
{
_loops = 0;
Debug.Print(_sb.ToString());
}
/* Reset built string */
_sb.Clear();
//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*/
Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(20);
}
}