void ResetTargetPosition()
{
_targetPosition = _talon.GetSelectedSensorPosition(0);
_talon.SetSelectedSensorPosition(_targetPosition, 0); //Sets current and desired positions to be equal so we don't move unexpectedly at startup
Thread.Sleep(100); //wait to make sure SetPosition takes effect
}
private bool GoToActuatorPosition(int position)
{
int distanceToGo = position - leftActuator.GetSelectedSensorPosition();
int direction = distanceToGo > 0 ? 1 : -1;
const float magnitude = 1.0f;
if (!stopped && (distanceToGo * direction) < 2)
{
stopped = true;
}
else if (stopped && (distanceToGo * direction) > 5)
{
stopped = false;
}
if (!stopped)
{
int posDifference = leftActuator.GetSelectedSensorPosition() - rightActuator.GetSelectedSensorPosition();
int sign = posDifference > 0 ? 1 : -1;
const float P_Value = .05f;
float lSpeed = (direction * magnitude) - (P_Value * posDifference);
float rSpeed = (direction * magnitude) + (P_Value * posDifference);
leftActuator.Set(ControlMode.PercentOutput, lSpeed);
rightActuator.Set(ControlMode.PercentOutput, rSpeed);
// Debug.Print(posDifference.ToString());
return(false);
}
StopAll();
return(true);
}
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)
{
/* directly control the output */
_talon.Set(ControlMode.PercentOutput, filteredY);
}
else if (_talon.GetControlMode() == ControlMode.PercentOutput)
{
_targetPosition = _talon.GetSelectedSensorPosition(0);
/* 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.GetSelectedSensorPosition(0); /* current position */
EnableClosedLoop();
}
else if (_btns[4])
{
_targetPosition = +40960.0f; /* ten rotations forward */
EnableClosedLoop();
}
else if (_btns[2])
{
_targetPosition = -40960.0f; /* ten rotations reverse */
EnableClosedLoop();
}
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
private void RunActuators(ref XboxController controller)
{
//Update Encoders
Utils.Print("R, L CURRENT");
Utils.Print(OUTPUT);
Utils.Print(RightActuator.GetSelectedSensorPosition());
Utils.Print("");
//Run Motors
if (controller.POV == controller.POV_UP)
{
OUTPUT = 300;
}
else if (controller.POV == controller.POV_DOWN)
{
OUTPUT = 800;
}
//if ((OUTPUT < RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() > 1)
// || (OUTPUT > RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() < 1))
//{
// LeftActuator.Set(ControlMode.PercentOutput, 0);
// RightActuator.Set(ControlMode.PercentOutput, 0);
//}
//else
//{
LeftActuator.Set(ControlMode.MotionMagic, OUTPUT);
RightActuator.Set(ControlMode.MotionMagic, OUTPUT);
//}
}
static float getHoodAngle()
{
int sensorPos = hood.GetSelectedSensorPosition();
return(linRelate(sensorPos, HOOD_LOWER_BOUND_ANALOG, HOOD_UPPER_BOUND_ANALOG,
HOOD_LOWER_BOUND_ANGLE, HOOD_UPPER_BOUND_ANGLE));
}
public double Get_distance()
{
int l = left_motor.GetSelectedSensorPosition();
int r = right_motor.GetSelectedSensorPosition();
double average = (l + r) * 0.5; //Finds average of what the wheel distance
//Finds the distance traveled using the encoders
double revolutions = average / Counts; //Finds the counts per revolution
double inches = (WheelDiameter * System.Math.PI) * revolutions; //Finds how many inches will be travelled per revolution
// double inches_traveled = inches * Drive_Gear_Ratio; //Finds the inches travelled using gear ratio
double inches_traveled = inches / Drive_Gear_Ratio; //Finds the inches travelled using gear ratio
return(inches_traveled);
}
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.isLast ? " 1 \t" : " \t");
_sb.Append(_talon.GetActiveTrajectoryPosition());
_sb.Append("[");
_sb.Append(_talon.GetActiveTrajectoryVelocity());
_sb.Append("]\t");
_sb.Append("\t\t\t");
_sb.Append(_talon.GetSelectedSensorPosition(0));
_sb.Append("[");
_sb.Append(_talon.GetSelectedSensorVelocity(0));
_sb.Append("]");
Debug.Print(_sb.ToString());
}
}
public static void Process(TalonSRX talon)
{
/* simple timeout to reduce printed lines */
if (++_instrumLoops1 > 10)
{
_instrumLoops1 = 0;
/* get closed loop info */
float pos = talon.GetSelectedSensorPosition(0);
float spd = talon.GetSelectedSensorVelocity(0);
int err = talon.GetClosedLoopError(0);
/* 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 */
}
}
}
static void Initialize()
{
// Serial port
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
_uart.Open();
// Victor SPX Slaves
// Left Slave
victor1.Set(ControlMode.Follower, 0);
// Right Slave
victor3.Set(ControlMode.Follower, 2);
// Talon SRX Slaves
// Feeder Slave
feederL.Set(ControlMode.Follower, 0);
feederL.SetInverted(true);
// Intake Slave
intakeLft.Set(ControlMode.Follower, 2);
intakeLft.SetInverted(true);
// Hood
hood.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog, 0, kTimeoutMs);
hood.SetSensorPhase(false);
hood.Config_kP(0, 30f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
hood.Config_kI(0, 0.0005f, kTimeoutMs);
hood.Config_kD(0, 0f, kTimeoutMs);
hood.Config_kF(0, 0f, kTimeoutMs);
/* use slot0 for closed-looping */
hood.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
hood.ConfigNominalOutputForward(0.0f, kTimeoutMs);
hood.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
hood.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
hood.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
hood.ConfigForwardSoftLimitThreshold(HOOD_LOWER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigReverseSoftLimitThreshold(HOOD_UPPER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
hood.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
hood.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
//***********************
// MAY NEED TUNING
//***********************
// Turret
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 1, kTimeoutMs);
int absPos = turret.GetSelectedSensorPosition(1);
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
turret.SetSelectedSensorPosition(0, 0, kTimeoutMs);
turret.SetSensorPhase(true);
turret.Config_IntegralZone(20);
turret.Config_kP(0, 2f, kTimeoutMs); // tweak this first, a little bit of overshoot is okay
turret.Config_kI(0, 0f, kTimeoutMs);
turret.Config_kD(0, 0f, kTimeoutMs);
turret.Config_kF(0, 0f, kTimeoutMs);
// use slot0 for closed-looping
turret.SelectProfileSlot(0, 0);
// set the peak and nominal outputs, 1.0 means full
turret.ConfigNominalOutputForward(0.0f, kTimeoutMs);
turret.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
turret.ConfigPeakOutputForward(+0.5f, kTimeoutMs);
turret.ConfigPeakOutputReverse(-0.5f, kTimeoutMs);
turret.ConfigReverseSoftLimitThreshold(TURRET_UPPER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigForwardSoftLimitThreshold(TURRET_LOWER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
turret.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
// how much error is allowed? This defaults to 0.
turret.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
// Shooter
shooterSensorTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
shooterSensorTalon.SetSensorPhase(false);
shooterSensorTalon.ConfigPeakOutputReverse(0.0f, kTimeoutMs);
shooterSensorTalon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
shooterVESC = new PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7);
shooterVESC.Set(0);
pcm.SetSolenoidOutput(shooterFeedbackLEDPort, false);
}