//static AnalogInput potentiometer0 = new AnalogInput(CTRE.HERO.IO.Port1.Analog_Pin5);
public static void Main()
{
//double analogRead0;
/* simple counter to print and watch using the debugger */
//int counter = 0;
/* loop forever */
//static System.IO.Ports.SerialPort _uart
// System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
CTRE.Phoenix.Controller.GameController myGamepad = new
CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor0 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(0);
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor1 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(1);
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor2 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(2);
CTRE.Phoenix.MotorControl.CAN.TalonSRX motor3 = new
CTRE.Phoenix.MotorControl.CAN.TalonSRX(3);
while (true)
{
/* print the three analog inputs as three columns */
//analogRead0 = potentiometer0.Read();
//Debug.Print("Motor 0 position: " + analogRead0);
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
//if ((analogRead0 > 0)&&(analogRead0 < .400))
{
Debug.Print("axis:" + myGamepad.GetAxis(1));
motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1) / 2);
motor2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(2) / 1.33);
//motor3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
//motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput,.5);
//analogRead0 = potentiometer0.Read();
//Debug.Print("Motor 0 position: " + analogRead0);
CTRE.Phoenix.Watchdog.Feed();
//if ((analogRead0 > .570) && (analogRead0 < .575))
// {
//motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
//}
//else
//{
// motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
//}
//CTRE.Phoenix.Watchdog.Feed();
}
//Debug.Print("Counter Value: " + counter);
/* increment counter */
// ++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public void SetRef(CTRE.Phoenix.Controller.GameController reference, uint idx)
{
if (idx >= 0 && idx <= 5)
{
_controllers[idx] = reference;
}
}
/** Returns only the first Button pressed despite other buttons' Statuses */
static public int GetFirstButton(CTRE.Phoenix.Controller.GameController gamepad)
{
for (uint i = 0; i < 16; ++i)
{
if (gamepad.GetButton(i))
{
if (i == 0)
{
return(1);
}
return((int)i);
}
}
return(-1);
}
public static void Main()
{
/* Start Init */
//robotInit();
CTRE.Phoenix.MotorControl.CAN.TalonSRX driveTalon1 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(0);
CTRE.Phoenix.MotorControl.CAN.TalonSRX driveTalon2 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(0);
CTRE.Phoenix.Controller.GameController gamepad =
new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
CTRE.Phoenix.Controller.GameControllerValues gv = new CTRE.Phoenix.Controller.GameControllerValues();
gamepad.GetAllValues(ref gv);
Intake intake = new Intake();
Conveyor conveyor = new Conveyor();
Climber climber = new Climber();
Drive drive = new Drive(driveTalon1, driveTalon2, gamepad);
TeleopStateMachine tsm = new TeleopStateMachine(intake, conveyor, climber, gamepad);
/* Start Auton */
/* Start Teleop */
/* simple counter to print and watch using the debugger */
int counter = 0;
/* loop forever */
while (true)
{
/* print the three analog inputs as three columns */
Debug.Print("Counter Value: " + counter);
/* increment counter */
++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
System.Threading.Thread.Sleep(100);
//TeleopStateMachine.moveRight();
}
}
static void Operate()
{
if (_gamepad == null)
{
_gamepad = new CTRE.Phoenix.Controller.GameController(UsbHostDevice.GetInstance(0));
}
//Drive Function
stringBuilder.Append("--DRIVEBASE CONTROLS--");
DriveSub.Drive(_gamepad, stringBuilder);
//Shooting Function
stringBuilder.Append("\n--SHOOTER CONTROLS--");
ShootSub.Shoot(_gamepad, stringBuilder);
//Tilt Function
stringBuilder.Append("\n--TILT CONTROLS--");
ShootSub.Tilt(_gamepad, stringBuilder);
}
public static void Main()
{
/* simple counter to print and watch using the debugger */
//int counter = 0;
// Controller Object
CTRE.Phoenix.Controller.GameController myGamepad = new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
// Talon Object
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(55);
/* loop forever */
while (true)
{
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
Debug.Print("Axis 0:" + myGamepad.GetAxis(0));
Debug.Print("Axis 1:" + myGamepad.GetAxis(1));
Debug.Print("Axis 2:" + myGamepad.GetAxis(2));
Debug.Print("Axis 5:" + myGamepad.GetAxis(5));
// pass axis value to talon
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1));
CTRE.Phoenix.Watchdog.Feed();
}
/* print the three analog inputs as three columns */
//Debug.Print("Counter Value: " + counter);
/* increment counter */
//++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public static void Main()
{
//Gamepad for input
CTRE.Phoenix.Controller.GameController _gamepad = new CTRE.Phoenix.Controller.GameController(CTRE.Phoenix.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.Phoenix.MotorControl.PWMSpeedController pwmSpeedController = new CTRE.Phoenix.MotorControl.PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7);
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.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.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);
}
}
public ButtonMonitor(CTRE.Phoenix.Controller.GameController gameCntrlr, int btnIdx, ButtonPressEventHandler handler)
{
_gameCntrlr = gameCntrlr;
_btnIdx = btnIdx;
ButtonPress = handler;
}
/** entry point of the application */
public static void Main()
{
for (int i = 0; i < 16; i++)
{
if (motors[i] == null)
{
}
else
{
motors[i].SetNeutralMode(CTRE.Phoenix.MotorControl.NeutralMode.Brake);
}
}
CTRE.Phoenix.Controller.GameController myGamepad = new
CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
/* temporary array */
byte[] scratch = new byte[1];
byte[] toReadIn = new byte[10];
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
//_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 9600);
_uart.Open();
//synch flags
bool lowering = false;
bool raising = true;
bool step0 = true;
bool step1 = false;
bool step2 = false;
bool step3 = false;
bool kneel0 = false;
bool kneel1 = false;
bool kneel2 = false;
bool kneel3 = false;
bool eGoalReached = false;
bool wGoalReached = false;
while (true)
{
string[] stringReadings;
//string[] strCopy;
int[] intReadings;
//if (_rx != null && _uart.BytesToRead > 0)
//{
if (debug)
{
Debug.Print("getting initial string readings");
}
stringReadings = getStrReadings();
intReadings = getIntReadings(stringReadings);
bool lastPress = false;
bool lastPress1 = false;
bool lastPress2 = false;
bool pressed1;
bool pressed2;
bool pressed;
bool RF_met = false;
bool RF_met1 = false;
bool RF_met2 = false;
bool RF_met3 = false;
//bool reachedLimit = false;
//raise the legs
/*
* if (raising && raise(0, intReadings))
* {
* Debug.Print("*** Raised leg 0");
* lowering = true;
* raising = false;
*
* }
* else
* {
* Debug.Print("Elbow Goal: " + jointPositions[1][OpUp] + " -- currently: " + sensor[1]);
* Debug.Print("Wrist Goal: " + jointPositions[2][OpUp] + " -- currently: " + sensor[2]);
* }
*
* if (lowering && lower(0, intReadings))
* {
* Debug.Print("*** Lowered leg 0");
* lowering = false;
* raising = true;
* }
* else
* {
* Debug.Print("LOWER Elbow Goal: " + jointPositions[1][OpUp] + " -- currently: " + sensor[1]);
* Debug.Print("LOWER Wrist Goal: " + jointPositions[2][OpUp] + " -- currently: " + sensor[2]);
* }
*/
// comment the following line to enable step motion
//step0 = false;
if (step0 && step(0, intReadings, ref raising, ref lowering, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished stepping");
raising = true;
lowering = false;
step0 = false;
step1 = true;
}
if (step1 && step(1, intReadings, ref raising, ref lowering, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished stepping");
raising = true;
lowering = false;
step1 = false;
step2 = true;
}
if (step2 && step(2, intReadings, ref raising, ref lowering, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished stepping");
raising = true;
lowering = false;
step2 = false;
step3 = true;
}
if (step3 && step(3, intReadings, ref raising, ref lowering, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished stepping");
raising = true;
lowering = false;
step3 = false;
step0 = false; // set true to repeat
kneel0 = true;
}
// comment the following to enable kneeling
//kneel0 = false;
//eGoalReached = false;
//wGoalReached = false;
if (kneel0 && kneel(0, intReadings, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished kneeling 0");
eGoalReached = false;
wGoalReached = false;
kneel0 = false;
kneel1 = true;
}
if (kneel1 && kneel(1, intReadings, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished kneeling 1");
eGoalReached = false;
wGoalReached = false;
kneel1 = false;
kneel2 = true;
}
if (kneel2 && kneel(2, intReadings, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished kneeling 2");
eGoalReached = false;
wGoalReached = false;
kneel2 = false;
kneel3 = true;
}
if (kneel3 && kneel(3, intReadings, ref eGoalReached, ref wGoalReached))
{
Debug.Print("Finished kneeling 3");
eGoalReached = false;
wGoalReached = false;
kneel3 = false;
kneel0 = false; //set true to repeat
}
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
talons[3].Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1));
talons[7].Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1));
talons[11].Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(2));
talons[15].Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(2));
Debug.Print(myGamepad.GetAxis(1).ToString());
Debug.Print(myGamepad.GetAxis(2).ToString());
CTRE.Phoenix.Watchdog.Feed();
/*
* Debug.Print("gamepad connected");
* pressed = myGamepad.GetButton(1);
* pressed2 = myGamepad.GetButton(2);
* if (pressed != lastPress || pressed == true)
* {
* Debug.Print("Button pressed, Kneeling selected");
* //while(reachedLimit == false)
*
* //{
* stringReadings = getStrReadings();
* intReadings = getIntReadings(stringReadings);
* Debug.Print("potentiometer 2 is " + intReadings[2].ToString());
* if (!RF_met1)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met1 = raise(1, intReadings,ref eGoalReached, ref wGoalReached);
* Debug.Print("raising leg");
* lastPress = pressed;
* }
* if (!RF_met2)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met2 = raise(2, intReadings, ref eGoalReached, ref wGoalReached);
* Debug.Print("raising leg");
* lastPress = pressed;
* }
* if (!RF_met)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met = raise(0, intReadings, ref eGoalReached, ref wGoalReached);
* Debug.Print("raising leg");
* lastPress = pressed;
* }
* if (!RF_met3)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met3 = raise(3, intReadings, ref eGoalReached, ref wGoalReached);
* Debug.Print("raising leg");
* lastPress = pressed;
* }
* }
* //if (pressed2 == true)
* //{
* //motorRFT.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1) / 2);
* //Hardware._rightFrontTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1*myGamepad.GetAxis(1), CTRE.Phoenix.MotorControl.DemandType.ArbitraryFeedForward, -1*myGamepad.GetAxis(2) / 2);
* //Hardware._leftFrontTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1*myGamepad.GetAxis(1), CTRE.Phoenix.MotorControl.DemandType.ArbitraryFeedForward, 1*myGamepad.GetAxis(2) / 2);
* //Hardware._rightBackTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1*myGamepad.GetAxis(1), CTRE.Phoenix.MotorControl.DemandType.ArbitraryFeedForward, -1*myGamepad.GetAxis(2) / 2);
* //Hardware._leftBackTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1*myGamepad.GetAxis(1), CTRE.Phoenix.MotorControl.DemandType.ArbitraryFeedForward, 1*myGamepad.GetAxis(2) / 2);
* //}
* /*if (pressed1 != lastPress1 || pressed1 == true)
* {
* Debug.Print("Button pressed, Kneeling selected");
* //while(reachedLimit == false)
*
* //{
* stringReadings = getStrReadings();
* intReadings = getIntReadings(stringReadings);
* Debug.Print("potentiometer 2 is " + intReadings[2].ToString());
* if (!RF_met)
* {
* RF_met = raise(0, intReadings);
* Debug.Print("raising leg");
* lastPress1 = pressed1;
* }
* if (!RF_met3)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met3 = raise(3, intReadings);
* Debug.Print("raising leg");
* lastPress1 = pressed1;
* }
* }
* if (pressed2 != lastPress2 || pressed2 == true)
* {
* Debug.Print("Button pressed, Kneeling selected");
* //while(reachedLimit == false)
*
* //{
* stringReadings = getStrReadings();
* intReadings = getIntReadings(stringReadings);
* Debug.Print("potentiometer 2 is " + intReadings[2].ToString());
* if (!RF_met1)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met1 = lower(1, intReadings);
* Debug.Print("raising leg");
* lastPress2 = pressed2;
* }
* if (!RF_met2)//&& !RF_met1 && !RF_met2 && !RF_met3)
* {
* RF_met2 = lower(2, intReadings);
* Debug.Print("raising leg");
* lastPress2 = pressed2;
* }
*
* }*/
System.Threading.Thread.Sleep(10);
}
}
}
/** entry point of the application */
public static void Main()
{
CTRE.Phoenix.Controller.GameController myGamepad = new
CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
/* temporary array */
byte[] scratch = new byte[1];
byte[] toReadIn = new byte[10];
/* open the UART, select the com port based on the desired gadgeteer port.
* This utilizes the CTRE.IO Library.
* The full listing of COM ports on HERO can be viewed in CTRE.IO
*
*/
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
//_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 9600);
_uart.Open();
/* send a message to the terminal for the user to see */
//_uart.Write(_helloMsg, 0, _helloMsg.Length);
/* loop forever */
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor1 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(1);
while (true)
{
//Debug.Print("In loop");
//System.Threading.Thread.Sleep(10);
/* read bytes out of uart */
//if (_uart.BytesToRead > 0)
//{
//Debug.Print("In if");
/*
* int readCnt = _uart.Read(_rx, 0, CalcRemainingCap());
* for (int i = 0; i < readCnt; ++i)
* {
* Debug.Print("In for");
* PushByte(_rx[i]);
* }
*/
//Debug.Print("reading uart");
//_uart.Read(_rx, 0, 13);
//Debug.Print("BYTES TO READ = "+_uart.BytesToRead.ToString());
//System.Threading.Thread.Sleep(10);
//int readIn =_uart.Read(_rx, 0, 10);
/*
* This section is in fucntion getStrReadings now. This function is called below.
*
* char[] readIn = Encoding.UTF8.GetChars(_rx);
* string str = new string(readIn);
* string[] readings = str.Split('-');
*/
string[] stringReadings;
//string[] strCopy;
int[] intReadings;
//if (_rx != null && _uart.BytesToRead > 0)
//{
Debug.Print("getting initial string readings");
stringReadings = getStrReadings();
//int[] intReadings;
//if (stringReadings[0] == "E" && stringReadings[3] == ";")
//{
if (newData == true)
{
intReadings = getIntReadings(stringReadings);
// intReadings = getIntReadings(stringReadings);
//}
//else
//{
// strCopy = stringReadings;
//stringReadings[0] = strCopy[2][1].ToString();
//Debug.Print("New index 0 " + stringReadings[0]);
//stringReadings[1] = strCopy[3] + strCopy[0];
//Debug.Print("New index 1 " + stringReadings[1]);
//stringReadings[2] = strCopy[1];
//Debug.Print("New index 2 " + stringReadings[2]);
//stringReadings[3] = strCopy[2][0].ToString();
//Debug.Print("New index 3 " + stringReadings[3]);
//intReadings = getIntReadings(stringReadings);
//}
//if (intReadings[0] < 90)
//while (true)
bool lastPress = false;
bool pressed;
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
pressed = myGamepad.GetButton(1);
if (pressed != lastPress && pressed == true)
{
Debug.Print("Button pressed, Kneeling selected");
while (intReadings[0] < 500)
{
motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, .25);
stringReadings = getStrReadings();
intReadings = getIntReadings(stringReadings);
Debug.Print(intReadings[0].ToString());
CTRE.Phoenix.Watchdog.Feed();
}
// else
//{
// lastPress = pressed;
//}
}
lastPress = pressed; //debounce
//set motor to run forward until specified angle
//motor0.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, .25);
//_uart.Read(_rx, 0, 13);
//Debug.Print("BYTEs TO READ = " + _uart.BytesToRead.ToString());
//System.Threading.Thread.Sleep(10);
//if (_rx != null)
//{
// stringReadings = getStrReadings();
//}
//if (stringReadings[0] == "E" && stringReadings[3] == ";")
//{
//intReadings = getIntReadings(stringReadings);
//}
//else
//{
//{
//hard coded to most common error for now
// strCopy = stringReadings;
// stringReadings[0] = strCopy[2][1].ToString();
// Debug.Print("New index 0 " + stringReadings[0]);
//stringReadings[1] = strCopy[3] + strCopy[0];
//Debug.Print("New index 1 " + stringReadings[1]);
//stringReadings[2] = strCopy[1];
//Debug.Print("New index 2 " + stringReadings[2]);
//stringReadings[3] = strCopy[2][0].ToString();
//Debug.Print("New index 3 " + stringReadings[3]);
//intReadings = getIntReadings(stringReadings);
//}
//}
Debug.Print("Motor running, goal not met");
//CTRE.Phoenix.Watchdog.Feed();
}
Debug.Print("Motor stopped, goal met");
//}
/* wait a bit, keep the main loop time constant, this way you can add to this example (motor control for example). */
System.Threading.Thread.Sleep(10);
}
}
}
public Controller()
{
gameController = new CTRE.Phoenix.Controller.GameController(CTRE.Phoenix.UsbHostDevice.GetInstance(0));
CTRE.Phoenix.UsbHostDevice.GetInstance(0).SetSelectableXInputFilter(
CTRE.Phoenix.UsbHostDevice.SelectableXInputFilter.XInputDevices);
}
public static void Main()
{
/* create a gamepad object */
CTRE.Phoenix.Controller.GameController myGamepad = new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
/* create a talon, the Talon Device ID in HERO LifeBoat is zero */
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(1);
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon2 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(2);
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon3 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(25);
float speed;
float turn;
float avg;
bool rampDown = false;
bool rampUp = false;
int c = 0;
var startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(50000000))
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("Test: " + c++);
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(50000000))
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1);
Debug.Print("Test: " + c++);
CTRE.Phoenix.Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(50);
/* loop forever */
while (true)
{
/* added inside the while loop */
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
//If you want to change it to right bumper use get.axis(2) and (5)
speed = myGamepad.GetAxis(1);
turn = myGamepad.GetAxis(0);
avg = speed / 2 + turn / 2;
rampDown = myGamepad.GetButton(1);
rampUp = myGamepad.GetButton(2);
/* print the axis value */
//Turning is overriding the speed
Debug.Print("axis:" + myGamepad.GetAxis(0) + ", " + myGamepad.GetAxis(1) + ", " + myGamepad.GetAxis(2) + ", " + myGamepad.GetAxis(5));
Debug.Print("speed:" + speed);
Debug.Print("turn:" + turn);
Debug.Print("Button 1(DOWN): " + rampDown);
Debug.Print("Button 2(UP): " + rampUp);
/* pass axis value to talon */
// myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, speed);
// myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, speed);
//myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, turn);
//myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, turn * -1);
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, avg * -1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, avg);
if (rampDown)
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1.0);
}
else if (rampUp)
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1.0);
}
else
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
}
//once button is pressed move motor specific amount of times so that the ramp rests at a 90 degree angle
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(50);
}
}
