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); } }