uint [] _debLeftY = { 0, 0 }; // _debLeftY[0] is how many times leftY is zero, _debLeftY[1] is how many times leftY is not zeero.
public void Run()
{
/* first choose the sensor */
_talon.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_talon.SetSensorDirection(false);
//_talon.ConfigEncoderCodesPerRev(XXX), // if using CTRE.TalonSrx.FeedbackDevice.QuadEncoder
//_talon.ConfigPotentiometerTurns(XXX), // if using CTRE.TalonSrx.FeedbackDevice.AnalogEncoder or CTRE.TalonSrx.FeedbackDevice.AnalogPot
/* set closed loop gains in slot0 */
_talon.SetP(0, 0.2f); /* tweak this first, a little bit of overshoot is okay */
_talon.SetI(0, 0f);
_talon.SetD(0, 0f);
_talon.SetF(0, 0f); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0);
/* set the peak and nominal outputs, 12V means full */
_talon.ConfigNominalOutputVoltage(+0.0f, -0.0f);
_talon.ConfigPeakOutputVoltage(+3.0f, -3.0f);
/* how much error is allowed? This defaults to 0. */
_talon.SetAllowableClosedLoopErr(0, 0);
/* zero the sensor and throttle */
ZeroSensorAndThrottle();
/* loop forever */
while (true)
{
Loop10Ms();
//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*/
CTRE.Watchdog.Feed();
}
/* print signals to Output window */
Instrument();
/* 10ms loop */
Thread.Sleep(10);
}
}
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();
}
}