void ShooterArm::SetTargetAngle(float tgtAngle)
{
_targetAngle = tgtAngle;
_onStage2 = false;
_onStage3 = false;
PIDController *pid = GetPIDController();
pid->SetPID(_bothStage_P,_stage_1_I,_stage_1_D);
GetPIDController()->SetAbsoluteTolerance(fabs(_stage_1_tolerance * VOLTAGE_DEG_SCALAR));
_voltageTarget = DEGREES_TO_VOLTAGE(tgtAngle);
GetPIDController()->SetSetpoint(_voltageTarget);
Enable();
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
Catapult::Catapult() : PIDSubsystem("Catapult", -0.003, -0.001, -0.005) {
SetAbsoluteTolerance(5.0);
GetPIDController()->SetContinuous(false);
LiveWindow::GetInstance()->AddActuator("Catapult", "PIDSubsystem Controller", GetPIDController());
GetPIDController()->SetOutputRange(-1.0, 0.35);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
encoder = RobotMap::catapultEncoder;
jag2 = RobotMap::catapultJag2;
jag3 = RobotMap::catapultJag3;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
}
void HorizontalAlign::UsePIDOutput(double output)
{
if (GetPIDController()->OnTarget())
{
onTargetCounter++;
}
else
{
onTargetCounter = 0;
}
SmartDashboard::PutNumber("OnTarget Counter", onTargetCounter);
printf("%f", output);
if (output > 0)
output += ROT_SPEED_MIN;
else if (output < 0)
output -= ROT_SPEED_MIN;
if (!PIDUserDisabled && !IsFinished())
Robot::drivetrain->SetTankDrive(output, -output);
//printf("\noutput");
SmartDashboard::PutNumber("AutoAlign Output", output);
printf("wowowow %f, %u\n" , output, PIDUserDisabled);
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
Arm::Arm() : PIDSubsystem("Arm", 1.0, 0.0, 0.0) {
SetAbsoluteTolerance(0.2);
GetPIDController()->SetContinuous(false);
LiveWindow::GetInstance()->AddActuator("Arm", "PIDSubsystem Controller", GetPIDController());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
motor = RobotMap::armMotor;
pot = RobotMap::armPot;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
ArmPIDSubsystem::ArmPIDSubsystem()
: PIDSubsystem("ArmPIDSubsystem", Kp, Ki, Kd),
upperArmLimitHit(false),
lowerArmLimitHit(false),
pidEnabled(false),
potOrigin(PotOriginDefault),
potOriginCheckComplete(false)
{
Logger::GetInstance()->Log(ArmPIDSubsystemLogId, Logger::kINFO, "ArmPIDSubsystem::ctor() called.");
SetAbsoluteTolerance(0.2);
GetPIDController()->SetContinuous(false);
LiveWindow::GetInstance()->AddActuator("Arm PID Subsystem", "PIDSubsystem Controller", GetPIDController());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
potentiometer1 = RobotMap::armPIDSubsystemPotentiometer1;
armSpeedController = RobotMap::armPIDSubsystemSpeedController1;
upperArmLimit = RobotMap::armPIDSubsystemupperArmLimit;
lowerArmLimit = RobotMap::armPIDSubsystemlowerArmLimit;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
Logger::GetInstance()->Log(ArmPIDSubsystemLogId, Logger::kINFO, "ArmPIDSubsystem::ctor() exiting.");
SmartDashboard::PutBoolean(ReadyToFireField, false);
}
SwerveSteer::SwerveSteer(int dsc, int data_pin, int chipselect_pin, int clock_pin, int motor_channel) :
PIDSubsystem("SwerveSteer", Kp, Ki, Kd),
data(dsc, data_pin),
chip_select(dsc, chipselect_pin),
clock(dsc, clock_pin),
motor(dsc, motor_channel)
{
GetPIDController()->SetContinuous(true);
GetPIDController()->SetInputRange(-1,1);
GetPIDController()->SetOutputRange(-1,1);
chip_select.Set(1);
clock.Set(1);
motor.SetSafetyEnabled(false);
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
}
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);
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
ShooterArm::ShooterArm() : PIDSubsystem("ShooterArm", 0.75, 0.16, 0.7) {
SetAbsoluteTolerance(0.3);
GetPIDController()->SetContinuous(false);
LiveWindow::GetInstance()->AddActuator("ShooterArm", "PIDSubsystem Controller", GetPIDController());
GetPIDController()->SetOutputRange(-1.0, 0.75);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
shooterArmPot = RobotMap::shooterArmshooterArmPot;
shooterArmMotor = RobotMap::shooterArmshooterArmMotor;
fiveVoltMeasure = RobotMap::shooterArmFiveVoltMeasure;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
GetPIDController()->SetInputRange(MIN_ANGLE, MAX_ANGLE);//Minimum voltage defines the max angle, max volage defines the min angle
_targetAngle = 0.0;
_onStage2 = false;
_onStage3 = false;
_targetPosition = CUSTOM;
ResetPID();
}
Wrist::Wrist() : frc::PIDSubsystem("Wrist", kP_real, 0.0, 0.0) {
#ifdef SIMULATION // Check for simulation and update PID values
GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
#endif
SetAbsoluteTolerance(2.5);
// Let's show everything on the LiveWindow
AddChild("Motor", m_motor);
AddChild("Pot", m_pot);
}
void ShooterArm::ResetPID()
{
GetPIDController()->Reset();
_bothStage_P = 0.850;
_stage_1_I = 0.0;
_stage_2_I = 0.14;
_stage_1_D = 0.0;
_stage_2_D = 4.8;
_stage_1_tolerance = 10.0;
_stage_2_tolerance = 1.0;
}
ChassisSubsystem::ChassisSubsystem()
: PIDSubsystem("Chassis", 0.05, 0.01, 0.15)
, moveSpeed(0)
, m_onPID(false)
{
m_drive = new RobotDrive(M_FrontLeft,M_RearLeft,M_FrontRight,M_RearRight);
m_drive->SetSafetyEnabled(false);
m_cEncoderL = new Encoder(4,5);
m_cEncoderR = new Encoder(6,7);
GetPIDController()->Disable();
}
void HorizontalAlign::Execute()
{
//Only if we need to FIND a target
if (!hasTarget)
{
hasTarget = Robot::vision->UpdateCurrentTarget();
}
else
{
if (!GetPIDController()->IsEnabled())
{
PIDUserDisabled = false;
//GetPIDController()->SetPID(1, 0, 0);
auto pid = GetPIDController();
pid->SetPID(SmartDashboard::GetNumber("dP", 0.004), SmartDashboard::GetNumber("dI", 0), SmartDashboard::GetNumber("dD", 0));
pid->Enable();
}
else
printf("PID Enabled\n");
}
}
void ShooterArm::UsePIDOutput(double output)
{
//Go from stage 1 to to 2, or from stage 3 to 2 if off target
if (((_onStage2 == false) && (OnTarget())) ||
((_onStage3 == true) && (OnTarget() == false)))
{
_onStage2 = true;
_onStage3 = false;
PIDController *pid = GetPIDController();
pid->SetPID(_bothStage_P,_stage_2_I,_stage_2_D);
pid->SetAbsoluteTolerance(fabs(_stage_2_tolerance * VOLTAGE_DEG_SCALAR));
}
else if ((_onStage2 == true) && (_onStage3 == false) && (OnTarget()))
{
_onStage3 = true;
PIDController *pid = GetPIDController();
pid->SetPID(_bothStage_P,0.0,_stage_2_D);
//pid->SetAbsoluteTolerance(fabs(_stage_2_tolerance * VOLTAGE_DEG_SCALAR));
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
shooterArmMotor->PIDWrite(output);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
ShooterSubsystem::ShooterSubsystem() : PIDSubsystem("ShooterSubsystem", 1.0, 0.0, 0.0) {
SetAbsoluteTolerance(0.2);
GetPIDController()->SetContinuous(false);
LiveWindow::GetInstance()->AddActuator("ShooterSubsystem", "PIDSubsystem Controller", GetPIDController());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
shooterEncoder = RobotMap::shooterSubsystemShooterEncoder;
shooterWheelJaguar = RobotMap::shooterSubsystemShooterWheelJaguar;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
}
//Inherited from PID Command, returns the input from the vision targets
double HorizontalAlign::ReturnPIDInput()
{
//Makes sure that the target still exists, if not, it goes bye bye
std::shared_ptr<VisionTarget> target = Robot::vision->GetTrackedGoal();
if (target == nullptr)
{
PIDUserDisabled = true;
hasTarget = false;
GetPIDController()->Disable();
return 0;
}
else
{
printf("Center X %u\n", target->GetCenter().x);
return (double) target->GetCenter().x;
}
}
void ShooterArm::TestPID(float setpoint,float p, float stage1I, float stage2I, float stage1D,float stage2D, float stage1Tol, float stage2Tol)
{
//_maxFeedForward = 0.0;
//float newFeedForward = 0.0;//_maxFeedForward * cos(GetCurrentRadians());
//GetPIDController()->SetPID(p,i,d, newFeedForward);
Disable();
_bothStage_P = p;
_stage_1_I = stage1I;
_stage_2_I = stage2I;
_stage_1_D = stage1D;
_stage_2_D = stage2D;
_stage_1_tolerance = stage1Tol;
_stage_2_tolerance = stage2Tol;
GetPIDController()->SetPID(_bothStage_P,_stage_1_I,_stage_1_D);
SetAngle(setpoint);
}
SlavedLauncherPID::SlavedLauncherPID(std::string side, MotorPin motorPin, MotorPin motorPin2, DIOPin encoderPin) :
PIDSubsystem(side + "SlavedLauncherPID", 4.5, 0.167, .0),
pidMotor(motorPin),
pidMotor2(motorPin2),
pidEncoder(encoderPin),
reverseMultiplier(1)
{
LiveWindow::GetInstance()->AddActuator(side + "LauncherPID2", side + "PIDController", GetPIDController());
LiveWindow::GetInstance()->AddActuator(side + "LauncherPID2", side + "OutputMotor", pidMotor);
LiveWindow::GetInstance()->AddActuator(side + "LauncherPID2", side + "OutputMotor2", pidMotor2);
LiveWindow::GetInstance()->AddSensor(side + "LauncherPID2", side + "Encoder", pidEncoder);
// SmartDashboard::PutData(side+"LauncherPidController", GetPIDController().get());
GetPIDController()->SetOutputRange(0,1);
SetAbsoluteTolerance(.05);
SetPIDSourceType(PIDSourceType::kDisplacement);
pidEncoder.SetSamplesToAverage(1);
// Use these to get going:
// SetSetpoint() - Sets where the PID controller should move the system
// to
// Enable() - Enables the PID controller.
}
void HorizontalAlign::End()
{
//TODO: Calculate launch angle and move the launcher here
GetPIDController()->Disable();
Robot::drivetrain->SetTankDrive(0, 0);
}
void HorizontalAlign::Interrupted()
{
GetPIDController()->Disable();
Robot::drivetrain->SetTankDrive(0, 0);
}
void Catapult::CatLimitOut(double min, double max){
GetPIDController()->SetOutputRange(min, max);
}
#set($subsystem = $helper.getByName($subsystem-name, $robot))
#class($subsystem.name)::#class($subsystem.name)() : PIDSubsystem("#class($subsystem.name)", ${subsystem.getProperty("P").getValue()}, ${subsystem.getProperty("I").getValue()}, ${subsystem.getProperty("D").getValue()}) {
SetAbsoluteTolerance(${subsystem.getProperty("Tolerance").getValue()});
GetPIDController()->SetContinuous(${subsystem.getProperty("Continuous").getValue()});
LiveWindow::GetInstance()->AddActuator("$subsystem-name", "PIDSubsystem Controller", GetPIDController());
#if($subsystem.getProperty("Limit Input").getValue())
GetPIDController()->SetInputRange(${subsystem.getProperty("Minimum Input").getValue()}, ${subsystem.getProperty("Maximum Input").getValue()});
#end
#if($subsystem.getProperty("Limit Output").getValue())
GetPIDController()->SetOutputRange(${subsystem.getProperty("Minimum Output").getValue()}, ${subsystem.getProperty("Maximum Output").getValue()});
#end
void ChassisSubsystem::HoldAngle(double angle)
{
GetPIDController()->SetSetpoint(GetAngle() + angle);
GetPIDController()->Enable();
m_onPID = true;
}
