public static void Main()
{
/* Initialize Display */
CTRE.Gadgeteer.Module.DisplayModule.LabelSprite lineOne, lineTwo, lineThree, lineFour, lineFive,
lineSix, lineSeven, lineEight, lineNine, lineTen;
/* State and battery display in the 1st row */
lineOne = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Red, 0, 0, 100, 10);
lineTwo = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Green, 0, 10, 100, 10);
lineThree = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Cyan, 0, 20, 100, 10);
lineFour = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Cyan, 0, 30, 100, 10);
lineFive = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Yellow, 0, 40, 100, 10);
lineSix = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Blue, 0, 50, 100, 10);
lineSeven = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Orange, 0, 60, 100, 10);
lineEight = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.White, 0, 70, 100, 10);
lineNine = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.White, 0, 80, 100, 10);
lineTen = Display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.White, 0, 90, 100, 10);
while (true)
{
Gamepad.GetButtons(_currentBtns);
for (int i = 1; i < _currentBtns.Length; i++)
{
if (_currentBtns[i] && !_previousBtns[i])
{
++_btnCount[i];
}
}
/* Velocity CAN Frame */
byte[] frame2 = new byte[8];
ulong data2 = (ulong)BitConverter.ToUInt64(frame2, 0);
CTRE.Native.CAN.Send(8, data2, 8, 0);
/* State and battery display in the 1st row */
lineOne.SetText("Btn 1: " + Gamepad.GetButton(1) + " " + _btnCount[1]);
lineTwo.SetText("Btn 2: " + Gamepad.GetButton(2) + " " + _btnCount[2]);
lineThree.SetText("Btn 3: " + Gamepad.GetButton(3) + " " + _btnCount[3]);
lineFour.SetText("Btn 4: " + Gamepad.GetButton(4) + " " + _btnCount[4]);
lineFive.SetText("Btn 5: " + Gamepad.GetButton(5) + " " + _btnCount[5]);
lineSix.SetText("Btn 6: " + Gamepad.GetButton(6) + " " + _btnCount[6]);
lineSeven.SetText("Btn 7: " + Gamepad.GetButton(7) + " " + _btnCount[7]);
lineEight.SetText("Btn 8: " + Gamepad.GetButton(8) + " " + _btnCount[8]);
lineNine.SetText("Btn 9: " + Gamepad.GetButton(9) + " " + _btnCount[9]);
lineTen.SetText("Btn 10: " + Gamepad.GetButton(10) + " " + _btnCount[10]);
System.Array.Copy(_currentBtns, _previousBtns, _previousBtns.Length);
Thread.Sleep(110);
}
}
public void Process()
{
bool down = _gameCntrlr.GetButton((uint)_btnIdx);
if (!_isDown && down)
{
ButtonPress(_btnIdx, down);
}
_isDown = down;
}
public void run()
{
Loop10Ms();
//Enable or Disable the entire system using the START and BACK buttons.
if (_gamepad.GetButton((int)JOYSTICK.START))
{
Debug.Print("ENABLING!!!!!");
isRunning = true;
}
else if (_gamepad.GetButton((int)JOYSTICK.BACK))
{
Debug.Print("Disabled");
isRunning = false;
}
if (isRunning)
{
CTRE.Phoenix.Watchdog.Feed();
}
}
public static void Main()
{
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();
OutputPort solonoid_extend = new OutputPort(CTRE.HERO.IO.Port5.Pin4, false);
OutputPort solonoid_retract = new OutputPort(CTRE.HERO.IO.Port5.Pin6, false);
OutputPort compressor = new OutputPort(CTRE.HERO.IO.Port5.Pin8, false);
bool xButton = true; //compressor
bool aButton = true;
bool bButton = true;
while (true) //feeds us info
{
Talon1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, _gamepad.GetAxis(1) + _gamepad.GetAxis(2));
Talon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, _gamepad.GetAxis(1) - _gamepad.GetAxis(2));
pwm_9.Duration = (uint)_gamepad.GetAxis(1) * 50000;
xButton = _gamepad.GetButton(1);
aButton = _gamepad.GetButton(2);
bButton = _gamepad.GetButton(3);
compressor.Write(xButton);
solonoid_extend.Write(aButton);
solonoid_retract.Write(bButton);
Thread.Sleep(10);
}
}
/** 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()
{
//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 static void Main()
{
/* Initialize Display Module elements */
InitDisplayModule();
/* Initialize Talon and Victor with various configurations */
InitMotors();
/* LED Variables */
float _Brightness = 0.25f; /* Default LED Brightness */
Boolean On = true; /* Color Flashing state */
byte i = 0; /* Color Duration track variable for flashing */
int colorDelay = 0; /* Color Duration track variable for Sequence speed */
/* State variables */
DriveState MotionState = DriveState.MotionMagicTalon;
LEDState OperationState = LEDState.Pigeon;
/* Buttons and boolean for motor control */
Boolean lastButton1 = false;
Boolean lastButton2 = false;
Boolean lastButton3 = false;
Boolean lastbutton4 = false;
while (true)
{
/* Check gamepad connection */
if (_Gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
OperationState = LEDState.Controller;
}
else
{
OperationState = LEDState.Pigeon;
}
if (OperationState == LEDState.Controller)
{
CTRE.Phoenix.Watchdog.Feed();
if (_Battery.IsLow())
{
_labelTitle.SetText("Low voltage");
_labelTitle.SetColor(DisplayModule.Color.Red);
}
else
{
_labelTitle.SetText("Controller");
_labelTitle.SetColor(DisplayModule.Color.Green);
}
/* Get gamepad values */
float LeftX = _Gamepad.GetAxis(0);
float LeftY = _Gamepad.GetAxis(1);
float RightY = _Gamepad.GetAxis(5);
float RightX = _Gamepad.GetAxis(2);
/* Deadband gamepad values */
CTRE.Phoenix.Util.Deadband(ref LeftX);
CTRE.Phoenix.Util.Deadband(ref LeftY);
CTRE.Phoenix.Util.Deadband(ref RightX);
CTRE.Phoenix.Util.Deadband(ref RightY);
/* Update Guages */
UpdateGauge(_leftX, LeftX);
UpdateGauge(_leftY, LeftY);
UpdateGauge(_rightX, RightX);
UpdateGauge(_rightY, RightY);
/* Update Crosshairs */
_leftCrossHair.SetPosition((int)(30 + 15 * LeftX), 100 + (int)(15 * LeftY));
_rightCrossHair.SetPosition((int)(100 + 15 * RightX), 100 + (int)(15 * RightY));
/* Get single button presses to control MotionState */
Boolean Button1 = _Gamepad.GetButton(1);
Boolean Button2 = _Gamepad.GetButton(2);
Boolean Button3 = _Gamepad.GetButton(3);
Boolean Button4 = _Gamepad.GetButton(4);
if (Button1 && !lastButton1)
{
MotionState = DriveState.MotionMagicTalon;
}
else if (Button2 && !lastButton2)
{
MotionState = DriveState.PercentOutputBoth;
}
else if (Button3 && !lastButton3)
{
MotionState = DriveState.MotionMagicVictor;
}
else if (Button4 && !lastbutton4)
{
MotionState = DriveState.SensorReset;
}
lastButton1 = Button1;
lastButton2 = Button2;
lastButton3 = Button3;
lastbutton4 = Button4;
/* Controls Motoroutput based on MotionState */
MotorDriveControl(-LeftY, MotionState);
if (LeftX != 0 || LeftY != 0)
{
/** Left joystick in use, stop color cycling and give user control */
/* Grab brightness from the right joystick ([-1,1] + 1 * 0.5 => [0,1]) */
float Brightness = (RightY - 1f) * -0.5f;
_Brightness = Brightness;
/* Update LED strip with left joystick, right joystick, and brightness */
UpdateLedStrip(Brightness, LeftX, LeftY);
}
else
{
/** Left joystick not in use, start color cycling */
/* You can change the sequence in ColorSequencer.cs by ordering the premade colors or creating your own values */
colorDelay++;
if (colorDelay >= 3)
{
_ColorSequencer.Process();
colorDelay = 0;
}
/* Go through a color sequence at half brightness when idle */
UpdateLedStrip(_Brightness, _ColorSequencer.Red, _ColorSequencer.Green, _ColorSequencer.Blue);
}
}
else if (OperationState == LEDState.Pigeon)
{
if (_Battery.IsLow())
{
_labelTitle.SetText("Low Batt...");
_labelTitle.SetColor(DisplayModule.Color.Red);
}
else
{
_labelTitle.SetText("Pigeon");
_labelTitle.SetColor(DisplayModule.Color.Magenta);
}
/* Check status of Pigeon to see if it is connected */
CTRE.Phoenix.Sensors.PigeonState _PigeonState = _Pigeon.GetState();
if (_PigeonState == CTRE.Phoenix.Sensors.PigeonState.Ready)
{
/** Pigeon connected, giver user tilt control */
/* Pull Yaw, Pitch, and Roll from Pigeon */
float[] YPR = new float[3];
_Pigeon.GetYawPitchRoll(YPR);
float Pitch = YPR[1];
float Roll = YPR[2];
CTRE.Phoenix.Util.Cap(Pitch, 90);
CTRE.Phoenix.Util.Cap(Roll, 90);
float Brightness = 0.5f;
/* Update LED strip */
UpdateLedStrip(Brightness, Pitch / 90, Roll / 90, true);
}
else
{
/* Pigeon is not Ready/Available, so flash us */
i++;
if (i >= 100)
{
On = !On;
i = 0;
}
/* Decide if strip is white or off */
if (On == true)
{
UpdateLedStrip(1, 255, 255, 255); /* White */
}
else if (On == false)
{
UpdateLedStrip(1, 0, 0, 0); /* Off */
}
}
}
int idx = GetFirstButton(_Gamepad);
if (idx < 0)
{
_labelRow1.SetColor((DisplayModule.Color) 0xA0A0A0); // gray RGB
_labelRow2.SetColor((DisplayModule.Color) 0xA0A0A0); // gray RGB
_labelRow3.SetColor((DisplayModule.Color) 0xA0A0A0); // gray RGB
float[] ypr = new float[3];
_Pigeon.GetYawPitchRoll(ypr);
_labelRow1.SetText("Yaw:" + ypr[0]);
_labelRow2.SetText("Pitch:" + ypr[1]);
_labelRow3.SetText("Roll:" + ypr[2]);
}
else
{
switch (idx % 4)
{
case 0: _labelRow1.SetColor(DisplayModule.Color.Cyan); break;
case 1: _labelRow1.SetColor(DisplayModule.Color.Green); break;
case 2: _labelRow1.SetColor(DisplayModule.Color.Red); break;
case 3: _labelRow1.SetColor(DisplayModule.Color.Yellow); break;
}
_labelRow1.SetText("Pressed Button " + idx);
_labelRow2.SetText("");
_labelRow3.SetText("");
}
/* Let he program to sleep for a little */
Thread.Sleep(5);
}
}
//shoots cannons each time shoot button is pressed in ascending order
static void CannonShoot()
{
//Debug.Print(currentBarrel.ToString());
// reload state
//bool currentreloadstate = m_controller.GetButton(LEFT_BUMPER);
//if (!prevReloadState && currentreloadstate)
//{
// reloading = !reloading;
//}
//prevReloadState = currentreloadstate;
// bool flip so that you dont hold down button and rapid fire
bool currentShootStateMain = m_controller.GetButton(A_BUTTON); // A button for as trigger
bool currentShootStateOne = m_controller.GetButton(X_BUTTON); // Right Bumper + A button for shooting one cannon
bool currentShootStateTwo = m_controller.GetButton(B_BUTTON); // Left Bumper + A button for shooting six cannons
if (!prevShootStateMain && currentShootStateMain)
{
// starts timer
shootTimers[currentBarrel] = SHOOT_TIME;
// flips through barrels
currentBarrel++;
if (currentBarrel > 5)
{
currentBarrel = 0;
}
Debug.Print(currentBarrel.ToString());
}
// secret shoot all 6 at same time command
if (!prevShootStateTwo && currentShootStateTwo)
{
// starts all timers while flipping through barrels
for (int i = 0; i < shootTimers.Length; i++)
{
shootTimers[currentBarrel] = SHOOT_TIME;
currentBarrel++;
if (currentBarrel > 5)
{
currentBarrel = 0;
}
Debug.Print(currentBarrel.ToString());
}
Debug.Print("Shoot Six");
}
else if (prevShootStateTwo && !currentShootStateTwo)
{
Debug.Print("End Shoot Six");
}
prevShootStateOne = currentShootStateOne;
prevShootStateTwo = currentShootStateTwo;
prevShootStateMain = currentShootStateMain;
Shoot();
ReduceTimers();
}
public static void Main()
{
/* Current setup is a CTRE MAG Encoder and a 128 CPR Opitcal Encoder, where printf's can be used to compare accuracy */
magTalon.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.FeedbackDevice.PulseWidthEncodedPosition, 0, kTimeout);
anaTalon.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.FeedbackDevice.QuadEncoder, 0, kTimeout);
magTalon.ConfigSelectedFeedbackCoefficient(0.125f, 0, kTimeout); /* Mag Encoder 4096 CPR * 0.125 = 128 == 128 CPR Opitcal Encoder, could scale up */
anaTalon.ConfigSelectedFeedbackCoefficient(1, 0, kTimeout);
anaTalon.SetSensorPhase(false);
magTalon.SetInverted(true);
magTalon.SetSensorPhase(true);
/* Reduce the velocity averaging to generate useful plots in Vehicle spy */
magTalon.ConfigVelocityMeasurementWindow(1, kTimeout);
magTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_1Ms, kTimeout);
anaTalon.ConfigVelocityMeasurementWindow(1, kTimeout);
anaTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_1Ms, kTimeout);
/* Increase the rate of the CAN frame */
magTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_2_Feedback0, 4, kTimeout);
anaTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_2_Feedback0, 4, kTimeout);
/* For PWM test, scale down motor so optical has a chance to keep up? */
float peakOutput = 1f;
float nominalOutput = 0;
magTalon.ConfigPeakOutputForward(peakOutput, kTimeout);
magTalon.ConfigPeakOutputReverse(-peakOutput, kTimeout);
magTalon.ConfigNominalOutputForward(nominalOutput, kTimeout);
magTalon.ConfigNominalOutputReverse(-nominalOutput, kTimeout);
differenceDisplay = displayModule.AddLabelSprite(gFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Orange, 0, 0, 100, 15);
statusDisplay = displayModule.AddLabelSprite(gFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Green, 0, 20, 100, 15);
errorDisplay = displayModule.AddLabelSprite(gFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Red, 0, 60, 100, 15);
statusDisplay.SetText("Gamepad Mode");
errorDisplay.SetText("None");
/* Clear Position */
SetPositon(0);
/* Variables to test various features */
bool lastBtn = false;
bool lastBtn3 = false;
bool state = false;
float output = 0;
while (true)
{
/* CTRE Output enable */
if (gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Encoder Postion taring */
bool btn = gamepad.GetButton(2);
bool btn3 = gamepad.GetButton(3);
if (btn && !lastBtn)
{
SetPositon(0);
errorDisplay.SetText("Error: " + 0);
}
if (btn3 && !lastBtn3)
{
state = !state;
if (state)
{
stopwatch.Start();
statusDisplay.SetText("Sine Mode");
}
else
{
statusDisplay.SetText("Gamepad Mode");
}
}
lastBtn = btn;
lastBtn3 = btn3;
if (!state)
{
/* Joypad value */
output = gamepad.GetAxis(1);
/* Reduce speed if right trigger is held down */
if (gamepad.GetButton(6))
{
output *= 0.50f;
}
}
else
{
float amplitude = 1;
float frequencyHZ = 0.2f;
float time = stopwatch.Duration;
output = amplitude * (float)System.Math.Sin((2 * System.Math.PI) * frequencyHZ * time);
}
byte[] Frame = new byte[8];
Frame[0] = (byte)((int)(output * 1000) >> 8);
Frame[1] = (byte)((int)(output * 1000) & 0xFF);
ulong data = (ulong)BitConverter.ToUInt64(Frame, 0);
CTRE.Native.CAN.Send(0x09, data, 8, 0);
/* (scale for maximum rpm @3000 - 6900ish) */
output *= (1 / 5f);
/* Talon with CTRE Mag Encoder drives the Talon */
magTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, output);
/* Telemetry */
int magTemp = magTalon.GetSelectedSensorPosition(0);
int anaTemp = anaTalon.GetSelectedSensorPosition(0);
int difference = magTemp - anaTemp;
Debug.Print("mag: " + magTemp + " ana: " + anaTemp + " dif: " + difference);
differenceDisplay.SetText("Dif: " + difference);
if (output > -0.05 && output < 0.05)
{
/* We are slow enough to check */
if (System.Math.Abs(difference) > 50)
{
/* Disruption in position */
state = false;
statusDisplay.SetText("Bad Postion");
errorDisplay.SetText("Error: " + difference);
}
}
else if (System.Math.Abs(difference) > 500)
{
/* Disruption in position */
state = false;
statusDisplay.SetText("Bad Postion");
errorDisplay.SetText("Error: " + difference);
}
/* Allow some breathing room for the CAN Frames */
Thread.Sleep(5);
}
}
/** 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 void PrintActive()
{
for (int i = 1; i <= 15; i++)
{
if (gameController.GetButton((uint)i) == true)
{
Debug.Print("" + i);
}
}
}
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);
}
}