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