//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);
}
}
// Arcade Drive Controller
static void ArcadeDrive()
{
// Set slaves and inversion
m_masterRight.SetInverted(true);
m_slaveRight.SetInverted(true);
m_slaveRight.Follow(m_masterRight);
m_masterLeft.SetInverted(false);
m_slaveLeft.SetInverted(false);
m_slaveLeft.Follow(m_masterLeft);
// Get Axis Value (-1.0 to 1.0)
float y = m_controller.GetAxis(LEFT_JOYSTICK_Y);
float x = m_controller.GetAxis(RIGHT_JOYSTICK_X);
// Set speed values
double throttle = Deadband(y); // Throttle equals y axis value of left joystick with deadband
double steer = Deadband(x); // Steer equals x axis value of right joystick with deadband
throttle = System.Math.Pow(throttle, 3.0);
steer = System.Math.Pow(steer, 3.0);
double rightSpeed = throttle + steer;
double leftSpeed = throttle - steer;
// If either right or left speed are out of range (-1.0 to 1.0) Scale both until in range
double maxValue = 0;
if (System.Math.Abs(leftSpeed) > maxValue)
{
maxValue = System.Math.Abs(leftSpeed);
}
if (System.Math.Abs(rightSpeed) > maxValue)
{
maxValue = System.Math.Abs(rightSpeed);
}
//Scale down all values if max > 1.0
if (maxValue > 1.0)
{
leftSpeed /= maxValue;
rightSpeed /= maxValue;
}
// Set outputs
m_masterRight.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, rightSpeed);
m_masterLeft.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, leftSpeed);
}
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);
}
}
/** throw all the gamepad buttons into an array */
void FillBtns(ref bool[] btns)
{
for (uint i = 1; i <= NUM_BUTTONS; ++i)
{
btns[i] = _gamepad.GetButton(i);
}
if (_gamepad.GetAxis(0) < -1.0 + STICK_TOLERANCE)
{
btns[(int)JOYSTICK.XAXIS_L] = true;
btns[(int)JOYSTICK.XAXIS_R] = false;
}
else if (_gamepad.GetAxis(0) > 1.0 - STICK_TOLERANCE)
{
btns[(int)JOYSTICK.XAXIS_L] = false;
btns[(int)JOYSTICK.XAXIS_R] = true;
}
else
{
btns[(int)JOYSTICK.XAXIS_L] = false;
btns[(int)JOYSTICK.XAXIS_R] = false;
}
if (_gamepad.GetAxis(1) < -1.0 + STICK_TOLERANCE)
{
btns[(int)JOYSTICK.YAXIS_U] = true;
btns[(int)JOYSTICK.YAXIS_D] = false;
}
else if (_gamepad.GetAxis(1) > 1.0 - STICK_TOLERANCE)
{
btns[(int)JOYSTICK.YAXIS_U] = false;
btns[(int)JOYSTICK.YAXIS_D] = true;
}
else
{
btns[(int)JOYSTICK.YAXIS_U] = false;
btns[(int)JOYSTICK.YAXIS_D] = false;
}
}
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);
}
}
public static void Main()
{
/* start transmitting neutral */
_percentOutput = 0;
_canifier.SetPWMOutput((uint)kMotorControllerCh, 0);
_canifier.EnablePWMOutput(1, true);
_canifier.EnablePWMOutput((int)kMotorControllerCh, true);
while (true)
{
CTRE.Phoenix.Watchdog.Feed();
/* just grab three axis and direct control the components */
float axis = _gamepad.GetAxis(1);
/* scale to typical pwm withds */
float pulseUs = CTRE.Phoenix.LinearInterpolation.Calculate(axis, -1, 1000f, +1, 2000f); /* [-1,+1] => [1000,2000]us */
/* scale to period */
float periodUs = 4200; // hardcoded for now, this will be settable in future firmware update.
_percentOutput = pulseUs / periodUs;
/* set it */
Debug.Print(": " + _percentOutput);
_canifier.SetPWMOutput((uint)kMotorControllerCh, _percentOutput);
}
}
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);
}
}
public void OutputAxisValues()
{
Debug.Print("0: " + gameController.GetAxis(0));
Debug.Print("1: " + gameController.GetAxis(1));
Debug.Print("2: " + gameController.GetAxis(2));
Debug.Print("3: " + gameController.GetAxis(3));
Debug.Print("4: " + gameController.GetAxis(4));
Debug.Print("5: " + gameController.GetAxis(5));
Debug.Print("6: " + gameController.GetAxis(6));
Debug.Print("7: " + gameController.GetAxis(7));
}
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);
}
}