Merge pull request #491 from truher/main

cleaning, also navx yaw rate workaround
Team100 · Sep 30, 2024 · 2f0f37e · 2f0f37e
2 parents 267b32e + fdc921b
commit 2f0f37e
Show file tree

Hide file tree

Showing 112 changed files with 769 additions and 824 deletions.
diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/Amp.java b/comp/swerve100/src/main/java/org/team100/frc2024/Amp.java
@@ -5,7 +5,7 @@
 
 import org.team100.frc2024.motion.AmpUtil;
 import org.team100.lib.commands.drivetrain.DriveWithWaypoints;
-import org.team100.lib.controller.DriveMotionController;
+import org.team100.lib.follower.DriveTrajectoryFollower;
 import org.team100.lib.logging.SupplierLogger2;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
@@ -18,7 +18,7 @@ public class Amp extends SequentialCommandGroup {
     public Amp(
             SupplierLogger2 parent,
             SwerveDriveSubsystem m_swerve,
-            DriveMotionController controller,
+            DriveTrajectoryFollower controller,
             SwerveKinodynamics limits) {
 
         List<Pose2d> waypoint = new ArrayList<>();

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java b/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java
@@ -34,9 +34,9 @@
 import org.team100.lib.async.AsyncFactory;
 import org.team100.lib.commands.AllianceCommand;
 import org.team100.lib.commands.FullCycle;
-import org.team100.lib.commands.drivetrain.DriveInACircle;
 import org.team100.lib.commands.drivetrain.FancyTrajectory;
 import org.team100.lib.commands.drivetrain.SetRotation;
+import org.team100.lib.commands.drivetrain.for_testing.DriveInACircle;
 import org.team100.lib.commands.drivetrain.manual.DriveManually;
 import org.team100.lib.commands.drivetrain.manual.FieldManualWithNoteRotation;
 import org.team100.lib.commands.drivetrain.manual.ManualChassisSpeeds;
@@ -48,13 +48,13 @@
 import org.team100.lib.commands.drivetrain.manual.ManualWithTargetLock;
 import org.team100.lib.commands.drivetrain.manual.SimpleManualModuleStates;
 import org.team100.lib.config.Identity;
-import org.team100.lib.controller.DriveMotionController;
-import org.team100.lib.controller.DriveMotionControllerFactory;
-import org.team100.lib.controller.DriveMotionControllerUtil;
-import org.team100.lib.controller.DrivePIDFController;
-import org.team100.lib.controller.HolonomicDriveController100;
-import org.team100.lib.controller.HolonomicDriveController3;
+import org.team100.lib.controller.drivetrain.HolonomicDriveControllerFactory;
+import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController;
 import org.team100.lib.dashboard.Glassy;
+import org.team100.lib.follower.DrivePIDFFollower;
+import org.team100.lib.follower.DriveTrajectoryFollower;
+import org.team100.lib.follower.DriveTrajectoryFollowerFactory;
+import org.team100.lib.follower.DriveTrajectoryFollowerUtil;
 import org.team100.lib.framework.TimedRobot100;
 import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.hid.DriverControl;
@@ -230,13 +230,14 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
         // FullStateDriveController();
 
         // cartesian position, rotation full-state.
-        final HolonomicDriveController100 halfFullStateController = new HolonomicDriveController100(comLog);
+        HolonomicFieldRelativeController.Log hlog = new HolonomicFieldRelativeController.Log(comLog);
+        HolonomicFieldRelativeController halfFullStateController = HolonomicDriveControllerFactory.get(hlog);
 
         final List<TimingConstraint> constraints = new TimingConstraintFactory(swerveKinodynamics).allGood();
 
-        final DriveMotionControllerUtil util = new DriveMotionControllerUtil(comLog);
-        final DriveMotionControllerFactory driveControllerFactory = new DriveMotionControllerFactory(util);
-        DrivePIDFController.Log PIDFlog = new DrivePIDFController.Log(comLog);
+        final DriveTrajectoryFollowerUtil util = new DriveTrajectoryFollowerUtil(comLog);
+        final DriveTrajectoryFollowerFactory driveControllerFactory = new DriveTrajectoryFollowerFactory(util);
+        DrivePIDFFollower.Log PIDFlog = new DrivePIDFFollower.Log(comLog);
 
         whileTrue(driverControl::driveWithFancyTrajec,
                 new FancyTrajectory(
@@ -245,9 +246,8 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
                         driveControllerFactory.fancyPIDF(PIDFlog),
                         constraints));
 
-        // 254 PID follower
-        final HolonomicDriveController3 controller = new HolonomicDriveController3(comLog);
-        final DriveMotionController drivePID = driveControllerFactory.goodPIDF(PIDFlog);
+        final HolonomicFieldRelativeController controller = HolonomicDriveControllerFactory.get(hlog);
+        final DriveTrajectoryFollower drivePID = driveControllerFactory.goodPIDF(PIDFlog);
 
         whileTrue(driverControl::driveToNote,
                 new DriveWithProfileNote(

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainerParkingLot.java b/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainerParkingLot.java
@@ -7,22 +7,23 @@
 import org.team100.lib.async.Async;
 import org.team100.lib.async.AsyncFactory;
 import org.team100.lib.commands.drivetrain.CommandMaker;
-import org.team100.lib.commands.drivetrain.DrawSquare;
-import org.team100.lib.commands.drivetrain.DriveInACircle;
-import org.team100.lib.commands.drivetrain.DriveInALittleSquare;
 import org.team100.lib.commands.drivetrain.DriveToState101;
 import org.team100.lib.commands.drivetrain.DriveToWaypoint100;
 import org.team100.lib.commands.drivetrain.FullStateTrajectoryListCommand;
-import org.team100.lib.commands.drivetrain.Oscillate;
 import org.team100.lib.commands.drivetrain.PermissiveTrajectoryListCommand;
-import org.team100.lib.commands.drivetrain.Spin;
 import org.team100.lib.commands.drivetrain.TrajectoryListCommand;
-import org.team100.lib.controller.DriveMotionController;
-import org.team100.lib.controller.DriveMotionControllerFactory;
-import org.team100.lib.controller.DriveMotionControllerUtil;
-import org.team100.lib.controller.DrivePIDFController;
-import org.team100.lib.controller.HolonomicDriveController3;
+import org.team100.lib.commands.drivetrain.for_testing.DrawSquare;
+import org.team100.lib.commands.drivetrain.for_testing.DriveInACircle;
+import org.team100.lib.commands.drivetrain.for_testing.DriveInALittleSquare;
+import org.team100.lib.commands.drivetrain.for_testing.Oscillate;
+import org.team100.lib.commands.drivetrain.for_testing.Spin;
+import org.team100.lib.controller.drivetrain.HolonomicDriveControllerFactory;
+import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController;
 import org.team100.lib.dashboard.Glassy;
+import org.team100.lib.follower.DrivePIDFFollower;
+import org.team100.lib.follower.DriveTrajectoryFollower;
+import org.team100.lib.follower.DriveTrajectoryFollowerFactory;
+import org.team100.lib.follower.DriveTrajectoryFollowerUtil;
 import org.team100.lib.framework.TimedRobot100;
 import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.hid.DriverControl;
@@ -121,8 +122,8 @@ public class RobotContainerParkingLot implements Glassy {
                 poseEstimator,
                 swerveLocal,
                 visionDataProvider);
-
-        HolonomicDriveController3 controller = new HolonomicDriveController3(driveLogger);
+        HolonomicFieldRelativeController.Log hlog = new HolonomicFieldRelativeController.Log(driveLogger);
+        HolonomicFieldRelativeController controller = HolonomicDriveControllerFactory.get(hlog);
 
         ///////////////////////
 
@@ -151,11 +152,11 @@ public class RobotContainerParkingLot implements Glassy {
         List<TimingConstraint> constraints = new TimingConstraintFactory(swerveKinodynamics).allGood();
 
         // 254 PID follower
-        final DriveMotionControllerUtil util = new DriveMotionControllerUtil(driveLogger);
-        final DriveMotionControllerFactory driveControllerFactory = new DriveMotionControllerFactory(util);
-        DrivePIDFController.Log PIDFlog = new DrivePIDFController.Log(driveLogger);
+        final DriveTrajectoryFollowerUtil util = new DriveTrajectoryFollowerUtil(driveLogger);
+        final DriveTrajectoryFollowerFactory driveControllerFactory = new DriveTrajectoryFollowerFactory(util);
+        DrivePIDFFollower.Log PIDFlog = new DrivePIDFFollower.Log(driveLogger);
 
-        DriveMotionController drivePID = driveControllerFactory.autoPIDF(PIDFlog);
+        DriveTrajectoryFollower drivePID = driveControllerFactory.autoPIDF(PIDFlog);
         whileTrue(driverControl::never,
                 new DriveToWaypoint100(
                         driveLogger,
@@ -170,7 +171,7 @@ public class RobotContainerParkingLot implements Glassy {
 
         // 254 FF follower
 
-        DriveMotionController driveFF = driveControllerFactory.ffOnly(PIDFlog);
+        DriveTrajectoryFollower driveFF = driveControllerFactory.ffOnly(PIDFlog);
 
         whileTrue(driverControl::never,
                 new DriveToWaypoint100(
@@ -185,7 +186,7 @@ public class RobotContainerParkingLot implements Glassy {
         ///////////////////////
 
         // 254 Pursuit follower
-        DriveMotionController drivePP = DriveMotionControllerFactory.purePursuit(driveLogger, swerveKinodynamics);
+        DriveTrajectoryFollower drivePP = DriveTrajectoryFollowerFactory.purePursuit(driveLogger, swerveKinodynamics);
 
         whileTrue(driverControl::test,
                 new DriveToWaypoint100(
@@ -211,7 +212,7 @@ public class RobotContainerParkingLot implements Glassy {
 
         // 254 Ramsete follower
         // this one seems to have a pretty high tolerance?
-        DriveMotionController driveRam = DriveMotionControllerFactory.ramsete(driveLogger);
+        DriveTrajectoryFollower driveRam = DriveTrajectoryFollowerFactory.ramsete(driveLogger);
         whileTrue(driverControl::never,
                 new DriveToWaypoint100(
                         driveLogger,
@@ -247,8 +248,9 @@ public class RobotContainerParkingLot implements Glassy {
                         maker.permissiveSquare(), viz));
 
         // one-meter square with position and velocity feedback control
+        HolonomicFieldRelativeController fscontroller = HolonomicDriveControllerFactory.get(hlog);
         whileTrue(driverControl::never,
-                new FullStateTrajectoryListCommand(driveLogger, m_drive,
+                new FullStateTrajectoryListCommand(driveLogger, fscontroller, m_drive,
                         maker::square, viz));
 
         // this should be a field.

diff --git a/...swerve100/src/main/java/org/team100/frc2024/commands/drivetrain/DriveWithProfileNote.java b/...swerve100/src/main/java/org/team100/frc2024/commands/drivetrain/DriveWithProfileNote.java
@@ -4,8 +4,7 @@
 import java.util.function.Supplier;
 
 import org.team100.frc2024.motion.intake.Intake;
-import org.team100.lib.controller.HolonomicDriveController100;
-import org.team100.lib.controller.State100;
+import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController;
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.experiments.Experiment;
 import org.team100.lib.experiments.Experiments;
@@ -18,6 +17,7 @@
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.profile.TrapezoidProfile100;
+import org.team100.lib.state.State100;
 import org.team100.lib.telemetry.Telemetry.Level;
 import org.team100.lib.util.Math100;
 
@@ -40,7 +40,7 @@ public class DriveWithProfileNote extends Command implements Glassy  {
     private final Intake m_intake;
     private final Supplier<Optional<Translation2d>> m_fieldRelativeGoal;
     private final SwerveDriveSubsystem m_swerve;
-    private final HolonomicDriveController100 m_controller;
+    private final HolonomicFieldRelativeController m_controller;
     private final SwerveKinodynamics m_limits;
     private final TrapezoidProfile100 xProfile;
     private final TrapezoidProfile100 yProfile;
@@ -61,7 +61,7 @@ public DriveWithProfileNote(
             Intake intake,
             Supplier<Optional<Translation2d>> fieldRelativeGoal,
             SwerveDriveSubsystem drivetrain,
-            HolonomicDriveController100 controller,
+            HolonomicFieldRelativeController controller,
             SwerveKinodynamics limits) {
         m_field_log = fieldLogger;
         SupplierLogger2 child = parent.child(this);

diff --git a/...ve100/src/main/java/org/team100/frc2024/commands/drivetrain/manual/ManualWithAmpLock.java b/...ve100/src/main/java/org/team100/frc2024/commands/drivetrain/manual/ManualWithAmpLock.java
@@ -4,7 +4,6 @@
 
 import org.team100.frc2024.motion.drivetrain.ShooterUtil;
 import org.team100.lib.commands.drivetrain.manual.FieldRelativeDriver;
-import org.team100.lib.controller.State100;
 import org.team100.lib.framework.TimedRobot100;
 import org.team100.lib.geometry.TargetUtil;
 import org.team100.lib.hid.DriverControl;
@@ -13,6 +12,7 @@
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.profile.TrapezoidProfile100;
 import org.team100.lib.sensors.Gyro;
+import org.team100.lib.state.State100;
 import org.team100.lib.logging.FieldLogger;
 import org.team100.lib.logging.SupplierLogger2;
 import org.team100.lib.logging.SupplierLogger2.DoubleSupplierLogger2;

diff --git a/...0/src/main/java/org/team100/frc2024/commands/drivetrain/manual/ManualWithShooterLock.java b/...0/src/main/java/org/team100/frc2024/commands/drivetrain/manual/ManualWithShooterLock.java
@@ -5,7 +5,6 @@
 
 import org.team100.frc2024.motion.drivetrain.ShooterUtil;
 import org.team100.lib.commands.drivetrain.manual.FieldRelativeDriver;
-import org.team100.lib.controller.State100;
 import org.team100.lib.experiments.Experiment;
 import org.team100.lib.experiments.Experiments;
 import org.team100.lib.framework.TimedRobot100;
@@ -17,6 +16,7 @@
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.profile.TrapezoidProfile100;
 import org.team100.lib.sensors.Gyro;
+import org.team100.lib.state.State100;
 import org.team100.lib.logging.FieldLogger;
 import org.team100.lib.logging.SupplierLogger2;
 import org.team100.lib.logging.SupplierLogger2.DoubleSupplierLogger2;

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/AutoMaker.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/AutoMaker.java
@@ -12,11 +12,11 @@
 import org.team100.frc2024.motion.shooter.DrumShooter;
 import org.team100.frc2024.motion.shooter.RampShooter;
 import org.team100.lib.commands.drivetrain.DriveToWaypoint100;
-import org.team100.lib.controller.DriveMotionController;
-import org.team100.lib.controller.DriveMotionControllerFactory;
-import org.team100.lib.controller.DrivePIDFController;
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.field.FieldPoint2024;
+import org.team100.lib.follower.DrivePIDFFollower;
+import org.team100.lib.follower.DriveTrajectoryFollower;
+import org.team100.lib.follower.DriveTrajectoryFollowerFactory;
 import org.team100.lib.logging.SupplierLogger2;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.timing.TimingConstraint;
@@ -41,7 +41,7 @@ public class AutoMaker implements Glassy {
     private static final double kMaxAccelM_S_S = 2;
 
     private final SwerveDriveSubsystem m_swerve;
-    private final DriveMotionController m_controller;
+    private final DriveTrajectoryFollower m_controller;
     private final SensorInterface m_sensors;
     private final List<TimingConstraint> m_constraints;
     private final Intake m_intake;
@@ -53,16 +53,16 @@ public class AutoMaker implements Glassy {
      * methods below. TODO: pass it to the factories?
      */
     private final SupplierLogger2 m_logger;
-    private final DrivePIDFController.Log m_log;
+    private final DrivePIDFFollower.Log m_log;
     private final TrajectoryCommand100.Log m_commandLog;
-    private final DriveMotionControllerFactory m_factory;
+    private final DriveTrajectoryFollowerFactory m_factory;
     private final TrajectoryVisualization m_viz;
 
     public AutoMaker(
             SupplierLogger2 parent,
             SwerveDriveSubsystem swerve,
-            DriveMotionControllerFactory factory,
-            DriveMotionController controller,
+            DriveTrajectoryFollowerFactory factory,
+            DriveTrajectoryFollower controller,
             double shooterScale,
             FeederSubsystem feeder,
             DrumShooter shooter,
@@ -80,7 +80,7 @@ public AutoMaker(
         m_intake = intake;
         m_sensors = sensor;
         m_logger = parent.child(this);
-        m_log = new DrivePIDFController.Log(m_logger);
+        m_log = new DrivePIDFFollower.Log(m_logger);
         m_commandLog = new TrajectoryCommand100.Log(m_logger);
         m_viz = viz;
     }

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/TrajectoryCommand100.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/TrajectoryCommand100.java
@@ -1,13 +1,13 @@
 package org.team100.frc2024.motion;
 
-import org.team100.lib.controller.DriveMotionController;
 import org.team100.lib.dashboard.Glassy;
-import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
+import org.team100.lib.follower.DriveTrajectoryFollower;
 import org.team100.lib.logging.SupplierLogger2;
 import org.team100.lib.logging.SupplierLogger2.BooleanSupplierLogger2;
 import org.team100.lib.logging.SupplierLogger2.ChassisSpeedsLogger;
 import org.team100.lib.logging.SupplierLogger2.DoubleSupplierLogger2;
 import org.team100.lib.logging.SupplierLogger2.Pose2dLogger;
+import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.telemetry.Telemetry.Level;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.TrajectoryTimeIterator;
@@ -46,7 +46,7 @@ public Log(SupplierLogger2 parent) {
 
     private final Log m_log;
     private final SwerveDriveSubsystem m_robotDrive;
-    private final DriveMotionController m_controller;
+    private final DriveTrajectoryFollower m_controller;
     private final Trajectory100 m_trajectory;
     private final Pose2d m_goal;
     private final TrajectoryVisualization m_viz;
@@ -55,7 +55,7 @@ public TrajectoryCommand100(
             Log log,
             SwerveDriveSubsystem robotDrive,
             Trajectory100 trajectory,
-            DriveMotionController controller,
+            DriveTrajectoryFollower controller,
             TrajectoryVisualization viz) {
         m_log = log;
         m_robotDrive = robotDrive;

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFastThenSlow.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFastThenSlow.java
@@ -1,7 +1,7 @@
 package org.team100.frc2024.motion.amp;
 
-import org.team100.lib.controller.State100;
 import org.team100.lib.profile.TrapezoidProfile100;
+import org.team100.lib.state.State100;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java
@@ -5,7 +5,6 @@
 import org.team100.lib.config.Feedforward100;
 import org.team100.lib.config.Identity;
 import org.team100.lib.config.PIDConstants;
-import org.team100.lib.controller.State100;
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.encoder.AS5048RotaryPositionSensor;
 import org.team100.lib.encoder.CANSparkEncoder;
@@ -24,6 +23,7 @@
 import org.team100.lib.motor.NeoCANSparkMotor;
 import org.team100.lib.motor.SimulatedBareMotor;
 import org.team100.lib.profile.Profile100;
+import org.team100.lib.state.State100;
 import org.team100.lib.logging.SupplierLogger2;
 
 import edu.wpi.first.math.controller.PIDController;

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpState.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpState.java
@@ -2,9 +2,9 @@
 
 import java.util.OptionalDouble;
 
-import org.team100.lib.controller.State100;
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.profile.Profile100;
+import org.team100.lib.state.State100;
 
 import edu.wpi.first.wpilibj2.command.Command;
 

diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java
@@ -5,7 +5,7 @@
 import org.team100.frc2024.motion.intake.Intake;
 import org.team100.frc2024.motion.shooter.DrumShooter;
 import org.team100.lib.commands.drivetrain.DriveWithProfile2;
-import org.team100.lib.controller.HolonomicDriveController100;
+import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController;
 import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
@@ -36,7 +36,7 @@ public class DriveToAmp extends SequentialCommandGroup {
 
     public DriveToAmp(
             SwerveDriveSubsystem drive,
-            HolonomicDriveController100 controller,
+            HolonomicFieldRelativeController controller,
             SwerveKinodynamics limits,
             AmpPivot amp,
             AmpFeeder ampFeeder,