static void Drive()
{
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);
}
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);
}
static void Drive()
{
float y = (float)-0.5 * _gamepad.GetAxis(1);
float twist = (float)0.17 * _gamepad.GetAxis(2);
bool button1 = _gamepad.GetButton(1);
bool button3 = _gamepad.GetButton(3);
bool lt = _gamepad.GetButton(7);
bool rt = _gamepad.GetButton(8);
Deadband(ref y);
Deadband(ref twist);
// Cubic transform for forward/back
y = y * y * y;
// Overdrive if both throttles are set
if (lt && rt && y > 0.0)
{
y = (float)2.6 * y;
twist = (float)0.6 * twist;
}
float leftThrot = -(y + twist);
float rightThrot = -(y - twist);
if (button1)
{
servo_setting = 2300;
}
if (button3)
{
servo_setting = 900; //1250
}
drive_mix(leftThrot, rightThrot);
servo_pwm.Duration = servo_setting;
left_pwm.Duration = left_pwm_setting;
right_pwm.Duration = right_pwm_setting;
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
stringBuilder.Append("\t");
stringBuilder.Append(left_pwm_setting);
stringBuilder.Append("\t");
stringBuilder.Append(right_pwm_setting);
stringBuilder.Append("\t");
stringBuilder.Append(button1);
stringBuilder.Append("\t");
stringBuilder.Append(button3);
stringBuilder.Append("\t");
stringBuilder.Append(servo_setting);
stringBuilder.Append("\t");
stringBuilder.Append(lt);
stringBuilder.Append("\t");
stringBuilder.Append(rt);
}
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 Boolean isDpadUp()
{
return(gamepad.GetAxis(DPAD_Y) == -1);
}
static void Drive()
{
// left and right "stick" values are raw data, "x" and "y" are Deadband values
float leftstick = 0.0F;
float rightstick = 0.0F;
// These are variables for the calcualted power levels sent to MotorControllers.
float leftThrottle = 0.0F;
float rightThrottle = 0.0F;
float wheelAdjust = 0.0F;
// These are the Halo Drive Tuning Constants, values need to be tuned to bot, wheelbase and gear ratios determine tunings.
float WheelGain = 0.7F;
float quickTurnGain = 0.5F;
float HaloThreshold = 0.15F;
bool ControlTypeTank = _gamepad.GetButton(1);
bool ControlTypeHalo = _gamepad.GetButton(3);
string Type = "";
if (ControlTypeTank || Mode)
{
leftstick = -1 * _gamepad.GetAxis(1);
rightstick = -1 * _gamepad.GetAxis(5);
x = leftstick;
y = rightstick;
Deadband(ref x);
Deadband(ref y);
Mode = true;
Type = "Tank";
leftThrottle = x;
rightThrottle = y;
}
if (ControlTypeHalo || Mode == false)
{
leftstick = -1 * _gamepad.GetAxis(1);
rightstick = -1 * _gamepad.GetAxis(2);
x = leftstick;
y = rightstick;
Deadband(ref x);
Deadband(ref y);
Mode = false;
Type = "Halo";
// If Throttle is below threshold, then do quickturn calcuation
if (System.Math.Abs(x) < HaloThreshold)
{
wheelAdjust = quickTurnGain * y;
leftThrottle = -wheelAdjust;
rightThrottle = wheelAdjust;
}
// Not Quickturn, so do the typical Halo calculation
else
{
wheelAdjust = WheelGain * y;
leftThrottle = x - wheelAdjust;
rightThrottle = x + wheelAdjust;
}
}
// Forcing throttle values between -1 and 1 "There is probably a better function to do this, but I am ignorant"
if (leftThrottle > 1)
{
leftThrottle = 1;
}
if (leftThrottle < -1)
{
leftThrottle = -1;
}
if (rightThrottle > 1)
{
rightThrottle = 1;
}
if (rightThrottle < -1)
{
rightThrottle = -1;
}
// Write Motor Control Values to the Motor Controllers ONCE!!!!!
FrontLeft.Set(leftThrottle);
BackLeft.Set(leftThrottle);
FrontRight.Set(rightThrottle);
BackRight.Set(rightThrottle);
stringBuilder.Append(Type);
stringBuilder.Append("\t");
stringBuilder.Append(leftstick);
stringBuilder.Append("\t");
stringBuilder.Append(rightstick);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(leftThrottle);
stringBuilder.Append("\t");
stringBuilder.Append(rightThrottle);
stringBuilder.Append("\t");
stringBuilder.Append(wheelAdjust);
stringBuilder.Append("\t");
}
