public static void Main()
{
/* create a gamepad object */
CTRE.Gamepad myGamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
/* create a talon, the Talon Device ID in HERO LifeBoat is zero */
CTRE.TalonSrx myTalon = new CTRE.TalonSrx(0);
/* simple counter to print and watch using the debugger */
int counter = 0;
/* loop forever */
while (true)
{
/* added inside the while loop */
if (myGamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
/* print the axis value */
Debug.Print("axis:" + myGamepad.GetAxis(1));
/* pass axis value to talon */
myTalon.Set(myGamepad.GetAxis(1));
/* allow motor control */
CTRE.Watchdog.Feed();
}
/* increment counter */
++counter; /* try to land a breakpoint here */
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public BallLaunchModule() : base("BALL LAUNCH")
{
Winch = new CTRE.TalonSrx(WINCH_TALONSRX_ID);
Dog = new CTRE.TalonSrx(DOG_TALONSRX_ID);
State = BallLaunchState.Unknown;
Mode = FireMode.Normal;
}
public static void Main()
{
leftDrive = new CTRE.TalonSrx(1);
rightDrive = new CTRE.TalonSrx(2);
leftDrive.SetInverted(true);
leftDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
rightDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
leftDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
rightDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
leftDrive.ConfigEncoderCodesPerRev(560);
rightDrive.ConfigEncoderCodesPerRev(560);
leftDrive.SetSensorDirection(true);
leftDrive.SetEncPosition(0);
rightDrive.SetEncPosition(0);
CTRE.Gamepad gamePad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
/* loop forever */
while (true)
{
if (gamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CheesyDrive(gamePad.GetAxis(1), gamePad.GetAxis(2));
CTRE.Watchdog.Feed();
Debug.Print("" + leftDrive.GetPosition() + ":" + rightDrive.GetPosition());
}
else
{
Debug.Print("No Driver Pad");
}
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
//I think this is where the magic happens and the motors move
private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
{
//For each setpointData, for the talon that matches it set each mode and setpoint based on the list information
for (int i = 0; i < setpointDataList.Count; i++)
{
try
{
SetpointData setpointData = (SetpointData)setpointDataList[i];
float setpointVal = (float)(setpointData.getSetpoint());
CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
if (talon.GetControlMode() != setpointData.getMode())
{
talon.SetControlMode(setpointData.getMode());
//Debug.Print(setpointData.getMode().ToString());
}
if (talon.GetSetpoint() != setpointVal)
{
talon.Set(setpointVal);
Debug.Print("Setting it to value = " + setpointVal.ToString());
//Debug.Print(setpointVal.ToString());
}
}
catch (ArgumentOutOfRangeException ex)
{
Debug.Print(ex.ToString());
}
}
}
public DriveModule() : base("DRIVE")
{
/* Create our device objects */
Right = new CTRE.TalonSrx(RIGHT_TALONSRX_ID);
RightSlave = new CTRE.TalonSrx(RIGHT_SLAVE_TALONSRX_ID);
Left = new CTRE.TalonSrx(LEFT_TALONSRX_ID);
LeftSlave = new CTRE.TalonSrx(LEFT_SLAVE_TALONSRX_ID);
}
/// <summary>
/// Create a Pigeon object that communciates with Pigeon through the Gadgeteer ribbon
/// cable connected to a Talon on CAN Bus.
/// </summary>
/// <param name="talonSrx"> Talon SRX object that Pigeon is connected to.</param>
public PigeonImu(CTRE.TalonSrx talonSrx)
{
_deviceId = (UInt32)0x02000000 | talonSrx.GetDeviceNumber();
CTRE.Native.CAN.Send(CONTROL_1 | _deviceId, 0x00000000, 0, 100);
_ParamContainer = new ParamContainer(_deviceId, PARAM_REQUEST, PARAM_RESPONSE, PARAM_SET);
}
//Password to module wifi is "password1"
/** entry point of the application */
public static void Main()
{
ds = new CTRE.FRC.DriverStation(wifiport); //Create new Driver station object at port 4
_gamepad = new CTRE.Controller.GameController(ds, 0); //Set controller to look at DS with ID 0
//Create two TalonSrx objects and set their control mode
left1 = new CTRE.TalonSrx(1);
right1 = new CTRE.TalonSrx(2);
left1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
right1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
//Set IP module looks for and is configured with, Computer must set to Static IP
ds.SendIP(new byte[] { 10, 0, 33, 2 }, new byte[] { 10, 0, 33, 5 });
while (true)
{
//Must be called for DS to function
ds.update();
//Send Battery Voltage information to Driver Station
ds.SendBattery(left1.GetBusVoltage());
if (ds.IsAuton() && ds.IsEnabled())
{
//Auton Code while enabled
}
else if (ds.IsAuton() && !ds.IsEnabled())
{
//Auton Code while disabled
}
else if (!ds.IsAuton() && ds.IsEnabled())
{
//Teleop Code while enabled
Drive();
Debug.Print(stringBuilder.ToString());
ds.SendUDP(2550, Encoding.UTF8.GetBytes(stringBuilder.ToString()));
stringBuilder.Clear();
}
else if (!ds.IsAuton() && !ds.IsEnabled())
{
//Teleop Code while disabled
}
}
}
private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
{
for (int i = 0; i < setpointDataList.Count; i++)
{
SetpointData setpointData = (SetpointData)setpointDataList[i];
float setpointVal = (float)(setpointData.getSetpoint());
CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
if (talon.GetControlMode() != setpointData.getMode())
{
talon.SetControlMode(setpointData.getMode());
Debug.Print(setpointData.getMode().ToString());
}
if (talon.GetSetpoint() != setpointVal)
{
talon.Set(setpointVal);
Debug.Print(setpointVal.ToString());
}
}
}
public static void Process(CTRE.TalonSrx talon)
{
/* simple timeout to reduce printed lines */
if (++_instrumLoops1 > 10)
{
_instrumLoops1 = 0;
/* get closed loop info */
float pos = talon.GetPosition();
float spd = talon.GetSpeed();
int err = talon.GetClosedLoopError();
/* 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 void RunForever()
{
int toPrint = 0;
int loops = 0;
CTRE.TalonSrx _talon = new CTRE.TalonSrx(0); /* make a talon with deviceId 0 */
CTRE.PigeonImu _pigeon;
/** Use a USB gamepad plugged into the HERO */
CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance());
CTRE.GamepadValues _gamepadValues = new CTRE.GamepadValues();
uint _oldBtns = 0;
StringBuilder _sb = new StringBuilder();
/** choose pigeon connected to Talon or on CAN Bus */
if (true)
{
_pigeon = new CTRE.PigeonImu(0); /* Pigeon is on CAN Bus with device ID 0 */
}
else
{
_pigeon = new CTRE.PigeonImu(_talon); /* Pigeon is connected to _talon via Gadgeteer cable.*/
}
/* set frame rates */
_pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.BiasedStatus_4_Mag, 17);
_pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_1_General, 217);
_pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_11_GyroAccum, 9999);
_pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_9_SixDeg_YPR, 0);
_pigeon.SetAccumZAngle(217);
/* loop forever */
while (true)
{
int ret = 0;
/* signals for motion */
float[] ypr = new float[3];
float[] wxyz = new float[4];
float compassHeading, magnitudeMicroTeslas;
float tempC;
CTRE.PigeonImu.PigeonState state;
Int16[] rmxyz = new Int16[3];
Int16[] bmxyz = new Int16[3];
Int16[] baxyz = new short[3];
Int16 baNorm;
float[] xyz_dps = new float[3];
float [] tilts_XZ_YZ_XY = new float[3];
UInt32 timeSec;
float fusedHeading;
CTRE.PigeonImu.FusionStatus fusionStatus = new CTRE.PigeonImu.FusionStatus();
CTRE.PigeonImu.GeneralStatus generalStatus = new CTRE.PigeonImu.GeneralStatus();
/* get sigs */
ret |= _pigeon.GetYawPitchRoll(ypr);
ret |= _pigeon.Get6dQuaternion(wxyz);
compassHeading = _pigeon.GetCompassHeading(); ret |= _pigeon.GetLastError();
magnitudeMicroTeslas = _pigeon.GetCompassFieldStrength(); ret |= _pigeon.GetLastError();
tempC = _pigeon.GetTemp(); ret |= _pigeon.GetLastError();
state = _pigeon.GetState(); ret |= _pigeon.GetLastError();
ret |= _pigeon.GetRawMagnetometer(rmxyz);
ret |= _pigeon.GetBiasedMagnetometer(bmxyz);
ret |= _pigeon.GetBiasedAccelerometer(baxyz, out baNorm);
ret |= _pigeon.GetRawGyro(xyz_dps);
ret |= _pigeon.GetAccelerometerAngles(tilts_XZ_YZ_XY);
timeSec = _pigeon.GetUpTime();
fusedHeading = _pigeon.GetFusedHeading(fusionStatus);
ret |= _pigeon.GetGeneralStatus(generalStatus);
/* get buttons */
_gamepad.GetAllValues(ref _gamepadValues);
if (_gamepadValues.pov == 0) // up
{
_pigeon.EnableTemperatureCompensation(true);
}
if (_gamepadValues.pov == 4) // dn
{
_pigeon.EnableTemperatureCompensation(false);
}
if (_oldBtns != _gamepadValues.btns)
{
int btnIdx = GetButtonFromBits(_gamepadValues.btns);
switch (btnIdx)
{
case 4:
if (toPrint < kToPrintMaxInclus)
{
++toPrint;
}
break;
case 2:
if (toPrint > kToPrintMin)
{
--toPrint;
}
break;
case 5: // top left
_pigeon.SetYaw(100);
_pigeon.SetFusedHeading(100);
break;
case 7: // btm left
break;
case 6: // top right
_pigeon.SetYawToCompass();
_pigeon.SetFusedHeadingToCompass();
break;
case 8: // btm right
_pigeon.AddYaw(45);
_pigeon.AddYaw(45);
break;
case 10: // enter mag cal start btn
_pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature);
break;
case 9: // accel 0,0,
_pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature);
break;
}
_oldBtns = _gamepadValues.btns;
}
if (++loops >= 20)
{
loops = 0;
/* build row */
switch (toPrint)
{
case 0: _sb.Append("YPR:"); _sb.Append(ToString(ypr)); break;
case 1: _sb.Append("FusedHeading:"); _sb.Append(fusedHeading); _sb.Append(" deg, "); _sb.Append(fusionStatus.description); break;
case 2: _sb.Append("Compass:"******" deg, "); _sb.Append(magnitudeMicroTeslas); _sb.Append("uT"); break;
case 3: _sb.Append("tempC:"); _sb.Append(tempC); _sb.AppendLine(); break;
case 4: _sb.Append("PigeonState:"); _sb.Append(state); _sb.AppendLine(); break;
case 5: _sb.Append("BiasedM:"); _sb.Append(ToString(bmxyz)); break;
case 6: _sb.Append("RawM:"); _sb.Append(ToString(rmxyz)); break;
case 7: _sb.Append("BiasedAccel:"); _sb.Append(ToString(baxyz)); break;
case 8: _sb.Append("GyroDps:"); _sb.Append(ToString(xyz_dps)); break;
case 9: _sb.Append("AccelTilts:"); _sb.Append(ToString(tilts_XZ_YZ_XY)); break;
case 10: _sb.Append("Quat:"); _sb.Append(ToString(wxyz)); break;
case 11: _sb.Append("UpTime:"); _sb.Append(timeSec); break;
case 12: _sb.Append("GS:"); _sb.Append(generalStatus.description); break;
case 13: _sb.Append("tempCompensationCount:"); _sb.Append(generalStatus.tempCompensationCount);
_sb.Append(" tempC:"); _sb.Append(generalStatus.tempC);
_sb.Append(" noMotionBiasCount:"); _sb.Append(generalStatus.noMotionBiasCount);
break;
}
kToPrintMin = 0;
kToPrintMaxInclus = 13;
//_sb.Append(yaw); _sb.Append(","); _sb.Append(pitch); _sb.Append(","); _sb.Append(roll); _sb.AppendLine();
//_sb.Append(w); _sb.Append(","); _sb.Append(x); _sb.Append(","); _sb.Append(y); _sb.Append(","); _sb.Append(z); _sb.AppendLine();
//_sb.Append(compassHeading); _sb.Append(","); _sb.Append(magnitudeMicroTeslas); _sb.AppendLine();
//_sb.Append(tempC); _sb.AppendLine();
//_sb.Append(state); _sb.AppendLine();
//_sb.Append(rmx); _sb.Append(","); _sb.Append(rmy); _sb.Append(","); _sb.Append(rmz); _sb.AppendLine();
//_sb.Append(bmx); _sb.Append(","); _sb.Append(bmy); _sb.Append(","); _sb.Append(bmz); _sb.AppendLine();
//_sb.Append(bam); _sb.Append(","); _sb.Append(bay); _sb.Append(","); _sb.Append(baz); _sb.Append(","); _sb.Append(baNorm); _sb.AppendLine();
//_sb.Append(x_dps); _sb.Append(","); _sb.Append(y_dps); _sb.Append(","); _sb.Append(z_dps); _sb.AppendLine();
//_sb.Append(tiltXZ); _sb.Append(","); _sb.Append(tiltYZ); _sb.Append(","); _sb.Append(tiltXY); _sb.AppendLine();
//_sb.Append(timeSec); _sb.Append(",");
//_sb.Append(fusedHeading); _sb.Append(","); _sb.Append(bIsValid); _sb.Append(","); _sb.Append(bIsFusing); _sb.AppendLine();
//_sb.Append(ret); _sb.Append(",");
}
/* print line and clear buffer */
if (_sb.Length != 0)
{
Debug.Print(_sb.ToString());
_sb.Clear();
}
/* 10ms loop */
Thread.Sleep(10);
if (_pigeon.HasResetOccured())
{
int resetCount;
int resetFlags;
int firmVers;
resetCount = _pigeon.GetResetCount();
resetFlags = _pigeon.GetResetFlags();
firmVers = _pigeon.GetFirmVers();
Debug.Print("Pigeon Reset detected...");
Debug.Print(" Pigeon Reset Count:" + resetCount.ToString("D"));
Debug.Print(" Pigeon Reset Flags:" + resetFlags.ToString("X"));
Debug.Print(" Pigeon FirmVers:" + firmVers.ToString("X"));
Debug.Print(" Waiting for delay");
Thread.Sleep(3000);
}
}
}
public StatusData(int talonDeviceID, CTRE.TalonSrx talon)
{
this.talonDeviceID = talonDeviceID;
this.talon = talon;
}
public static void Main()
{
CTRE.TalonSrx leftMotor = new CTRE.TalonSrx(1);
CTRE.TalonSrx rightMotor = new CTRE.TalonSrx(2);
CTRE.TalonSrx scoopMotor = new CTRE.TalonSrx(3);
CTRE.TalonSrx depthMotor = new CTRE.TalonSrx(4);
CTRE.TalonSrx winchMotor = new CTRE.TalonSrx(5);
leftMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
rightMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
scoopMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
depthMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
winchMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
leftMotor.SetSensorDirection(false);
rightMotor.SetSensorDirection(true);
scoopMotor.SetSensorDirection(false);
depthMotor.SetSensorDirection(false);
winchMotor.SetSensorDirection(false);
leftMotor.ConfigEncoderCodesPerRev(80);
rightMotor.ConfigEncoderCodesPerRev(80);
scoopMotor.ConfigEncoderCodesPerRev(80);
depthMotor.ConfigEncoderCodesPerRev(80);
winchMotor.ConfigEncoderCodesPerRev(80);
leftMotor.SetP(0, 0.35F);
leftMotor.SetI(0, 0.0F);
leftMotor.SetD(0, 0.0F);
leftMotor.SetF(0, 0.0F);
leftMotor.SelectProfileSlot(0);
rightMotor.SetP(0, 0.35F);
rightMotor.SetI(0, 0.0F);
rightMotor.SetD(0, 0.0F);
rightMotor.SetF(0, 0.0F);
rightMotor.SelectProfileSlot(0);
scoopMotor.SetP(0, 0.6F);
scoopMotor.SetI(0, 0.0F);
scoopMotor.SetD(0, 0.0F);
scoopMotor.SetF(0, 0.0F);
scoopMotor.SelectProfileSlot(0);
depthMotor.SetP(0, 0.6F);
depthMotor.SetI(0, 0.0F);
depthMotor.SetD(0, 0.0F);
depthMotor.SetF(0, 0.0F);
depthMotor.SelectProfileSlot(0);
winchMotor.SetP(0, 0.6F);
winchMotor.SetI(0, 0.0F);
winchMotor.SetD(0, 0.0F);
winchMotor.SetF(0, 0.0F);
winchMotor.SelectProfileSlot(0);
leftMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
rightMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
scoopMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
depthMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
winchMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
leftMotor.SetAllowableClosedLoopErr(0, 0);
rightMotor.SetAllowableClosedLoopErr(0, 0);
scoopMotor.SetAllowableClosedLoopErr(0, 0);
depthMotor.SetAllowableClosedLoopErr(0, 0);
winchMotor.SetAllowableClosedLoopErr(0, 0);
leftMotor.SetPosition(0);
rightMotor.SetPosition(0);
scoopMotor.SetPosition(0);
depthMotor.SetPosition(0);
winchMotor.SetPosition(0);
leftMotor.SetVoltageRampRate(0);
rightMotor.SetVoltageRampRate(0);
scoopMotor.SetVoltageRampRate(0);
depthMotor.SetVoltageRampRate(0);
winchMotor.SetVoltageRampRate(0);
ArrayList motorSetpointData = new ArrayList();
ArrayList motorStatusData = new ArrayList();
ArrayList talons = new ArrayList();
talons.Add(leftMotor);
talons.Add(rightMotor);
talons.Add(scoopMotor);
talons.Add(depthMotor);
talons.Add(winchMotor);
String inboundMessageStr = "";
String outboundMessageStr = "";
SetpointData leftMotorSetpointData = new SetpointData(1, 0, 0.0F);
SetpointData rightMotorSetpointData = new SetpointData(2, 0, 0.0F);
SetpointData scoopMotorSetpointData = new SetpointData(3, 0, 0.0F);
SetpointData depthMotorSetpointData = new SetpointData(4, 0, 0.0F);
SetpointData winchMotorSetpointData = new SetpointData(5, 0, 0.0F);
StatusData leftMotorStatusData = new StatusData(1, leftMotor);
StatusData rightMotorStatusData = new StatusData(2, rightMotor);
StatusData scoopMotorStatusData = new StatusData(3, scoopMotor);
StatusData depthMotorStatusData = new StatusData(4, depthMotor);
StatusData winchMotorStatusData = new StatusData(5, winchMotor);
motorSetpointData.Add(leftMotorSetpointData);
motorSetpointData.Add(rightMotorSetpointData);
motorSetpointData.Add(scoopMotorSetpointData);
motorSetpointData.Add(depthMotorSetpointData);
motorSetpointData.Add(winchMotorSetpointData);
motorStatusData.Add(leftMotorStatusData);
motorStatusData.Add(rightMotorStatusData);
motorStatusData.Add(scoopMotorStatusData);
motorStatusData.Add(depthMotorStatusData);
motorStatusData.Add(winchMotorStatusData);
CTRE.Watchdog.Feed();
uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
uart.Open();
while (true)
{
//read whatever is available from the UART into the inboundMessageStr
motorSetpointData = readUART(ref inboundMessageStr);
CTRE.Watchdog.Feed();
//if any of the talon positions need to be reset, this will reset them
resetEncoderPositions(talons);
CTRE.Watchdog.Feed();
//attempt to process whatever was contained in the most recent message
processInboundData(motorSetpointData, talons);
CTRE.Watchdog.Feed();
//get a bunch of data from the motors in their current states
updateMotorStatusData(motorStatusData);
CTRE.Watchdog.Feed();
//package that motor data into a formatted message
outboundMessageStr = makeOutboundMessage(motorStatusData);
CTRE.Watchdog.Feed();
//send that message back to the main CPU
writeUART(outboundMessageStr);
//Debug.Print("set=" + winchMotor.GetSetpoint().ToString() + " mod=" + winchMotor.GetControlMode().ToString() +
// "pos=" + winchMotor.GetPosition().ToString() + " vel=" + winchMotor.GetSpeed().ToString() +
// "err=" + winchMotor.GetClosedLoopError().ToString() + "vlt=" + winchMotor.GetOutputVoltage());
//Debug.Print(DateTime.Now.ToString());
CTRE.Watchdog.Feed();
//keep the loop timing consistent //TODO: evaluate if this is necessary
//System.Threading.Thread.Sleep(10);
}
}
public static void Main()
{
//Set up the motors
CTRE.TalonSrx motor = new CTRE.TalonSrx(1);
/* CTRE.TalonSrx rightMotor = new CTRE.TalonSrx(2);
* CTRE.TalonSrx scoopMotor = new CTRE.TalonSrx(3);
* CTRE.TalonSrx depthMotor = new CTRE.TalonSrx(4);
* CTRE.TalonSrx winchMotor = new CTRE.TalonSrx(5);
*/
//Set encoders for each motor
motor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
/*
* rightMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
* scoopMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
* depthMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
* winchMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
*/
//Set direction of the encoders
motor.SetSensorDirection(false);
/* rightMotor.SetSensorDirection(true);
* scoopMotor.SetSensorDirection(false);
* depthMotor.SetSensorDirection(false);
* winchMotor.SetSensorDirection(false);
*/
//Set Ticks per Rev of the encoders
motor.ConfigEncoderCodesPerRev(80);
/* rightMotor.ConfigEncoderCodesPerRev(80);
* scoopMotor.ConfigEncoderCodesPerRev(80);
* depthMotor.ConfigEncoderCodesPerRev(80);
* winchMotor.ConfigEncoderCodesPerRev(80);
*/
//Sets PIDF values for each motor//
motor.SetP(0, 0.35F);
motor.SetI(0, 0.0F);
motor.SetD(0, 0.0F);
motor.SetF(0, 0.0F);
motor.SelectProfileSlot(0);
/* rightMotor.SetP(0, 0.35F);
* rightMotor.SetI(0, 0.0F);
* rightMotor.SetD(0, 0.0F);
* rightMotor.SetF(0, 0.0F);
* rightMotor.SelectProfileSlot(0);
*
* scoopMotor.SetP(0, 0.6F);
* scoopMotor.SetI(0, 0.0F);
* scoopMotor.SetD(0, 0.0F);
* scoopMotor.SetF(0, 0.0F);
* scoopMotor.SelectProfileSlot(0);
*
* depthMotor.SetP(0, 0.6F);
* depthMotor.SetI(0, 0.0F);
* depthMotor.SetD(0, 0.0F);
* depthMotor.SetF(0, 0.0F);
* depthMotor.SelectProfileSlot(0);
*
* winchMotor.SetP(0, 0.6F);
* winchMotor.SetI(0, 0.0F);
* winchMotor.SetD(0, 0.0F);
* winchMotor.SetF(0, 0.0F);
* winchMotor.SelectProfileSlot(0);
* //////////////////////////////////
*/
//Sets Nominal Output Voltage for each motor
motor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
/*rightMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
* scoopMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
* depthMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
* winchMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
*/
// Set allowed error for closed loop feedback
motor.SetAllowableClosedLoopErr(0, 0);
/*rightMotor.SetAllowableClosedLoopErr(0, 0);
* scoopMotor.SetAllowableClosedLoopErr(0, 0);
* depthMotor.SetAllowableClosedLoopErr(0, 0);
* winchMotor.SetAllowableClosedLoopErr(0, 0);
*/
//Set Initial position of the motors
motor.SetPosition(0);
/* rightMotor.SetPosition(0);
* scoopMotor.SetPosition(0);
* depthMotor.SetPosition(0);
* winchMotor.SetPosition(0);
*/
//Sets Voltage Ramp rate of each motor
motor.SetVoltageRampRate(0);
/*rightMotor.SetVoltageRampRate(0);
* scoopMotor.SetVoltageRampRate(0);
* depthMotor.SetVoltageRampRate(0);
* winchMotor.SetVoltageRampRate(0);
*/
ArrayList motorSetpointData = new ArrayList();
ArrayList motorStatusData = new ArrayList();
ArrayList talons = new ArrayList();
//Add talons to each motor
talons.Add(motor);
/*talons.Add(rightMotor);
* talons.Add(scoopMotor);
* talons.Add(depthMotor);
* talons.Add(winchMotor);
*/
//Initializes inbound and outbound message strings to empty
String inboundMessageStr = "";
String outboundMessageStr = "";
//Initializes and adds the SetpointData and the StatusData for each motor (ID, mode, setpoint//
SetpointData motor = new SetpointData(1, 0, 0.0F);
/*SetpointData rightMotorSetpointData = new SetpointData(2, 0, 0.0F);
* SetpointData scoopMotorSetpointData = new SetpointData(3, 0, 0.0F);
* SetpointData depthMotorSetpointData = new SetpointData(4, 0, 0.0F);
* SetpointData winchMotorSetpointData = new SetpointData(5, 0, 0.0F);
*/
StatusData motor = new StatusData(1, motor);
/*StatusData rightMotorStatusData = new StatusData(2, rightMotor);
* StatusData scoopMotorStatusData = new StatusData(3, scoopMotor);
* StatusData depthMotorStatusData = new StatusData(4, depthMotor);
* StatusData winchMotorStatusData = new StatusData(5, winchMotor);
*/
motorSetpointData.Add(motor);
/*motorSetpointData.Add(rightMotorSetpointData);
* motorSetpointData.Add(scoopMotorSetpointData);
* motorSetpointData.Add(depthMotorSetpointData);
* motorSetpointData.Add(winchMotorSetpointData);
*/
motorStatusData.Add(motor);
/*motorStatusData.Add(rightMotorStatusData);
* motorStatusData.Add(scoopMotorStatusData);
* motorStatusData.Add(depthMotorStatusData);
* motorStatusData.Add(winchMotorStatusData);*/
//////////////////////////////////////////
CTRE.Watchdog.Feed();
//Open up UART communication to the linux box
uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
uart.Open();
//The loop
while (true)
{
//read whatever is available from the UART into the inboundMessageStr
motorSetpointData = readUART(ref inboundMessageStr);
CTRE.Watchdog.Feed();
//if any of the talon positions need to be reset, this will reset them
resetEncoderPositions(talons);
CTRE.Watchdog.Feed();
//attempt to process whatever was contained in the most recent message
processInboundData(motorSetpointData, talons);
CTRE.Watchdog.Feed();
//get a bunch of data from the motors in their current states
updateMotorStatusData(motorStatusData);
CTRE.Watchdog.Feed();
//package that motor data into a formatted message
outboundMessageStr = makeOutboundMessage(motorStatusData);
CTRE.Watchdog.Feed();
//send that message back to the main CPU
writeUART(outboundMessageStr);
CTRE.Watchdog.Feed();
}
}
public Battery(CTRE.TalonSrx talon)
{
_talon = talon;
}
