//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());
}
}
}
private static ArrayList readUART(ref String messageStr)
{
ArrayList setpointData = new ArrayList();
if (uart.BytesToRead > 0)
{
//Debug.Print("Here");
int readCnt = uart.Read(rx, 0, 1024);
for (int i = 0; i < readCnt; ++i)
{
messageStr += (char)rx[i];
if ((char)rx[i] == '\n')
{
// Debug.Print("Here2");
checkEncoderResetFlags(messageStr);
Debug.Print("Inbound Message String: " + messageStr);
setpointData = MessageParser.parseMessage(messageStr);
// Debug.Print("Setpoint Data: " + setpointData[4]);
// int c = setpointData.Count;
//Console.WriteLine(c);
//Debug.Print(c.ToString());
messageStr = "";
}
CTRE.Watchdog.Feed();
}
}
//Debug.Print("Value = ");
//Debug.Print("Value = " + setpointData[0]);
if (setpointData.Count > 1)
{
SetpointData leftsetdata = (SetpointData)setpointData[0];
//Debug.Print("Value: " + testsetdata.getSetpoint() + "ID: " + testsetdata.getDeviceID());
//Debug.Print("Value: " + testsetdata.getSetpoint() + " Device ID: " + testsetdata.getDeviceID());
SetpointData rightsetdata = (SetpointData)setpointData[1];
//Debug.Print("Value: " + testsetdata1.getSetpoint() + " Device ID: " + testsetdata1.getDeviceID());
CTRE.Watchdog.Feed();
}
return(setpointData);
}
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();
}
}
