FeederIO PID

src/main/java/frc/robot/Constants.java

@@ -66,7 +66,7 @@ public static final class RobotMap {
 
     public static final int DRIVER_PORT = 0;
     public static final int OPERATOR_PORT = 1;
-    
+
     public static final int SHOOTER_LEFT_FLYWHEEL_ID = 4110;
     public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 4540;
 
@@ -99,6 +99,8 @@ public static final class IntakeConstants {
   }
 
   public static final class FeederConstants {
+    public static final PIDFFGains FEEDER_GAINS =
+        PIDFFGains.builder().name("Feeder Controller").kP(0.0).kD(0.0).build();
     // TODO: FIX
     public static final double GERING = 1.;
     public static final double MAX_RPM = 5000;

src/main/java/frc/robot/Robot.java

@@ -23,15 +23,15 @@
 import frc.robot.commands.otf.RotateScore;
 import frc.robot.subsystems.elevatorIO.Elevator;
 import frc.robot.subsystems.elevatorIO.ElevatorIOSim;
-import frc.robot.subsystems.shooterIO.Shooter;
-import frc.robot.subsystems.shooterIO.ShooterIOSim;
-import frc.robot.subsystems.shooterIO.ShooterIOVortex;
 import frc.robot.subsystems.feederIO.Feeder;
 import frc.robot.subsystems.feederIO.FeederIOSim;
 import frc.robot.subsystems.feederIO.FeederIOSparks;
 import frc.robot.subsystems.intakeIO.Intake;
 import frc.robot.subsystems.intakeIO.IntakeIOSim;
 import frc.robot.subsystems.intakeIO.IntakeIOSparks;
+import frc.robot.subsystems.shooterIO.Shooter;
+import frc.robot.subsystems.shooterIO.ShooterIOSim;
+import frc.robot.subsystems.shooterIO.ShooterIOVortex;
 import frc.robot.subsystems.shooterPivot.ShooterPivot;
 import frc.robot.subsystems.shooterPivot.ShooterPivotIOSim;
 import frc.robot.subsystems.swerveIO.SwerveIOPigeon2;

src/main/java/frc/robot/subsystems/feederIO/Feeder.java

@@ -1,45 +1,39 @@
 package frc.robot.subsystems.feederIO;
 
-import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import frc.robot.Robot;
-import frc.robot.rhr.RHRPIDFFController;
-import frc.robot.util.PIDFFGains;
+import frc.robot.util.LoggableMotor;
+import lombok.Getter;
 import org.littletonrobotics.junction.Logger;
 
 public class Feeder extends SubsystemBase {
-  private FeederIO io;
+  @Getter private LoggableMotor feederMotor;
+
+  private FeederIO IO;
   private double target;
   private FeederInputsAutoLogged inputs;
-  private RHRPIDFFController controller =
-      new RHRPIDFFController(PIDFFGains.builder().kP(0.04).kD(0.).build());
 
-  public Feeder(FeederIO io) {
-    this.io = io;
+  public Feeder(FeederIO IO) {
+    this.IO = IO;
     this.inputs = new FeederInputsAutoLogged();
+    this.feederMotor = new LoggableMotor("Feeder", DCMotor.getNEO(1));
   }
 
   public void setTarget(double targetRPM) {
     target = targetRPM;
-    // io.setSetpoint(targetRPM);
+    IO.setSetpoint(targetRPM);
   }
 
   @Override
   public void periodic() {
-    io.updateInputs(inputs);
-    var volts = controller.calculate(inputs.velocityRPM, target);
-    volts = MathUtil.clamp(volts, -12, 12);
-
-    Logger.recordOutput("Feeder/Sent Volts", volts);
+    IO.updateInputs(inputs);
     Logger.recordOutput("Feeder/TargetRPM", target);
     Logger.recordOutput("Feeder/atTarget", atTarget());
-
-    io.setVoltage(volts);
-
     Logger.processInputs("Feeder", inputs);
   }
 

src/main/java/frc/robot/subsystems/feederIO/FeederIO.java

@@ -13,11 +13,11 @@ public static class FeederInputs {
     public double positionDeg = 0.0;
   }
 
-  // public void setSetpoint(double setpointRPM);
+  public void setSetpoint(double setpointRPM);
 
   public void setVoltage(double volts);
 
-  // public boolean atTarget();
+  public boolean atTarget();
 
   public void updateInputs(FeederInputsAutoLogged inputs);
 }

src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java

@@ -4,24 +4,32 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import frc.robot.Constants;
+import frc.robot.Constants.FeederConstants;
+import frc.robot.rhr.RHRPIDFFController;
 
 public class FeederIOSim implements FeederIO {
+  private double setpointRPM;
   DCMotorSim sim =
       new DCMotorSim(
           DCMotor.getNEO(1), Constants.FeederConstants.GERING, Constants.FeederConstants.MOI);
   private double volts;
+  RHRPIDFFController feederController;
 
-  public FeederIOSim() {}
+  public FeederIOSim() {
+    feederController = FeederConstants.FEEDER_GAINS.createRHRController();
+  }
 
   @Override
   public void setVoltage(double volts) {
     this.volts = volts;
     sim.setInputVoltage(volts);
   }
-  // @Override
-  // public boolean atTarget() {
-  //     return Math.abs(sim.getAngularVelocityRPM() -) > 0.01;
-  // }
+
+  @Override
+  public boolean atTarget() {
+    return Math.abs(sim.getAngularVelocityRPM() - setpointRPM) < 0.01;
+  }
+
   @Override
   public void updateInputs(FeederInputsAutoLogged inputs) {
     sim.update(0.02);
@@ -32,4 +40,10 @@ public void updateInputs(FeederInputsAutoLogged inputs) {
     inputs.velocityRPM = sim.getAngularVelocityRPM();
     inputs.positionDeg = Units.rotationsToDegrees(sim.getAngularPositionRotations());
   }
+
+  @Override
+  public void setSetpoint(double setpointRPM) {
+    feederController.setSetpoint(setpointRPM);
+    this.setpointRPM = setpointRPM;
+  }
 }

src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java

@@ -1,32 +1,36 @@
 package frc.robot.subsystems.feederIO;
 
+import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants.FeederConstants;
 import frc.robot.Constants.RobotMap;
 
 public class FeederIOSparks implements FeederIO {
   CANSparkMax motor;
-  // double setpoint = 0;
+  double setpoint = 0;
 
   public FeederIOSparks() {
     motor = new CANSparkMax(RobotMap.FEEDER_CAN_ID, MotorType.kBrushless);
+    motor.getPIDController().setP(FeederConstants.FEEDER_GAINS.getKP());
+    motor.getPIDController().setD(FeederConstants.FEEDER_GAINS.getKD());
   }
 
-  // @Override
-  // public void setSetpoint(double setpointRPM) {
-  //     setpoint = setpointRPM;
-  // }
+  @Override
+  public void setSetpoint(double setpointRPM) {
+    motor.getPIDController().setReference(setpointRPM, ControlType.kVelocity, 0);
+  }
 
   @Override
   public void setVoltage(double volts) {
     motor.setVoltage(volts);
   }
 
-  // @Override
-  // public boolean atTarget() {
-  //     return Math.abs(motor.getEncoder().getVelocity() - setpoint) < 0.1;
-  // }
+  @Override
+  public boolean atTarget() {
+    return Math.abs(motor.getEncoder().getVelocity() - setpoint) < 0.1;
+  }
 
   @Override
   public void updateInputs(FeederInputsAutoLogged inputs) {

src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java

@@ -1,13 +1,19 @@
 package frc.robot.subsystems.swerveIO;
 
+import com.choreo.lib.Choreo;
+import com.choreo.lib.ChoreoControlFunction;
+import com.choreo.lib.ChoreoTrajectory;
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
@@ -17,12 +23,21 @@
 import edu.wpi.first.networktables.TimestampedDoubleArray;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import frc.robot.Constants;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.Robot;
+import frc.robot.rhr.auto.RHRPathPlannerAuto;
 import frc.robot.subsystems.swerveIO.module.SwerveModule;
 import frc.robot.subsystems.swerveIO.module.SwerveModuleIO;
+import frc.robot.util.ErrorTracker;
 import frc.robot.util.MotionHandler;
+import frc.robot.util.PIDFFGains;
+import frc.robot.util.SwerveHeadingController;
 import org.littletonrobotics.junction.Logger;
 
 public class SwerveSubsystem extends SubsystemBase {
@@ -399,5 +414,88 @@ public void periodic() {
     Logger.recordOutput("Swerve/Desired speeds/r-radps", desiredSpeeds.omegaRadiansPerSecond);
   }
 
-  public static class Commands {}
+  public static class Commands {
+    public ChoreoControlFunction modifiedChoreoSwerveController(
+        PIDController xController, PIDController yController, PIDController rotationController) {
+      rotationController.enableContinuousInput(-Math.PI, Math.PI);
+      ErrorTracker errorTracker =
+          new ErrorTracker(
+              10,
+              PIDFFGains.fromPIDGains(xController),
+              PIDFFGains.fromPIDGains(rotationController));
+      return (pose, referenceState) -> {
+        Logger.recordOutput(
+            "Choreo/Target Pose",
+            new Pose2d(
+                new Translation2d(referenceState.x, referenceState.y),
+                Rotation2d.fromRadians(referenceState.heading)));
+
+        var error =
+            new Pose2d(
+                new Translation2d(referenceState.x - pose.getX(), referenceState.y - pose.getY()),
+                Rotation2d.fromRadians(referenceState.heading - pose.getRotation().getRadians()));
+
+        errorTracker.addObservation(error);
+        double xFF = referenceState.velocityX;
+        double yFF = referenceState.velocityY;
+        double rotationFF = referenceState.angularVelocity;
+
+        double xFeedback = xController.calculate(pose.getX(), referenceState.x);
+        double yFeedback = yController.calculate(pose.getY(), referenceState.y);
+        double rotationFeedback =
+            rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading);
+
+        return ChassisSpeeds.fromFieldRelativeSpeeds(
+            xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation());
+      };
+    }
+
+    public static Command setHeading(Rotation2d degrees) {
+      return new InstantCommand(() -> SwerveHeadingController.getInstance().setSetpoint(degrees));
+    }
+
+    public static Command setHeadingandWait(Rotation2d degrees) {
+      return new SequentialCommandGroup(
+          setHeading(degrees),
+          new WaitUntilCommand(SwerveHeadingController.getInstance()::atSetpoint));
+    }
+
+    public static Command getAutonomousCommand(String trajectory) {
+      return new RHRPathPlannerAuto(trajectory);
+    }
+
+    public static Command resetOdometry(Pose2d pose) {
+      return new InstantCommand(() -> Robot.swerveDrive.resetOdometry(pose));
+    }
+
+    public static Command resetOdometry(String trajectory) {
+      PathPlannerPath p = PathPlannerPath.fromChoreoTrajectory(trajectory);
+      return new InstantCommand(
+          () -> Robot.swerveDrive.resetOdometry(p.getPreviewStartingHolonomicPose()));
+    }
+
+    public Command choreoCommandBuilder(ChoreoTrajectory traj) {
+      var alliance = DriverStation.getAlliance();
+      boolean useAllianceColour = false;
+      if (alliance.isPresent()) {
+        useAllianceColour = alliance.get() == DriverStation.Alliance.Red;
+      }
+
+      return new SequentialCommandGroup(
+          Choreo.choreoSwerveCommand(
+              traj, //
+              Robot.swerveDrive::getUsablePose, //
+              modifiedChoreoSwerveController(
+                  new PIDController(3, 0.0, 0.0), //
+                  new PIDController(3, 0.0, 0.0), //
+                  new PIDController(3, 0.0, 0.0)),
+              (ChassisSpeeds speeds) -> {
+                Robot.swerveDrive.setDesiredChassisSpeeds(speeds);
+              },
+              useAllianceColour,
+              Robot.swerveDrive //
+              ),
+          new InstantCommand(() -> Robot.swerveDrive.setDesiredChassisSpeeds(new ChassisSpeeds())));
+    }
+  }
 }

src/main/java/frc/robot/util/SwerveHeadingController.java

@@ -64,6 +64,9 @@ public Rotation2d getSetpoint() {
     return setpoint;
   }
 
+  public boolean atSetpoint() {
+    return this.error < 0.1;
+  }
   /**
    * Updates the heading controller PID with the setpoint and calculates output.
    *
@@ -77,6 +80,7 @@ public double update() {
     Logger.recordOutput("Heading Controller/setpoint", setpoint.getDegrees());
 
     double output = 0;
+
     if (!controller.atSetpoint()) {
       Rotation2d currentHeading = Robot.swerveDrive.getYaw();
       output = controller.calculate(currentHeading.getDegrees(), setpoint.getDegrees());