//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 static void Main()
{
//Create global objects and variables
LogitechWirelessGamepad gamepad = new LogitechWirelessGamepad(0); //TODO: 0 is the USB port... maybe move toa configFile along with the SPX Ids?
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor1 = new CTRE.Phoenix.MotorControl.CAN.VictorSPX(5); //Motor1 is LifeBoat index:5
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor2 = new CTRE.Phoenix.MotorControl.CAN.VictorSPX(6); //Motor2 is LifeBoat index:6
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor3 = new CTRE.Phoenix.MotorControl.CAN.VictorSPX(7); //Motor3 is LifeBoat index:7
//WARNING!
while (true) /* loop forever */
{
//TODO: Needs some type of BATTERY VOLTAGE CHECK, maybe vibe contoller when low?
//NOTE: Magic Math Stolen from https://makezine.com/projects/make-40/kiwi/
//and Thad's Wilson Code: https://github.com/MetalCowRobotics/MCR15B---Wilson/blob/master/src/org/team4213/wilson/KiwiDrive.java
//THIS CONTROLLER CHECK IS A FAILSAFE CHECK, DO NOT REMOVE!!!!
if (gamepad.isConnected())
{
//float lx = gamepad.getLX();
float x = -gamepad.getRX();
float y = gamepad.getRY();
float omega = -gamepad.getLX();
double w1 = -x + omega * 0.5;
double w2 = 0.5 * x + 0.866 * y + omega * 0.5;
double w3 = -(-0.5 * x + 0.866 * y - omega * 0.5);
Debug.Print(" w1:" + w1 + " w2:" + w2 + " w3:" + w3);
motor1.Set(ControlMode.PercentOutput, w3);
motor2.Set(ControlMode.PercentOutput, w2);
motor3.Set(ControlMode.PercentOutput, w1);
CTRE.Phoenix.Watchdog.Feed(); //this keeps the bot active... just leave it alone
}
else
{ //if the Controller Disconnects, stop the motors.
motor1.Set(ControlMode.PercentOutput, 0);
motor2.Set(ControlMode.PercentOutput, 0);
motor3.Set(ControlMode.PercentOutput, 0);
}
System.Threading.Thread.Sleep(10); //read the inputs every 10 milliseconds
}
}
/** 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);
}
}
}
