/* Does all motor output control based on Arguements */
static void MotorDriveControl(float value, DriveState driveOption)
{
float SensorNativeUnits = 4096; // Native units per rotation
float ServotoRotation = value * 5 * SensorNativeUnits; // 5 rotations forward and reverse
if (driveOption == DriveState.MotionMagicTalon)
{
_Talon.Set(CTRE.Phoenix.MotorControl.ControlMode.MotionMagic, ServotoRotation);
_victor.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
}
else if (driveOption == DriveState.MotionMagicVictor)
{
_victor.Set(CTRE.Phoenix.MotorControl.ControlMode.MotionMagic, ServotoRotation);
_Talon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
}
else if (driveOption == DriveState.PercentOutputBoth)
{
_victor.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, value);
_Talon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, value);
}
else if (driveOption == DriveState.SensorReset)
{
_victor.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
_Talon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
_victor.SetSelectedSensorPosition(0, 0, kTimeoutMs);
_Talon.SetSelectedSensorPosition(0, 0, kTimeoutMs);
}
}
// 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);
}
}
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()
{
/* 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 static void Main()
{
/* Initialize Display elements */
CTRE.Gadgeteer.Module.DisplayModule.LabelSprite goodCountDisplay, badCountDisplay, statusDisplay, currentTimeDisplay, longestTimeDisplay, shortestTimeDisplay, averageTimeDisplay,
longestTimeDisplay_test, shortestTimeDisplay_test, averageTimeDisplay_test, OriginalGCDisplay, TestGCDisplay, myDisplay;
/* Count and state */
goodCountDisplay = display.AddLabelSprite(bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Green, 0, 0, 60, 15);
badCountDisplay = display.AddLabelSprite(bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Red, 70, 0, 50, 15);
statusDisplay = display.AddLabelSprite(bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Orange, 0, 20, 45, 15);
currentTimeDisplay = display.AddLabelSprite(bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Orange, 50, 20, 70, 15);
/* Instructions */
myDisplay = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Cyan, 0, 40, 120, 15);
/* Times */
longestTimeDisplay = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Blue, 0, 80, 60, 15);
shortestTimeDisplay = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Blue, 0, 100, 60, 15);
averageTimeDisplay = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Blue, 0, 120, 60, 15);
/* Test */
longestTimeDisplay_test = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Yellow, 60, 80, 60, 15);
shortestTimeDisplay_test = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Yellow, 60, 100, 60, 15);
averageTimeDisplay_test = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Yellow, 60, 120, 60, 15);
/* Random */
OriginalGCDisplay = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Blue, 0, 60, 60, 15);
TestGCDisplay = display.AddLabelSprite(smallFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Yellow, 60, 60, 60, 15);
myDisplay.SetText("Orig Blue : Test Yellow");
/* Track test version */
Debug.Print("Ver. 1.4");
/* Start the test with the Pigeon off */
digital1.Write(true);
Thread.Sleep(1000); //Allow enough time for shut down to take effect
while (true)
{
/* Always enable talon (No Motors connected) */
CTRE.Phoenix.Watchdog.Feed();
/* Update general pigeon status' */
_pidgey.GetGeneralStatus(_pidgeyStatus);
_testPidgey.GetGeneralStatus(_testPidgeyStatus);
if (checkReset)
{
/* Reset/Update information used to check if Pigeon is disconnected */
temp = _pidgeyStatus.upTimeSec;
tempTime = myStopwatch.Duration;
isPigeonStale = false;
checkReset = false;
/* Reset/Update information used to check if Pigeon is disconnected */
temp = _pidgeyStatus.upTimeSec;
tempTime = myStopwatch.Duration;
isPigeonStale = false;
checkReset = false;
}
/* Ouput values on Display Module */
/* Print current status 1 = Initialization, 2 = ready, other values only work on CAN */
statusDisplay.SetText("PS: " + state); // _pidgey.GetState());
goodCountDisplay.SetText("GC: " + goodCount);
badCountDisplay.SetText("BC: " + badCount);
longestTimeDisplay.SetText("LT: " + longestTime);
shortestTimeDisplay.SetText("ST: " + shortestTime);
averageTimeDisplay.SetText("AT: " + averageTime);
longestTimeDisplay_test.SetText("LT: " + longestTime_test);
shortestTimeDisplay_test.SetText("ST: " + shortestTime_test);
averageTimeDisplay_test.SetText("AT: " + averageTime_test);
OriginalGCDisplay.SetText("BC: " + _originalBC);
TestGCDisplay.SetText("BC: " + _testBC);
/* Print current cycle time */
currentTimeDisplay.SetText("CT: " + myStopwatch.Duration);
/* Determine if Pigeon is OFF */
if (myStopwatch.Duration > tempTime + 1)
{
/* 1 second has passed with stopwatch */
if (_pidgeyStatus.upTimeSec > temp + 1)
{
/* Uptime with pigeon has increased, update */
temp = _pidgeyStatus.upTimeSec;
tempTime = myStopwatch.Duration;
isPigeonStale = false;
}
else
{
/* Pigeon stale, notify */
isPigeonStale = true;
}
}
if (HeroButton.Read())
{
/* Pigeon One */
longestTime = 0;
shortestTime = 0;
averageTime = 0;
sum = 0;
/* Pigeon Two */
longestTime_test = 0;
shortestTime_test = 0;
averageTime_test = 0;
sum_test = 0;
}
switch (state)
{
case 0:
/* Start stopwatch */
myStopwatch.Start();
/* Reset Information */
checkReset = true;
/* Turn On Talons */
digital1.Write(false);
state = 1;
break;
case 1:
uint numOfPigeonInit = 0;
/* Ensure Pigeons are Initializing, this tells us that we have entered a new reset */
if (_pidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Initializing)
{
numOfPigeonInit++;
}
if (_testPidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Initializing)
{
numOfPigeonInit++;
}
/* Proceed to next state if Pigeon Count is good */
if (numOfPigeonInit == 2)
{
_motorStartTime = myStopwatch.Duration;
state = 2;
}
/* Timeout */
if (myStopwatch.Duration >= Timeout)
{
state = 5;
}
break;
case 2:
/* Run motor for short time */
_talon.Set(ControlMode.PercentOutput, 0.08f);
/* Run motor for 5 seconds */
if ((myStopwatch.Duration - _motorStartTime) >= 5)
{
/* Stop Talon and proceed to next state */
_talon.Set(ControlMode.PercentOutput, 0);
firstTime = true;
firstTime_test = true;
_pigeonStartTime = myStopwatch.Duration;
state = 3;
}
break;
case 3:
uint _pigeonFinishCount = 0;
if (_pidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Ready)
{
/* Regular Pigeon Finisehd */
_pigeonFinishCount++;
if (firstTime)
{
float Duration = myStopwatch.Duration - _pigeonStartTime;
if (longestTime == 0 && shortestTime == 0)
{
longestTime = Duration;
shortestTime = Duration;
}
else
{
if (Duration > longestTime)
{
longestTime = Duration;
}
else if (Duration < shortestTime)
{
shortestTime = Duration;
}
}
sum += Duration;
averageTime = (float)sum / (goodCount + 1);
firstTime = false;
}
}
if (_testPidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Ready)
{
/* Test Pigeon Finished */
_pigeonFinishCount++;
if (firstTime_test)
{
float Duration = myStopwatch.Duration - _pigeonStartTime;
if (longestTime_test == 0 && shortestTime_test == 0)
{
longestTime_test = Duration;
shortestTime_test = Duration;
}
else
{
if (Duration > longestTime_test)
{
longestTime_test = Duration;
}
else if (Duration < shortestTime_test)
{
shortestTime_test = Duration;
}
}
sum_test += Duration;
averageTime_test = (float)sum_test / (goodCount + 1);
firstTime_test = false;
}
}
/* Proceed to next state */
if (_pigeonFinishCount == 2)
{
state = 4;
}
/* Timeout */
if (myStopwatch.Duration >= Timeout)
{
state = 5;
}
break;
case 4:
/* Increment good count */
goodCount++;
Thread.Sleep(2000); // Allows us to view the pigeon after if has finished boot calibrating
digital1.Write(true); // Turn on the relay to rest the Talon/Pigeon
state = 6;
break;
case 5:
/* Increment bad count */
badCount++;
if (firstTime)
{
_originalBC++;
}
if (firstTime_test)
{
_testBC++;
}
digital1.Write(true);
state = 6;
break;
case 6:
/* Wait for Pigeons to turn off */
Thread.Sleep(1000);
if (isPigeonStale)
{
state = 0;
}
break;
default:
/* Do nothing by default */
break;
}
/* The usual thread sleep */
Thread.Sleep(5);
}
}
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);
}
}