feat: ValorControllers

Competition/src/main/cpp/controllers/ValorFalconController.cpp

@@ -0,0 +1,126 @@
+#include "controllers/ValorFalconController.h"
+
+#define FALCON_TICKS_PER_REV 2048
+
+ValorFalconController::ValorFalconController(int canID,
+                                             NeutralMode _mode,
+                                             bool _inverted,
+                                             const std::__cxx11::string &canbus) :
+    ValorController(_mode, _inverted),
+    conversion(1)
+{
+    motor = new WPI_TalonFX{canID, canbus};
+    init();
+}
+
+void ValorFalconController::init()
+{
+    motor->ConfigFactoryDefault();
+    motor->SetInverted(inverted);
+    motor->SetNeutralMode(mode);
+
+    // @TODO should this be enabled for all falcons going forward?
+    motor->EnableVoltageCompensation(true);
+    motor->ConfigVoltageCompSaturation(10);
+    motor->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); //potentially could do 40 60
+
+    motor->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10);
+    motor->ConfigAllowableClosedloopError(0, 0);
+    motor->Config_IntegralZone(0, 0);
+
+    setPIDF(0, motionPIDF);
+    reset();
+}
+
+void ValorFalconController::reset()
+{
+    motor->SetSelectedSensorPosition(0);
+}
+
+void ValorFalconController::setupFollower(int canID)
+{
+    followerMotor = new WPI_TalonFX(canID);
+    followerMotor->Follow(*motor);
+    followerMotor->SetNeutralMode(mode);
+}
+
+void ValorFalconController::setLimits(int reverse, int forward)
+{
+    motor->ConfigForwardSoftLimitThreshold(reverse);
+    motor->ConfigReverseSoftLimitThreshold(forward);
+    motor->ConfigForwardSoftLimitEnable(true);
+    motor->ConfigReverseSoftLimitEnable(true);
+}
+
+void ValorFalconController::setPIDF(int slot, PIDF pidf)
+{
+    ValorController::setPIDF(pidf);
+    motor->Config_kP(slot, motionPIDF.P);
+    motor->Config_kI(slot, motionPIDF.I);
+    motor->Config_kD(slot, motionPIDF.D);
+    motor->Config_kF(slot, motionPIDF.F);
+    motor->ConfigMotionCruiseVelocity(motionPIDF.velocity);
+    motor->ConfigMotionAcceleration(motionPIDF.acceleration);
+}
+
+void ValorFalconController::setConversion(double _conversion)
+{
+    conversion = _conversion;
+}
+
+double ValorFalconController::getCurrent()
+{
+    return motor->GetOutputCurrent();
+}
+
+/**
+ * Get the position in units (specified by conversion)
+ */
+double ValorFalconController::getPosition()
+{
+    return motor->GetSelectedSensorPosition() * conversion / FALCON_TICKS_PER_REV;
+}
+
+/**
+ * Get the speed in units per second (specified by conversion)
+ */
+double ValorFalconController::getSpeed()
+{
+    return motor->GetSelectedSensorVelocity() * 10 * conversion / FALCON_TICKS_PER_REV;
+}
+
+void ValorFalconController::setRange(int slot, double min, double max)
+{
+
+}
+
+/**
+ * Set the position in units (specified by conversion). Example: inches
+ */
+void ValorFalconController::setPosition(double position)
+{
+    motor->Set(ControlMode::MotionMagic, position / conversion * FALCON_TICKS_PER_REV);
+}
+
+/**
+ * Set the speed in units per second (specified by conversion). Example: inches per second
+ */
+void ValorFalconController::setSpeed(double speed)
+{
+    motor->Set(ControlMode::Velocity, speed / 10 / conversion * FALCON_TICKS_PER_REV);
+}
+
+void ValorFalconController::setPower(double speed)
+{
+    motor->Set(speed);
+}
+
+void ValorFalconController::setProfile(int profile)
+{
+    motor->SelectProfileSlot(profile, 0);
+}
+
+void ValorFalconController::preventBackwards()
+{
+    motor->ConfigPeakOutputReverse(0);
+}

Competition/src/main/cpp/controllers/ValorNeoController.cpp

@@ -0,0 +1,121 @@
+#include "controllers/ValorNeoController.h"
+
+ValorNeoController::ValorNeoController(int canID,
+                                       rev::CANSparkMax::IdleMode _mode,
+                                       bool _inverted,
+                                       const std::__cxx11::string &canbus) :
+    ValorController(_mode, _inverted),
+    pidController(motor->GetPIDController()),
+    encoder(motor->GetEncoder())
+{
+    motor = new rev::CANSparkMax{canID, rev::CANSparkMax::MotorType::kBrushless};
+    init();
+}
+
+void ValorNeoController::init()
+{
+    motor->RestoreFactoryDefaults();
+    motor->SetInverted(inverted);
+    motor->SetIdleMode(mode);
+    setRange(0,-1,1);
+    setPIDF(0, motionPIDF);
+    reset();
+}
+
+void ValorNeoController::reset()
+{
+    encoder.SetPosition(0);
+}
+
+void ValorNeoController::setupFollower(int canID)
+{
+    followerMotor = new rev::CANSparkMax(canID, rev::CANSparkMax::MotorType::kBrushless);
+    followerMotor->Follow(*motor);
+    followerMotor->SetIdleMode(mode);
+}
+
+void ValorNeoController::setLimits(int reverse, int forward)
+{
+    motor->EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, true);
+    motor->SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kForward, forward);
+    motor->EnableSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, true);
+    motor->SetSoftLimit(rev::CANSparkMax::SoftLimitDirection::kReverse, reverse);
+}
+
+void ValorNeoController::setPIDF(int slot, PIDF pidf)
+{
+    ValorController::setPIDF(pidf);
+
+    pidController.SetP(motionPIDF.P, slot);
+    pidController.SetI(motionPIDF.I, slot);
+    pidController.SetD(motionPIDF.D, slot);
+    pidController.SetFF(motionPIDF.F, slot);
+    pidController.SetIZone(0, slot);
+
+    pidController.SetSmartMotionMaxVelocity(pidf.velocity, slot);
+    pidController.SetSmartMotionMaxAccel(pidf.acceleration, slot);
+    pidController.SetSmartMotionAllowedClosedLoopError(motionPIDF.error, slot);
+}
+
+/**
+ * Set the conversion rate.
+ * Converts between your desired units and rotations of the neo motor shaft (includes gear ratio)
+ * @param conversion Conversion rate for position
+ */
+void ValorNeoController::setConversion(double conversion)
+{
+    encoder.SetPositionConversionFactor(conversion);
+    // convert from minutes to seconds for velocity
+    encoder.SetVelocityConversionFactor(conversion / 60.0);
+}
+
+void ValorNeoController::setRange(int slot, double min, double max)
+{
+    pidController.SetOutputRange(min, max, slot);
+}
+
+double ValorNeoController::getCurrent()
+{
+    return motor->GetOutputCurrent();
+}
+
+/**
+ * Get the position in units (specified by conversion)
+ */
+double ValorNeoController::getPosition()
+{
+    return encoder.GetPosition();
+}
+
+/**
+ * Get the speed in units per second (specified by conversion)
+ */
+double ValorNeoController::getSpeed()
+{
+    return encoder.GetVelocity();
+}
+
+/**
+ * Set the position in units (specified by conversion). Example: inches
+ */
+void ValorNeoController::setPosition(double position)
+{
+    pidController.SetReference(position, rev::ControlType::kSmartMotion);
+}
+
+void ValorNeoController::setProfile(int profile)
+{
+}
+
+/**
+ * Set the speed in units per second (specified by conversion). Example: inches per second
+ */
+void ValorNeoController::setSpeed(double speed)
+{
+    pidController.SetReference(speed, rev::ControlType::kSmartVelocity);
+}
+
+void ValorNeoController::setPower(double speed)
+{
+    motor->Set(speed);
+}

Competition/src/main/cpp/subsystems/Feeder.cpp

@@ -12,12 +12,21 @@
 #include "subsystems/Feeder.h"
 #include <iostream>
 
+#define INTAKE_SPD_FORWARD 0.7
+#define INTAKE_SPD_REVERSE -0.7
+#define FEEDER_SPD_FORWARD 0.5
+#define FEEDER_SPD_SHOOT   0.9
+#define FEEDER_SPD_REVERSE -1.0
+
+#define CACHE_SIZE 20
+#define JAM_CURRENT 22
+
 Feeder::Feeder() : ValorSubsystem(),
                            driverController(NULL),
                            operatorController(NULL),
-                           motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"),
-                           motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"),
-                           banner(FeederConstants::BANNER_DIO_PORT)
+                           intakeController(CANIDs::INTAKE, Coast, false, "baseCAN"),
+                           stageController(CANIDs::STAGE, Coast, true, "baseCAN"),
+                           banner(DIOPorts::BANNER)
 {
     frc2::CommandScheduler::GetInstance().RegisterSubsystem(this);
     init();
@@ -26,33 +35,21 @@ Feeder::Feeder() : ValorSubsystem(),
 void Feeder::init()
 {
     initTable("Feeder");
-    motor_intake.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10);
-    motor_intake.SetNeutralMode(ctre::phoenix::motorcontrol::Coast);
-    motor_intake.SetInverted(false);
-    motor_intake.EnableVoltageCompensation(false);
-    motor_intake.ConfigVoltageCompSaturation(10);
-    motor_intake.ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75)); //potentially could do 40 60
-
-    motor_stage.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10);
-    motor_stage.SetNeutralMode(ctre::phoenix::motorcontrol::Coast);
-    motor_stage.SetInverted(true);
-    motor_stage.EnableVoltageCompensation(true);
-    motor_stage.ConfigVoltageCompSaturation(10);
 
     table->PutBoolean("Reverse Feeder?", false);
-    table->PutNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE);
-    table->PutNumber("Feeder Reverse Speed", FeederConstants::DEFAULT_FEEDER_SPEED_REVERSE);
-    table->PutNumber("Intake Forward Speed", FeederConstants::DEFAULT_INTAKE_SPEED_FORWARD);
-    table->PutNumber("Feeder Forward Speed Default", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT);
-    table->PutNumber("Feeder Forward Speed Shoot", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_SHOOT);
-    table->PutNumber("Intake Spike Current", FeederConstants::JAM_CURRENT);
+    table->PutNumber("Intake Reverse Speed", INTAKE_SPD_REVERSE);
+    table->PutNumber("Intake Forward Speed", INTAKE_SPD_FORWARD);
+    table->PutNumber("Intake Spike Current", JAM_CURRENT);
+    table->PutNumber("Feeder Reverse Speed", FEEDER_SPD_REVERSE);
+    table->PutNumber("Feeder Forward Speed Default", FEEDER_SPD_FORWARD);
+    table->PutNumber("Feeder Forward Speed Shoot", FEEDER_SPD_SHOOT);
 
     table->PutNumber("Average Amps", 0);
     table->PutBoolean("Spiked: ", 0);
     table->PutBoolean("Banner: ", 0);
 
     resetState();
-    currentSensor.setGetter([this]() { return motor_intake.GetOutputCurrent(); });
+    currentSensor.setGetter([this]() { return intakeController.getCurrent(); });
     debounceSensor.setGetter([this]() { return !banner.Get(); });
 
 }
@@ -99,11 +96,12 @@ void Feeder::analyzeDashboard()
     debounceSensor.calculate();
 
     state.reversed = table->GetBoolean("Reverse Feeder?", false);
-    state.intakeReverseSpeed = table->GetNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE);
-    state.feederReverseSpeed = table->GetNumber("Feeder Reverse Speed", FeederConstants::DEFAULT_FEEDER_SPEED_REVERSE);
-    state.intakeForwardSpeed = table->GetNumber("Intake Forward Speed", FeederConstants::DEFAULT_INTAKE_SPEED_FORWARD);
-    state.feederForwardSpeedDefault = table->GetNumber("Feeder Forward Speed Default", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_DEFAULT);
-    state.feederForwardSpeedShoot = table->GetNumber("Feeder Forward Speed Shoot", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_SHOOT);
+
+    state.intakeReverseSpeed = table->GetNumber("Intake Reverse Speed", INTAKE_SPD_REVERSE);
+    state.intakeForwardSpeed = table->GetNumber("Intake Forward Speed", INTAKE_SPD_FORWARD);
+    state.feederReverseSpeed = table->GetNumber("Feeder Reverse Speed", FEEDER_SPD_REVERSE);
+    state.feederForwardSpeedDefault = table->GetNumber("Feeder Forward Speed Default", FEEDER_SPD_FORWARD);
+    state.feederForwardSpeedShoot = table->GetNumber("Feeder Forward Speed Shoot", FEEDER_SPD_SHOOT);
 
     table->PutNumber("Average Amps", currentSensor.getSensor());
     table->PutBoolean("Spiked: ", currentSensor.spiked());
@@ -115,43 +113,43 @@ void Feeder::assignOutputs()
 {
 
     if (state.feederState == FeederState::FEEDER_DISABLE) {
-        motor_intake.Set(0);
-        motor_stage.Set(0);
+        intakeController.setPower(0);
+        stageController.setPower(0);
     }
     else if (state.feederState == FeederState::FEEDER_SHOOT) {
-        motor_intake.Set(state.intakeForwardSpeed);
-        motor_stage.Set(state.feederForwardSpeedShoot);
+        intakeController.setPower(state.intakeForwardSpeed);
+        stageController.setPower(state.feederForwardSpeedShoot);
     }
     else if (state.feederState == Feeder::FEEDER_REVERSE) {
-        motor_intake.Set(state.intakeReverseSpeed);
-        motor_stage.Set(state.feederReverseSpeed);
+        intakeController.setPower(state.intakeReverseSpeed);
+        stageController.setPower(state.feederReverseSpeed);
     }
     else if (state.feederState == Feeder::FEEDER_REGULAR_INTAKE){
-        motor_intake.Set(state.intakeForwardSpeed);
-        motor_stage.Set(0);
+        intakeController.setPower(state.intakeForwardSpeed);
+        stageController.setPower(0);
     }
     else if (state.feederState == FeederState::FEEDER_CURRENT_INTAKE) { //includes currrent sensing
         if (debounceSensor.getSensor()) {
             if (debounceSensor.spiked()) {
                 resetIntakeSensor();
             }
             if (currentSensor.spiked()) {
-                motor_intake.Set(0);
-                motor_stage.Set(0);
+                intakeController.setPower(0);
+                stageController.setPower(0);
             }
             else {
-                motor_intake.Set(state.intakeForwardSpeed);
-                motor_stage.Set(0);
+                intakeController.setPower(state.intakeForwardSpeed);
+                stageController.setPower(0);
             }
         }
         else {
-            motor_intake.Set(state.intakeForwardSpeed);
-            motor_stage.Set(state.feederForwardSpeedDefault);
+            intakeController.setPower(state.intakeForwardSpeed);
+            stageController.setPower(state.feederForwardSpeedDefault);
         }
     }
     else {
-        motor_intake.Set(0);
-        motor_stage.Set(0);
+        intakeController.setPower(0);
+        stageController.setPower(0);
     }
 }