Arm::Arm() :
PIDSubsystem("Arm", ARM_P, ARM_I, ARM_D)
{
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
// Create the analog control before starting the PID loop.
controlPot.reset(new AnalogInput(PID_CONTROL));
// Create the motor objects first before starting up the arm
motor1.reset(new Talon(ARM_MOTOR_1));
motor2.reset(new Talon(ARM_MOTOR_2));
// Set motor inversions correctly
motor1->SetInverted(ARM_MOTOR_1_REVERSED);
motor2->SetInverted(ARM_MOTOR_2_REVERSED);
// Add this to the smart window? This is awesome!
LiveWindow::GetInstance()->AddSensor("Arm", "Arm Control Pot", controlPot);
// ALWAYS SET A PID SYSTEM TO A START POINT!
SetSetpoint(ARM_CARRY_POSITION);
Enable();
}
void HorizontalAlign::Initialize()
{
printf("test");
auto pid = GetPIDController();
pid->SetAbsoluteTolerance(40);
pid->SetSetpoint(SCREEN_CENTER_X); //Point we're trying to get to
pid->Disable();
pid->SetOutputRange(-(ROT_SPEED_CAP - ROT_SPEED_MIN), ROT_SPEED_CAP - ROT_SPEED_MIN);
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
void PIDController::SetInputRange(float minimumInput, float maximumInput) {
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
SetSetpoint(m_setpoint);
}
void Arm::SetNewPosition(double newTarget, bool isRelative)
{
//printf("set new target arm position %f\n", newTarget);
if (!isRelative)
{
setPoint = newTarget;
}
SetSetpoint(newTarget);
}
float ArmPIDSubsystem::SetAbsolutePIDTarget(float target)
{
if (!pidEnabled)
{
SetPIDSubsystem(true);
}
SetSetpoint(target);
return target;
}
float ArmPIDSubsystem::SetRelativePIDTarget(float target)
{
if (!pidEnabled)
{
SetPIDSubsystem(true);
}
float pidInput = target + potOrigin;
SetSetpoint(pidInput);
return pidInput;
}
void Arm::SetNewPosition(double newTarget)
{
if (newTarget > ARM_MAXIMUM_VALUE)
{
newTarget = ARM_MAXIMUM_VALUE;
}
if (newTarget < ARM_MINIMUM_VALUE)
{
newTarget = ARM_MINIMUM_VALUE;
}
setPoint = newTarget;
SetSetpoint(setPoint);
}
Arm::Arm() : PIDSubsystem("Arm", 0.05, 0.0, 0.0) {
aRM_MOTOR_1 = RobotMap::armMotor1;
aRM_MOTOR_2 = RobotMap::armMotor2;
encoder = RobotMap::encoder;
// ALWAYS SET A PID SYSTEM TO A START POINT!
setPoint = 0;
currentPosition = 0.;
SetSetpoint(setPoint);
for (int i = 0; i < 10; i++)
{
rollingAverage[i] = 90.;
}
Enable();
isEnabled = true;
}
void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (key == kP || key == kI || key == kD || key == kF) {
if (m_P != m_table->GetNumber(kP, 0.0) ||
m_I != m_table->GetNumber(kI, 0.0) ||
m_D != m_table->GetNumber(kD, 0.0) ||
m_F != m_table->GetNumber(kF, 0.0)) {
SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
}
} else if (key == kSetpoint && value->IsDouble() &&
m_setpoint != value->GetDouble()) {
SetSetpoint(value->GetDouble());
} else if (key == kEnabled && value->IsBoolean() &&
m_enabled != value->GetBoolean()) {
if (value->GetBoolean()) {
Enable();
} else {
Disable();
}
}
}
bool OTGWBase::WriteToHardware(const char *pdata, const unsigned char length)
{
const tRBUF *pSen = reinterpret_cast<const tRBUF*>(pdata);
unsigned char packettype = pSen->ICMND.packettype;
unsigned char subtype = pSen->ICMND.subtype;
if (packettype == pTypeLighting2)
{
std::string LCmd = "";
int nodeID = 0;
int svalue = 0;
//light command
nodeID = pSen->LIGHTING2.id4;
if ((pSen->LIGHTING2.cmnd == light2_sOff) || (pSen->LIGHTING2.cmnd == light2_sGroupOff))
{
LCmd = "Off";
svalue = 0;
}
else if ((pSen->LIGHTING2.cmnd == light2_sOn) || (pSen->LIGHTING2.cmnd == light2_sGroupOn))
{
LCmd = "On";
svalue = 255;
}
SwitchLight(nodeID, LCmd, svalue);
}
else if ((packettype == pTypeThermostat) && (subtype == sTypeThermSetpoint))
{
const _tThermostat *pMeter=reinterpret_cast<const _tThermostat*>(pdata);
float temp = pMeter->temp;
unsigned char idx = pMeter->id4;
SetSetpoint(idx, temp);
}
else
{
_log.Log(LOG_STATUS, "OTGW: Skipping writing to Hardware for type: %02X, subType: %02X", packettype, subtype);
}
return true;
}
void Shooter::SetSpeed(double speed)
{
SetSetpoint(speed);
Enable();
}
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);
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPitmanArm::Synchronize(double time, double steering) {
auto fun = std::static_pointer_cast<ChFunction_Setpoint>(m_revolute->GetAngleFunction());
fun->SetSetpoint(getMaxAngle() * steering, time);
}
