void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* get the left y stick, invert so forward is positive */
float leftY = kJoystickScaler * _gamepad.GetAxis(1);
Deadband(ref leftY);
/* debounce the transition from nonzero => zero axis */
float filteredY = leftY;
if (filteredY != 0)
{
/* put in a ramp to prevent the user from flipping their mechanism */
_talon.SetVoltageRampRate(12.0f); /* V per sec */
/* directly control the output */
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
_talon.Set(filteredY);
}
else if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kPercentVbus)
{
_targetPosition = _talon.GetPosition();
/* user has let go of the stick, lets closed-loop whereever we happen to be */
EnableClosedLoop();
}
/* if a button is pressed while stick is let go, servo position */
if (filteredY == 0)
{
if (_btns[1])
{
_targetPosition = _talon.GetPosition(); /* twenty rotations forward */
EnableClosedLoop();
}
else if (_btns[4])
{
_targetPosition = +10.0f; /* twenty rotations forward */
EnableClosedLoop();
}
else if (_btns[2])
{
_targetPosition = -10.0f; /* twenty rotations reverese */
EnableClosedLoop();
}
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
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);
}
}
//reads the talons and updates the data
public void updateStatusData()
{
talonCurrent = (talon.GetOutputCurrent());
talonTemperature = (talon.GetTemperature());
talonVoltage = (talon.GetOutputVoltage());
talonSpeed = (talon.GetSpeed());
talonPosition = (talon.GetPosition());
talonSetpoint = (talon.GetSetpoint());
talonForwardLimitReached = talon.IsFwdLimitSwitchClosed() ? 1 : 0;
talonReverseLimitReached = talon.IsRevLimitSwitchClosed() ? 1 : 0;
controlMode = talon.GetControlMode();
}
void Instrument()
{
if (--_timeToColumns <= 0)
{
_timeToColumns = 400;
_sb.Clear();
_sb.Append("topCnt \t");
_sb.Append("btmCnt \t");
_sb.Append("setval \t");
_sb.Append("HasUndr\t");
_sb.Append("IsUnder\t");
_sb.Append(" IsVal \t");
_sb.Append(" IsLast\t");
_sb.Append("VelOnly\t");
_sb.Append(" TargetPos[AndVelocity] \t");
_sb.Append("Pos[AndVelocity]");
Debug.Print(_sb.ToString());
}
if (--_timeToPrint <= 0)
{
_timeToPrint = 40;
_sb.Clear();
_sb.Append(_motionProfileStatus.topBufferCnt);
_sb.Append("\t\t");
_sb.Append(_motionProfileStatus.btmBufferCnt);
_sb.Append("\t\t");
_sb.Append(_motionProfileStatus.outputEnable);
_sb.Append("\t\t");
_sb.Append(_motionProfileStatus.hasUnderrun ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.isUnderrun ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePointValid ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePoint.isLastPoint ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePoint.velocityOnly ? " 1 \t" : " \t");
_sb.Append(_motionProfileStatus.activePoint.position);
_sb.Append("[");
_sb.Append(_motionProfileStatus.activePoint.velocity);
_sb.Append("]\t");
_sb.Append("\t\t\t");
_sb.Append(_talon.GetPosition());
_sb.Append("[");
_sb.Append(_talon.GetSpeed());
_sb.Append("]");
Debug.Print(_sb.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 static void Main()
{
//Set up the motors
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);
//Set encoders for each motor
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);
//Set direction of the encoders
leftmotor.SetSensorDirection(false);
rightmotor.SetSensorDirection(false);
scoopMotor.SetSensorDirection(false);
depthMotor.SetSensorDirection(false);
winchMotor.SetSensorDirection(false);
//Set Ticks per Rev of the encoders
leftmotor.ConfigEncoderCodesPerRev(80);
rightmotor.ConfigEncoderCodesPerRev(80);
scoopMotor.ConfigEncoderCodesPerRev(80);
depthMotor.ConfigEncoderCodesPerRev(50);
winchMotor.ConfigEncoderCodesPerRev(80);
//Sets PIDF values for each motor//
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);
//////////////////////////////////
//Sets Nominal Output Voltage for each motor
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);
// Set allowed error for closed loop feedback
leftmotor.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
leftmotor.SetPosition(0);
rightmotor.SetPosition(0);
scoopMotor.SetPosition(0);
depthMotor.SetPosition(0);
winchMotor.SetPosition(0);
//Sets Voltage Ramp rate of each motor
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();
//Add talons to each motor
talons.Add(leftmotor);
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 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();
//Open up UART communication to the linux box
uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
//uart.DiscardInBuffer();
uart.Open();
//uart.DiscardInBuffer();
//The loop
while (true)
{
//read whatever is available from the UART into the inboundMessageStr
motorSetpointData = readUART(ref inboundMessageStr);
CTRE.Watchdog.Feed();
if (motorSetpointData.Count > 0)
{
//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);
//Debug.Print(outboundMessageStr);
CTRE.Watchdog.Feed();
/*string[] teststring = outboundMessageStr.Split(':');
* Debug.Print(" Enc:" + teststring[5] + " Enc2:"+ teststring[14]);
* CTRE.Watchdog.Feed();*/
//send that message back to the main CPU
//Debug.Print(outboundMessageStr);
//if (uart.IsOpen) {
Debug.Print("Depth Encoder Value: " + depthMotor.GetPosition());
writeUART(outboundMessageStr);
CTRE.Watchdog.Feed();
//}
CTRE.Watchdog.Feed();
//Debug.Print(testmotor.GetPosition().ToString());
//CTRE.Watchdog.Feed();
}
}
