/**
* Interface for anything that provides gamepad/joystick values (could be from a host pc or from USB attached gamepad).
* @return Negative If values could not be retrieved or values are very old. toFill is cleared.
* Positive if values are updated. toFill is filled in.
*/
private int SyncGet(ref GameControllerValues toFill, uint numDwords)
{
/* always get latest data for now */
_updateCnt = 0;
//TODO: Implement CTRE.Phoenix.Controller.GetCANJoy()
int ret = GetCANJoy(ref _updateCnt, _gpInts, (uint)numDwords, _gpFlts, (uint)_gpFlts.Length);
if (ret < 0)
{
/* negative error code means data is unreliable */
if (toFill != null)
{
toFill.Copy(ZeroGameControllerValues);
}
/* on the next poll, always get latest */
_updateCnt = 0;
}
else
{
/* new data, copy it over */
if (toFill != null)
{
toFill.axes[0] = _gpFlts[0];
toFill.axes[1] = _gpFlts[1];
toFill.axes[2] = _gpFlts[2];
toFill.axes[3] = _gpFlts[3];
toFill.axes[4] = _gpFlts[4];
toFill.axes[5] = _gpFlts[5];
toFill.btns = (uint)_gpInts[0];
toFill.pov = _gpInts[1];
}
}
return(ret);
}
/**
* Retrieves a copy of the internal gamepadvalues structure used in decoding signals.
* This can be used to retrieve signals not readily available through the Gamepad API (such as vendor specific signals or VID/PID).
* To use this function, first create a gamepadValues instance and pass by reference.
* <pre>{@code
* GamepadValues values = new GamepadValues(); // Create only once and use functiont to update it periodically.
* ...
* gamepad.GetAllValues(gamepadValues); // Get latest values
* }</pre>
* @param gamepadValues reference to update with latest values.
* @return object reference to gamepadValues for function chaining.
*/
public GameControllerValues GetAllValues(ref GameControllerValues gamepadValues)
{
/* get latest copy if there is new data */
_provider.Get(ref _values, _index);
/* copy it to caller */
gamepadValues.Copy(_values);
return(gamepadValues);
}
public int Get(ref GameControllerValues toFill, uint idx)
{
/* set the device mask bits, these are global */
//_gpInts[16] = (int)_maskBits;
const uint numDwords = 2; /* only send params to be read */
return(SyncGet(ref toFill, numDwords));
}
public int Sync(ref GameControllerValues toFill, uint rumbleL, uint rumbleR, uint ledCode, uint controlFlags, uint idx)
{
/* This doesn't set any values right now.
* Need to implement control frame from
* HERO --> RIO if we want to use these.
**/
/* save commands */
//_gpInts[20] = (int)rumbleL;
//_gpInts[21] = (int)rumbleR;
//_gpInts[22] = (int)ledCode;
//_gpInts[23] = (int)controlFlags;
/* set the device mask bits, these are global */
//_gpInts[16] = (int)_maskBits;
return(SyncGet(ref toFill, (uint)_gpInts.Length));
}
public void Copy(GameControllerValues rhs)
{
axes[0] = rhs.axes[0];
axes[1] = rhs.axes[1];
axes[2] = rhs.axes[2];
axes[3] = rhs.axes[3];
axes[4] = rhs.axes[4];
axes[5] = rhs.axes[5];
btns = rhs.btns;
//btnChanges = rhs.btnChanges;
//btnsLast = rhs.btnsLast;
pov = rhs.pov;
vid = rhs.vid;
pid = rhs.pid;
vendorSpecI = rhs.vendorSpecI;
vendorSpecF = rhs.vendorSpecF;
flagBits = rhs.flagBits;
/* leave commands alone */
}
public GameControllerValues(GameControllerValues rhs)
{
Copy(rhs);
}
public static void Main()
{
const int zmove = 0; //Forwards Backwards
const int xmove = 1; //Left Right
const int zxmove = 2; // All directions
const int stoppls = 4;
int mode = zmove;
int auton = 1;
const int mechTalonid1 = 668;
const int mechTalonid2 = 25;
//const int meterTime = 100000000;
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(0);
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon2 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(25);
// CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon3 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(mechTalonid1);
// CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon4 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(mechTalonid2);
/*
* double read0;
* double read1;
* double read2;
*/
/* create a gamepad object */
CTRE.Phoenix.Controller.GameController myGamepad = new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
CTRE.Phoenix.Controller.GameControllerValues gv = new CTRE.Phoenix.Controller.GameControllerValues();
/* loop forever */
//AUTON!!!!
var startTime = DateTime.UtcNow;
if (auton == 1)
{
startTime = DateTime.Now;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(20500000 * 77 / 100))
{
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1);
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("Test: ");
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(10000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(7000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("turn");
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(28000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1);
Debug.Print("fwrd");
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(1000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("turn2");
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(1000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
Debug.Print("stopping");
CTRE.Phoenix.Watchdog.Feed();
}
}
while (true)
{
/*
* read0 = analogInput0.Read();
* read1 = analogInput1.Read();
* read2 = analogInput2.Read();
*/
//Debug.Print("bruh");
myGamepad.GetAllValues(ref gv);
if (myGamepad.GetButton(1) == true)
{
mode = zmove; //forwardsbackwards
}
else if (myGamepad.GetButton(2) == true)
{
mode = xmove; //leftright
}
else if (myGamepad.GetButton(3) == true)
{
mode = zxmove;
}
else if (myGamepad.GetButton(4) == true)
{
mode = stoppls;
}
float speed = myGamepad.GetAxis(1) * -1;
float turn = myGamepad.GetAxis(0) * 1;
float shoulder = myGamepad.GetAxis(2);
float elbow = myGamepad.GetAxis(5);
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
/* print the axis values */
Debug.Print("axis0 - LSH:" + myGamepad.GetAxis(0)); //Left Stick Horizontal // For movement of the wheels
Debug.Print("axis1 - LSV:" + myGamepad.GetAxis(1)); //Left Stick Vertical //For movement of the wheels
Debug.Print("axis2 - RSH:" + myGamepad.GetAxis(2)); //Right Stick Horizontal //For movement of the Arm
Debug.Print("axis3 - RSV:" + myGamepad.GetAxis(5)); //Right Stick Vertical //For movement of the Arm
if (mode == zmove)
{
Debug.Print("Forward/Reverse Mode");
}
else if (mode == xmove)
{
Debug.Print("Turning Mode");
}
else if (mode == zxmove)
{
Debug.Print("Forward/Reverse + Turning Mode");
}
else if (mode == stoppls)
{
Debug.Print("STOP!!!");
}
/* allow motor control */
if (gv.pov == 1)
{
startTime = DateTime.Now;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(20500000 * 77 / 100))
{
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1);
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("Test: ");
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(10000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(7000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("turn");
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(28000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1);
Debug.Print("fwrd");
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(1000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("turn2");
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(1000000))
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
Debug.Print("stopping");
CTRE.Phoenix.Watchdog.Feed();
}
}
if (gv.pov == 2)
{
Debug.Print("p2");
}
else if (gv.pov == 3)
{
Debug.Print("p3");
}
else if (gv.pov == 4)
{
Debug.Print("p4");
}
else if (gv.pov == 5)
{
Debug.Print("p5");
}
else if (gv.pov == 6)
{
Debug.Print("p6");
}
else if (gv.pov == 7)
{
Debug.Print("p7");
}
else if (gv.pov == 8)
{
Debug.Print("p8");
}
else
{
}
if (mode == zmove)
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, speed * 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, speed * -1);
// myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, shoulder);
// myTalon4.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, elbow);
}
else if (mode == xmove)
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, turn * 1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, turn * 1);
//myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, shoulder);
//myTalon4.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, elbow);
}
else if (mode == zxmove)
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, (speed * 1) + turn);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, (speed * -1) + turn);
//myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, shoulder);
//myTalon4.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, elbow);
}
else if (mode == stoppls)
{
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
//myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
//myTalon4.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
}
}
Thread.Sleep(10);
CTRE.Phoenix.Watchdog.Feed();
}
}
public static void Main()
{
DifferentialDrive drive = new DifferentialDrive(Hardware._leftDrive, Hardware._rightDrive);
Lid lid = new Lid(Hardware._lid, Hardware.lid_limit_switch, Hardware.lid_limit_switch_led);
Shooter shooter = new Shooter();
shooter.startup();
CTRE.Phoenix.Controller.GameControllerValues gv = new CTRE.Phoenix.Controller.GameControllerValues();
bool btn_a_was = false;
bool btn_b_was = false;
Timer tick = new Timer();
tick.reset();
tick.start();
while (true)
{
bool enabled = false;
// only run motors if Gamepad is attached
if (Hardware._gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
enabled = true;
Hardware._display.updateConnected(true);
}
else
{
Hardware._display.updateConnected(false);
}
if (enabled)
{
CTRE.Phoenix.Watchdog.Feed();
}
Hardware._display.updateEnabled(enabled);
// read gamepad data all at once
Hardware._gamepad.GetAllValues(ref gv);
// drive the robot with F710 right side joystick
drive.arcadeDrive(gv.axes[3], gv.axes[2], false);
// work the lid based on F710 POV
int pov = gv.pov;
if ((pov & 0x1) != 0)
{
// POV UP
lid.lidUp();
}
else if ((pov & 0x2) != 0)
{
// POV DOWN
lid.lidDown();
}
else
{
lid.lidStop();
}
// look at BTN
bool btn_a = (gv.btns & 0x1) != 0;
if (btn_a && !btn_a_was)
{
Debug.Print("btn_a hit");
if (!shooter.requestFill(0))
{
Debug.Print("tank 0 not available to fill");
}
}
btn_a_was = btn_a;
bool btn_b = (gv.btns & 0x2) != 0;
if (btn_b && !btn_b_was)
{
Debug.Print("btn_b hit");
if (!shooter.requestFill(1))
{
Debug.Print("tank 1 not available to fill");
}
}
btn_b_was = btn_b;
//
shooter.periodic(enabled);
double v = Hardware.voltage.Read();
Hardware._display.updateVoltage(v);
bool limit = Hardware.lid_limit_switch.Read();
Hardware._display.updateLimitSwitch(limit);
if (tick.hasPeriodPassed(5.0))
{
Debug.Print("tick...");
}
/* wait 10 milliseconds and do it all over again */
Thread.Sleep(10);
}
}