public TeleopStateMachine(Intake intake_, Conveyor conveyor_, Climber climber_, GameController gamepad_)
{
intake = intake_;
conveyor = conveyor_;
climber = climber_;
gamepad = gamepad_;
}
static void Drive()
{
if (null == _gamepad)
{
_gamepad = new GameController(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(ControlMode.PercentOutput, leftThrot);
//leftSlave.Set(ControlMode.PercentOutput, leftThrot);
right.Set(ControlMode.PercentOutput, -rightThrot);
//rightSlave.Set(ControlMode.PercentOutput, -rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}
public static Button(uint id, dummy execute, dummy end) // to assign to button, cannot take or return values.
{
buttonID = id;
toDo = execute;
onStop = end;
if (controller == null)
{
controller = new GameController(UsbHostDevice.GetInstance());
} // Create controller.
}
public static void Main()
{
/* loop forever */
while (true)
{
if (null == _gamepad)
{
_gamepad = new GameController(new CTRE.Phoenix.UsbHostDevice(0));
}
// This is the code that keeps the robot safe, if we loose the controller, "DON'T FEED THE DOG"
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
/* feed watchdog to keep Talon's enabled */
CTRE.Phoenix.Watchdog.Feed();
/* drive robot using gamepad */
Drive();
}
/* print whatever is in our string builder */
Debug.Print(stringBuilder.ToString());
stringBuilder.Clear();
/* run this task every 20ms */
Thread.Sleep(20);
}
}
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 LogitechWirelessGamepad(uint usbPortNumber)
{
gamepad = new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(usbPortNumber));
}