//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()
{
// Loop for running Tshirt Code
while (true)
{
// CTRE Saftey feature to make it so that if the controller is disconnected we no longer send values to the motor controllers
if (m_controller.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed(); // Allows motor control
}
ArcadeDrive();
CannonShoot();
System.Threading.Thread.Sleep(20);
}
}
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 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);
}
}
/** 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);
}
}
}
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 bool IsConnected()
{
return(gameController.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected);
}
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);
}
}