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);
}
}
public override Int32 Begin()
{
#if DEBUG
Debug.Print(ToString() + " [BEGIN]");
#endif
/* Setup Left and Right followers */
RightSlave.SetControlMode(CTRE.TalonSrx.ControlMode.kFollower);
RightSlave.Set(RIGHT_TALONSRX_ID);
LeftSlave.SetControlMode(CTRE.TalonSrx.ControlMode.kFollower);
LeftSlave.Set(LEFT_TALONSRX_ID);
/* Configure the Left TalonSRX */
Left.SetInverted(LEFT_INVT);
Left.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
Left.ConfigFwdLimitSwitchNormallyOpen(true);
Left.ConfigRevLimitSwitchNormallyOpen(true);
LeftSlave.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
LeftSlave.ConfigFwdLimitSwitchNormallyOpen(true);
LeftSlave.ConfigRevLimitSwitchNormallyOpen(true);
/* Configure the Right TalonSRX */
Right.SetInverted(!LEFT_INVT);
Right.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
Right.ConfigFwdLimitSwitchNormallyOpen(true);
Right.ConfigRevLimitSwitchNormallyOpen(true);
RightSlave.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
RightSlave.ConfigFwdLimitSwitchNormallyOpen(true);
RightSlave.ConfigRevLimitSwitchNormallyOpen(true);
if (USE_SPEED_MODE)
{
/* Setup Left and Right leaders with PID */
Right.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
Right.SetSensorDirection(false);
Right.ConfigEncoderCodesPerRev(RIGHT_EncTPR);
Right.SetControlMode(CTRE.TalonSrx.ControlMode.kSpeed);
Right.SetPID(0, RIGHT_P, RIGHT_I, RIGHT_D);
Right.SetF(0, RIGHT_F);
Left.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
Left.SetSensorDirection(false);
Left.ConfigEncoderCodesPerRev(LEFT_EncTPR);
Left.SetControlMode(CTRE.TalonSrx.ControlMode.kSpeed);
Left.SetPID(0, LEFT_P, LEFT_I, LEFT_D);
Left.SetF(0, LEFT_F);
}
/* Enable the TalonSRXs */
Right.Enable();
RightSlave.Enable();
Left.Enable();
LeftSlave.Enable();
return(0);
}
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();
}
}