/**
* @param gearRatio ratio between the motor output and the final geared output.
* Typically a reduction, example: ToDO
* @param integratedEncoderRatio ratio between the motor output and the sensor output.
* Typically a reduction, example: ToDO
*/
public VersaPlanetaryWithMagEnc(float slice1, TalonSRX talon)
: base(slice1, talon, FeedbackDevice.QuadEncoder)
{
}
public static void Main()
{
/* Do auton????? Maybe????? EE????? */
autonActive = true;
/* Creating our gamepad */
GameController veryBadController = new GameController(new CTRE.Phoenix.UsbHostDevice(0));
/* Talon defining. */
TalonSRX leftDriveTalon = new TalonSRX(0);
TalonSRX rightDriveTalon = new TalonSRX(1);
TalonSRX climbyTalon = new TalonSRX(2);
/* simple counter to print and watch using the debugger */
int counter = 0;
/* This loops everything inside forever. So, everything in the loop just... keeps going... assuming it works */
while (true)
{
/* this is checked periodically. Recommend every 20ms or faster */
if (veryBadController.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
/* print axis value */
Debug.Print("axis:" + veryBadController.GetAxis(1));
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
// Awfully amazing Auton
if (autonActive == true)
{
Debug.Print("Auton is attempting to E");
leftDriveTalon.Set(ControlMode.PercentOutput, 0.5); //sets left drive talon to 50%
rightDriveTalon.Set(ControlMode.PercentOutput, 0.5); //sets right drive talon to 50%
if (counter >= 600)
{
Debug.Print("Auton ending, E complete.");
leftDriveTalon.Set(ControlMode.PercentOutput, 0.0); //sets left drive talon to 0%
rightDriveTalon.Set(ControlMode.PercentOutput, 0.0); //sets right drive talon to 0%
autonActive = false;
}
;
}
else // Less awfully amazing teleop
{
/* NOTE TO HAYDON: ADD THE ACTUAL CONTROLLER AXISES IDS
* stick 1 x axis: id 2?
* stick 1 y axis: id 1?
* stick 2 x axis: id
* stick 2 y axis: id
*/
//determine inputs
forwardAxis = veryBadController.GetAxis(1);
turnAxis = veryBadController.GetAxis(2);
//accounting for forward and turn b4 Eing into the talons
finalLeftTalonValue = (forwardAxis * forwardGain) + (turnAxis * turnGain);
finalRightTalonValue = (forwardAxis * forwardGain) - (turnAxis * turnGain);
/* pass motor value to talons */
leftDriveTalon.Set(ControlMode.PercentOutput, finalLeftTalonValue);
rightDriveTalon.Set(ControlMode.PercentOutput, finalRightTalonValue);
/* the climby controls NOTE TO HAYDON: ADD THE ACTUAL CONTROLLER BUTTON IDS*/
if (veryBadController.GetButton(6808099))
{
elevatorState = ElevatorState.RISE;
}
else if (veryBadController.GetButton(9897))
{
elevatorState = ElevatorState.LOWER;
}
else
{
elevatorState = ElevatorState.NOTHING;
}
}
/* elevator state machine garbo */
switch (elevatorState)
{
case ElevatorState.NOTHING:
Debug.Print("the elevator aint doin shit");
climbyTalon.Set(ControlMode.PercentOutput, 0.0); //sets the climber talon to 0%
break;
case ElevatorState.RISE:
Debug.Print("OH LOARD HE RISEN");
climbyTalon.Set(ControlMode.PercentOutput, 0.5); //sets the climber talon to 50%
break;
case ElevatorState.LOWER:
Debug.Print("OH LOARD HE FALLEN");
climbyTalon.Set(ControlMode.PercentOutput, -0.5); //sets the climber talon to -50%
break;
case ElevatorState.F**K:
Debug.Print("what f**k");
break;
default:
Debug.Print("WHAT F**K??????///??");
break;
}
/* 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 */
/* Amount of time to wait before repeating the loop */
System.Threading.Thread.Sleep(10);
}
}
public static void Main()
{
/* Create Talons */
for (int i = 0; i < (kNumOfTalons); i++)
{
_talonCollection[i] = new TalonSRX(i + _talonStartIndex);
}
///* Set Status Frame 7, Check for Dropped Transmit */
//foreach (TalonSRX _talon in _talonCollection)
// _talon.SetStatusFramePeriod(StatusFrameEnhanced.Status_7_CommStatus, 150, kTimeout);
/* Init done, indicate current timeout value */
Debug.Print("Teleop Init completed, timeout: " + kTimeout);
int value = 0;
while (true)
{
/* Reset errorDetected and error */
CTRE.ErrorCode error;
bool errorDetected = false;
CTRE.Phoenix.Watchdog.Feed();
/* Second indexing method */
if (talonCount < kNumOfTalons)
{
/* Get current Talon Device ID */
int talonNumber = _talonCollection[talonCount].GetDeviceID();
error = _talonCollection[talonCount].Config_kP(0, 1, kTimeout); // 310
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].Config_kI(0, 1, kTimeout); // 311
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].Config_kD(0, 1, kTimeout); // 312
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].Config_kF(0, 1, kTimeout); // 313
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].Config_IntegralZone(0, 100, kTimeout); // 314
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].ConfigAllowableClosedloopError(0, 100, kTimeout); // 315
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].ConfigNominalOutputForward(0.5f, kTimeout); // 306
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].ConfigNominalOutputReverse(-0.5f, kTimeout); // 308
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].ConfigPeakOutputForward(1, kTimeout); // 305
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
error = _talonCollection[talonCount].ConfigPeakOutputReverse(-1, kTimeout); // 307
if (error != CTRE.ErrorCode.OK)
{
errorDetected = true;
}
if (!errorDetected)
{
passCount++;
}
else if (errorDetected)
{
failCount++;
}
if (talonNumber == 13)
{
/* test setselectesendosr*/
value++;
_talonCollection[talonCount].SetSelectedSensorPosition(value, 0, kTimeout);
_talonCollection[talonCount].GetSelectedSensorPosition(0);
}
byte[] temp1 = new byte[8];
ulong data1 = (ulong)BitConverter.ToUInt64(temp1, 0);
CTRE.Native.CAN.Send(0x0000A00A, data1, 8, 0);
Debug.Print("Success Count: " + passCount + " Fail Count: " + failCount + " Talon #: " + talonNumber);
talonCount++;
}
else
{
talonCount = 0;
}
Thread.Sleep(10);
}
}
public static void Process(TalonSRX talon)
{
/* simple timeout to reduce printed lines */
if (++_instrumLoops1 > 10)
{
_instrumLoops1 = 0;
/* get closed loop info */
float pos = talon.GetSelectedSensorPosition(0);
float spd = talon.GetSelectedSensorVelocity(0);
int err = talon.GetClosedLoopError(0);
/* build the string */
_sb.Clear();
_sb.Append(pos);
if (_sb.Length < 16)
{
_sb.Append(' ', 16 - _sb.Length);
}
_sb.Append(spd);
if (_sb.Length < 32)
{
_sb.Append(' ', 32 - _sb.Length);
}
_sb.Append(err);
if (_sb.Length < 48)
{
_sb.Append(' ', 48 - _sb.Length);
}
Debug.Print(_sb.ToString()); /* print data row */
if (++_instrumLoops2 > 8)
{
_instrumLoops2 = 0;
_sb.Clear();
_sb.Append("Position");
if (_sb.Length < 16)
{
_sb.Append(' ', 16 - _sb.Length);
}
_sb.Append("Velocity");
if (_sb.Length < 32)
{
_sb.Append(' ', 32 - _sb.Length);
}
_sb.Append("Error");
if (_sb.Length < 48)
{
_sb.Append(' ', 48 - _sb.Length);
}
Debug.Print(_sb.ToString()); /* print columns */
}
}
}
public static void Main()
{
/* Hardware */
TalonSRX _talon = new TalonSRX(1);
/** Use a USB gamepad plugged into the HERO */
GameController _gamepad = new GameController(UsbHostDevice.GetInstance());
/* String for output */
StringBuilder _sb = new StringBuilder();
/** hold bottom left shoulder button to enable motors */
const uint kEnableButton = 7;
/* Loop tracker for prints */
int _loops = 0;
/* Initialization */
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* Config sensor used for Primary PID [Velocity] */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
Constants.kPIDLoopIdx,
Constants.kTimeoutMs);
/**
* Phase sensor accordingly.
* Positive Sensor Reading should match Green (blinking) Leds on Talon
*/
_talon.SetSensorPhase(false);
/* Config the peak and nominal outputs */
_talon.ConfigNominalOutputForward(0, Constants.kTimeoutMs);
_talon.ConfigNominalOutputReverse(0, Constants.kTimeoutMs);
_talon.ConfigPeakOutputForward(1, Constants.kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1, Constants.kTimeoutMs);
/* Config the Velocity closed loop gains in slot0 */
_talon.Config_kF(Constants.kPIDLoopIdx, Constants.kF, Constants.kTimeoutMs);
_talon.Config_kP(Constants.kPIDLoopIdx, Constants.kP, Constants.kTimeoutMs);
_talon.Config_kI(Constants.kPIDLoopIdx, Constants.kI, Constants.kTimeoutMs);
_talon.Config_kD(Constants.kPIDLoopIdx, Constants.kD, Constants.kTimeoutMs);
/* loop forever */
while (true)
{
/* Get gamepad axis */
double leftYstick = -1 * _gamepad.GetAxis(1);
/* Get Talon/Victor's current output percentage */
double motorOutput = _talon.GetMotorOutputPercent();
/* Prepare line to print */
_sb.Append("\tout:");
/* Cast to int to remove decimal places */
_sb.Append((int)(motorOutput * 100));
_sb.Append("%"); // Percent
_sb.Append("\tspd:");
_sb.Append(_talon.GetSelectedSensorVelocity(Constants.kPIDLoopIdx));
_sb.Append("u"); // Native units
/**
* When button 1 is held, start and run Velocity Closed loop.
* Velocity Closed Loop is controlled by joystick position x2000 RPM, [-2000, 2000] RPM
*/
if (_gamepad.GetButton(1))
{
/* Velocity Closed Loop */
/**
* Convert 2000 RPM to units / 100ms.
* 4096 Units/Rev * 2000 RPM / 600 100ms/min in either direction:
* velocity setpoint is in units/100ms
*/
double targetVelocity_UnitsPer100ms = leftYstick * 2000.0 * 4096 / 600;
/* 2000 RPM in either direction */
_talon.Set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);
/* Append more signals to print when in speed mode. */
_sb.Append("\terr:");
_sb.Append(_talon.GetClosedLoopError(Constants.kPIDLoopIdx));
_sb.Append("\ttrg:");
_sb.Append(targetVelocity_UnitsPer100ms);
}
else
{
/* Percent Output */
_talon.Set(ControlMode.PercentOutput, leftYstick);
}
/* Print built string every 10 loops */
if (++_loops >= 10)
{
_loops = 0;
Debug.Print(_sb.ToString());
}
/* Reset built string */
_sb.Clear();
//if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) // check if gamepad is plugged in OR....
if (_gamepad.GetButton(kEnableButton)) // check if bottom left shoulder buttom is held down.
{
/* then enable motor outputs*/
Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(20);
}
}
public Battery(TalonSRX talon)
{
_talon = talon;
}
