static void Drive() { if (null == _gamepad) _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); float x = _gamepad.GetAxis(0); // Positive is strafe-right, negative is strafe-left float y = -1 * _gamepad.GetAxis(1); // Positive is forward, negative is reverse float turn = _gamepad.GetAxis(2); // Positive is turn-right, negative is turn-left Deadband(ref x); Deadband(ref y); Deadband(ref turn); float leftFrnt_throt = y + x + turn; // left front moves positive for forward, strafe-right, turn-right float leftRear_throt = y - x + turn; // left rear moves positive for forward, strafe-left, turn-right float rghtFrnt_throt = y - x - turn; // right front moves positive for forward, strafe-left, turn-left float rghtRear_throt = y + x - turn; // right rear moves positive for forward, strafe-right, turn-left /* normalize here, there a many way to accomplish this, this is a simple solution */ Normalize(ref leftFrnt_throt); Normalize(ref leftRear_throt); Normalize(ref rghtFrnt_throt); Normalize(ref rghtRear_throt); /* everything up until this point assumes positive spins motor so that robot moves forward. But typically one side of the robot has to drive negative (red LED) to move robor forward. Assuming the left-side has to be negative to move robot forward, flip the left side */ leftFrnt_throt *= -1; leftRear_throt *= -1; leftFrnt.Set(leftFrnt_throt); leftRear.Set(leftRear_throt); rghtFrnt.Set(rghtFrnt_throt); rghtRear.Set(rghtRear_throt); }
public static void Main() { //Gamepad for input CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); //Create the DriverModule object by giving it the port you plugged it in to. CTRE.HERO.Module.DriverModule driver = new CTRE.HERO.Module.DriverModule(CTRE.HERO.IO.Port5); //these just act as shorter names for the driveLow and pullUp states bool driveLow = CTRE.HERO.Module.DriverModule.OutputState.driveLow; bool pullUp = CTRE.HERO.Module.DriverModule.OutputState.pullUp; while (true) { //When the 'X' button is pressed, enable outputs if (_gamepad.GetButton(1) == true) { driver.Set(1, driveLow); driver.Set(2, driveLow); } else { driver.Set(1, pullUp); driver.Set(2, pullUp); } System.Threading.Thread.Sleep(10); } }
public static void Main() { /* create a gamepad object */ CTRE.Gamepad myGamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice()); /* create a talon, the Talon Device ID in HERO LifeBoat is zero */ CTRE.TalonSrx myTalon = new CTRE.TalonSrx(0); /* simple counter to print and watch using the debugger */ int counter = 0; /* loop forever */ while (true) { /* added inside the while loop */ if (myGamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { /* print the axis value */ Debug.Print("axis:" + myGamepad.GetAxis(1)); /* pass axis value to talon */ myTalon.Set(myGamepad.GetAxis(1)); /* allow motor control */ CTRE.Watchdog.Feed(); } /* increment counter */ ++counter; /* try to land a breakpoint here */ /* wait a bit */ System.Threading.Thread.Sleep(10); } }
static void Drive() { if (null == _gamepad) { _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); } float x = _gamepad.GetAxis(0); float y = -1 * _gamepad.GetAxis(1); float twist = _gamepad.GetAxis(2); Deadband(ref x); Deadband(ref y); Deadband(ref twist); float leftThrot = y + twist; float rightThrot = y - twist; left.Set(leftThrot); leftSlave.Set(leftThrot); right.Set(-rightThrot); rightSlave.Set(-rightThrot); stringBuilder.Append("\t"); stringBuilder.Append(x); stringBuilder.Append("\t"); stringBuilder.Append(y); stringBuilder.Append("\t"); stringBuilder.Append(twist); }
public static void Main() { leftDrive = new CTRE.TalonSrx(1); rightDrive = new CTRE.TalonSrx(2); leftDrive.SetInverted(true); leftDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); rightDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); leftDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); rightDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); leftDrive.ConfigEncoderCodesPerRev(560); rightDrive.ConfigEncoderCodesPerRev(560); leftDrive.SetSensorDirection(true); leftDrive.SetEncPosition(0); rightDrive.SetEncPosition(0); CTRE.Gamepad gamePad = new CTRE.Gamepad(new CTRE.UsbHostDevice()); /* loop forever */ while (true) { if (gamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { CheesyDrive(gamePad.GetAxis(1), gamePad.GetAxis(2)); CTRE.Watchdog.Feed(); Debug.Print("" + leftDrive.GetPosition() + ":" + rightDrive.GetPosition()); } else { Debug.Print("No Driver Pad"); } /* wait a bit */ System.Threading.Thread.Sleep(10); } }
public static void Main() { //Gamepad for input CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); //Create the DriverModule object by giving it the port you plugged it in to. CTRE.HERO.Module.DriverModule driver = new CTRE.HERO.Module.DriverModule(CTRE.HERO.IO.Port5); //these just act as shorter names for the driveLow and pullUp states bool driveLow = CTRE.HERO.Module.DriverModule.OutputState.driveLow; bool pullUp = CTRE.HERO.Module.DriverModule.OutputState.pullUp; while (true) { //When the 'X' button is pressed, enable outputs if (_gamepad.GetButton(1) == true) { driver.Set(1, driveLow); driver.Set(2, driveLow); } else { driver.Set(1, pullUp); driver.Set(2, pullUp); } System.Threading.Thread.Sleep(10); } }
static void Drive() { if (null == _gamepad) _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); float x = _gamepad.GetAxis(0); float y = -1 * _gamepad.GetAxis(1); float twist = _gamepad.GetAxis(2); Deadband(ref x); Deadband(ref y); Deadband(ref twist); float leftThrot = y + twist; float rightThrot = y - twist; left.Set(leftThrot); leftSlave.Set(leftThrot); right.Set(-rightThrot); rightSlave.Set(-rightThrot); stringBuilder.Append("\t"); stringBuilder.Append(x); stringBuilder.Append("\t"); stringBuilder.Append(y); stringBuilder.Append("\t"); stringBuilder.Append(twist); }
/** get all buttons from a gamepad */ public static void FillBtns(CTRE.Gamepad gamepad, ref bool[] btns, ref int pov) { gamepad.GetAllValues(ref _gamepadValues); pov = _gamepadValues.pov; for (uint i = 1; i < btns.Length; ++i) { btns[i] = gamepad.GetButton(i); } }
public int GetFirstButton(CTRE.Gamepad gamepad) { for (uint i = 0; i < 16; ++i) { if (gamepad.GetButton(i)) { return((int)i); } } return(-1); }
public static void Main() { CTRE.Gamepad _gamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice()); //0 - left x //1 - left y //2 - right x // //5 - LB //6 - RB ledSpike.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage); ledSpike.SetCurrentLimit(1); while (true) { float y = -_gamepad.GetAxis(1); float turn = _gamepad.GetAxis(2); Deadband(ref y); Deadband(ref turn); if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { if (_gamepad.GetButton(5)) { ArcadeDrive(y, turn); /* feed watchdog to keep Talon's enabled */ CTRE.Watchdog.Feed(); } if (_gamepad.GetButton(6)) { ledSpike.Set(3.5f); CTRE.Watchdog.Feed(); } else { ledSpike.Set(0); } } /* run this task every 10ms */ Thread.Sleep(10); } }
public static void Main() { //Gamepad for input CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); //simple PWM for fine control of pulse width, period, timing... uint period = 50000; //period between pulses uint duration = 1500; //duration of pulse PWM pwm_9 = new PWM(CTRE.HERO.IO.Port3.PWM_Pin9, period, duration, PWM.ScaleFactor.Microseconds, false); pwm_9.Start(); //starts the signal // ...and just a PWM SpeedController for motor controller (Victor SP, Talon SR, Victor 888, etc.)... CTRE.PWMSpeedController pwmSpeedController = new CTRE.PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin4); while (true) { /* only enable motor control (PWM/CAN) if gamepad is connected. Logitech gamepads may be disabled using the X/D switch */ if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { CTRE.Watchdog.Feed(); } /* let axis control the pwm speed controller */ pwmSpeedController.Set(0.10f); /* 10% */ /* let button1 control the explicit PWM pin duration*/ if (_gamepad.GetButton(1) == true) { pwm_9.Duration = 2000; /* 2.0ms */ } else { pwm_9.Duration = 1000; /* 1.0ms */ } /* yield for a bit, this controls this task's frequency */ System.Threading.Thread.Sleep(10); } }
static void Drive() { if (null == _gamepad) { _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); } float x = _gamepad.GetAxis(0); // Positive is strafe-right, negative is strafe-left float y = -1 * _gamepad.GetAxis(1); // Positive is forward, negative is reverse float turn = _gamepad.GetAxis(2); // Positive is turn-right, negative is turn-left Deadband(ref x); Deadband(ref y); Deadband(ref turn); float leftFrnt_throt = y + x + turn; // left front moves positive for forward, strafe-right, turn-right float leftRear_throt = y - x + turn; // left rear moves positive for forward, strafe-left, turn-right float rghtFrnt_throt = y - x - turn; // right front moves positive for forward, strafe-left, turn-left float rghtRear_throt = y + x - turn; // right rear moves positive for forward, strafe-right, turn-left /* normalize here, there a many way to accomplish this, this is a simple solution */ Normalize(ref leftFrnt_throt); Normalize(ref leftRear_throt); Normalize(ref rghtFrnt_throt); Normalize(ref rghtRear_throt); /* everything up until this point assumes positive spins motor so that robot moves forward. * But typically one side of the robot has to drive negative (red LED) to move robor forward. * Assuming the left-side has to be negative to move robot forward, flip the left side */ leftFrnt_throt *= -1; leftRear_throt *= -1; leftFrnt.Set(leftFrnt_throt); leftRear.Set(leftRear_throt); rghtFrnt.Set(rghtFrnt_throt); rghtRear.Set(rghtRear_throt); }
public void RunForever() { int toPrint = 0; int loops = 0; CTRE.TalonSrx _talon = new CTRE.TalonSrx(0); /* make a talon with deviceId 0 */ CTRE.PigeonImu _pigeon; /** Use a USB gamepad plugged into the HERO */ CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); CTRE.GamepadValues _gamepadValues = new CTRE.GamepadValues(); uint _oldBtns = 0; StringBuilder _sb = new StringBuilder(); /** choose pigeon connected to Talon or on CAN Bus */ if (true) { _pigeon = new CTRE.PigeonImu(0); /* Pigeon is on CAN Bus with device ID 0 */ } else { _pigeon = new CTRE.PigeonImu(_talon); /* Pigeon is connected to _talon via Gadgeteer cable.*/ } /* set frame rates */ _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.BiasedStatus_4_Mag, 17); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_1_General, 217); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_11_GyroAccum, 9999); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_9_SixDeg_YPR, 0); _pigeon.SetAccumZAngle(217); /* loop forever */ while (true) { int ret = 0; /* signals for motion */ float[] ypr = new float[3]; float[] wxyz = new float[4]; float compassHeading, magnitudeMicroTeslas; float tempC; CTRE.PigeonImu.PigeonState state; Int16[] rmxyz = new Int16[3]; Int16[] bmxyz = new Int16[3]; Int16[] baxyz = new short[3]; Int16 baNorm; float[] xyz_dps = new float[3]; float [] tilts_XZ_YZ_XY = new float[3]; UInt32 timeSec; float fusedHeading; CTRE.PigeonImu.FusionStatus fusionStatus = new CTRE.PigeonImu.FusionStatus(); CTRE.PigeonImu.GeneralStatus generalStatus = new CTRE.PigeonImu.GeneralStatus(); /* get sigs */ ret |= _pigeon.GetYawPitchRoll(ypr); ret |= _pigeon.Get6dQuaternion(wxyz); compassHeading = _pigeon.GetCompassHeading(); ret |= _pigeon.GetLastError(); magnitudeMicroTeslas = _pigeon.GetCompassFieldStrength(); ret |= _pigeon.GetLastError(); tempC = _pigeon.GetTemp(); ret |= _pigeon.GetLastError(); state = _pigeon.GetState(); ret |= _pigeon.GetLastError(); ret |= _pigeon.GetRawMagnetometer(rmxyz); ret |= _pigeon.GetBiasedMagnetometer(bmxyz); ret |= _pigeon.GetBiasedAccelerometer(baxyz, out baNorm); ret |= _pigeon.GetRawGyro(xyz_dps); ret |= _pigeon.GetAccelerometerAngles(tilts_XZ_YZ_XY); timeSec = _pigeon.GetUpTime(); fusedHeading = _pigeon.GetFusedHeading(fusionStatus); ret |= _pigeon.GetGeneralStatus(generalStatus); /* get buttons */ _gamepad.GetAllValues(ref _gamepadValues); if (_gamepadValues.pov == 0) // up { _pigeon.EnableTemperatureCompensation(true); } if (_gamepadValues.pov == 4) // dn { _pigeon.EnableTemperatureCompensation(false); } if (_oldBtns != _gamepadValues.btns) { int btnIdx = GetButtonFromBits(_gamepadValues.btns); switch (btnIdx) { case 4: if (toPrint < kToPrintMaxInclus) { ++toPrint; } break; case 2: if (toPrint > kToPrintMin) { --toPrint; } break; case 5: // top left _pigeon.SetYaw(100); _pigeon.SetFusedHeading(100); break; case 7: // btm left break; case 6: // top right _pigeon.SetYawToCompass(); _pigeon.SetFusedHeadingToCompass(); break; case 8: // btm right _pigeon.AddYaw(45); _pigeon.AddYaw(45); break; case 10: // enter mag cal start btn _pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature); break; case 9: // accel 0,0, _pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature); break; } _oldBtns = _gamepadValues.btns; } if (++loops >= 20) { loops = 0; /* build row */ switch (toPrint) { case 0: _sb.Append("YPR:"); _sb.Append(ToString(ypr)); break; case 1: _sb.Append("FusedHeading:"); _sb.Append(fusedHeading); _sb.Append(" deg, "); _sb.Append(fusionStatus.description); break; case 2: _sb.Append("Compass:"******" deg, "); _sb.Append(magnitudeMicroTeslas); _sb.Append("uT"); break; case 3: _sb.Append("tempC:"); _sb.Append(tempC); _sb.AppendLine(); break; case 4: _sb.Append("PigeonState:"); _sb.Append(state); _sb.AppendLine(); break; case 5: _sb.Append("BiasedM:"); _sb.Append(ToString(bmxyz)); break; case 6: _sb.Append("RawM:"); _sb.Append(ToString(rmxyz)); break; case 7: _sb.Append("BiasedAccel:"); _sb.Append(ToString(baxyz)); break; case 8: _sb.Append("GyroDps:"); _sb.Append(ToString(xyz_dps)); break; case 9: _sb.Append("AccelTilts:"); _sb.Append(ToString(tilts_XZ_YZ_XY)); break; case 10: _sb.Append("Quat:"); _sb.Append(ToString(wxyz)); break; case 11: _sb.Append("UpTime:"); _sb.Append(timeSec); break; case 12: _sb.Append("GS:"); _sb.Append(generalStatus.description); break; case 13: _sb.Append("tempCompensationCount:"); _sb.Append(generalStatus.tempCompensationCount); _sb.Append(" tempC:"); _sb.Append(generalStatus.tempC); _sb.Append(" noMotionBiasCount:"); _sb.Append(generalStatus.noMotionBiasCount); break; } kToPrintMin = 0; kToPrintMaxInclus = 13; //_sb.Append(yaw); _sb.Append(","); _sb.Append(pitch); _sb.Append(","); _sb.Append(roll); _sb.AppendLine(); //_sb.Append(w); _sb.Append(","); _sb.Append(x); _sb.Append(","); _sb.Append(y); _sb.Append(","); _sb.Append(z); _sb.AppendLine(); //_sb.Append(compassHeading); _sb.Append(","); _sb.Append(magnitudeMicroTeslas); _sb.AppendLine(); //_sb.Append(tempC); _sb.AppendLine(); //_sb.Append(state); _sb.AppendLine(); //_sb.Append(rmx); _sb.Append(","); _sb.Append(rmy); _sb.Append(","); _sb.Append(rmz); _sb.AppendLine(); //_sb.Append(bmx); _sb.Append(","); _sb.Append(bmy); _sb.Append(","); _sb.Append(bmz); _sb.AppendLine(); //_sb.Append(bam); _sb.Append(","); _sb.Append(bay); _sb.Append(","); _sb.Append(baz); _sb.Append(","); _sb.Append(baNorm); _sb.AppendLine(); //_sb.Append(x_dps); _sb.Append(","); _sb.Append(y_dps); _sb.Append(","); _sb.Append(z_dps); _sb.AppendLine(); //_sb.Append(tiltXZ); _sb.Append(","); _sb.Append(tiltYZ); _sb.Append(","); _sb.Append(tiltXY); _sb.AppendLine(); //_sb.Append(timeSec); _sb.Append(","); //_sb.Append(fusedHeading); _sb.Append(","); _sb.Append(bIsValid); _sb.Append(","); _sb.Append(bIsFusing); _sb.AppendLine(); //_sb.Append(ret); _sb.Append(","); } /* print line and clear buffer */ if (_sb.Length != 0) { Debug.Print(_sb.ToString()); _sb.Clear(); } /* 10ms loop */ Thread.Sleep(10); if (_pigeon.HasResetOccured()) { int resetCount; int resetFlags; int firmVers; resetCount = _pigeon.GetResetCount(); resetFlags = _pigeon.GetResetFlags(); firmVers = _pigeon.GetFirmVers(); Debug.Print("Pigeon Reset detected..."); Debug.Print(" Pigeon Reset Count:" + resetCount.ToString("D")); Debug.Print(" Pigeon Reset Flags:" + resetFlags.ToString("X")); Debug.Print(" Pigeon FirmVers:" + firmVers.ToString("X")); Debug.Print(" Waiting for delay"); Thread.Sleep(3000); } } }
public OI(CTRE.Gamepad controller) { this.controller = controller; }