//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());
}
}
}
void Drive()
{
FillBtns(ref _btns);
float y = -1 * _gamepad.GetAxis(1);
Deadband(ref y);
_talon.ProcessMotionProfileBuffer();
/* button handler, if btn5 pressed launch MP, if btn7 pressed, enter voltage mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kMotionProfile);
_talon.Set(0);
CTRE.Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
CTRE.TalonSrx.TrajectoryPoint point = new CTRE.TalonSrx.TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0];
point.velocity = (float)MotionProfile.Points[i][1];
point.timeDurMs = MotionProfile.kDurationMs;
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.velocityOnly = false;
point.profileSlotSelect = 0;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
CTRE.Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);
}
/* if in voltage mode, update output voltage */
if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kVoltage)
{
_talon.Set(14.0f * y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
//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 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);
}
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());
}
}
}
