void Arm::SetNewRelativePosition(Joystick * stick)
{
double newPosition = GetSetpoint() +
(TUNING_CONSTANT * stick->GetY());
SetNewPosition(newPosition);
}
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
float PIDController::GetError() const {
double pidInput;
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput->PIDGet();
}
return GetSetpoint() - pidInput;
}
/**
* Calculate the feed forward term.
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDController class only calls it in synchronized
* code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
double PIDController::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
} else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
return temp;
}
}
void PIDController::InitTable(std::shared_ptr<ITable> table) {
if (m_table != nullptr) m_table->RemoveTableListener(this);
m_table = table;
if (m_table != nullptr) {
m_table->PutNumber(kP, GetP());
m_table->PutNumber(kI, GetI());
m_table->PutNumber(kD, GetD());
m_table->PutNumber(kF, GetF());
m_table->PutNumber(kSetpoint, GetSetpoint());
m_table->PutBoolean(kEnabled, IsEnabled());
m_table->AddTableListener(this, false);
}
}
bool Shooter::AtSetpoint()
{
return fabs(GetPosition() - GetSetpoint()) < SHOOTER_THRESHOLD;
}
void PIDCommand::SetSetpointRelative(double deltaSetpoint)
{
SetSetpoint(GetSetpoint() + deltaSetpoint);
}
/**
* Adds the given value to the setpoint.
*
* If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used,
* then the bounds will still be honored by this method.
*
* @param deltaSetpoint the change in the setpoint
*/
void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
SetSetpoint(GetSetpoint() + deltaSetpoint);
}
