Merge remote-tracking branch 'origin/master' into update-libs

Author: CoolSpy3

build.gradle

@@ -92,7 +92,7 @@ publishing {
         gpr(MavenPublication) {
             groupId = 'org.carlmontrobotics'
             artifactId = 'lib199'
-            version = '4.0.0'
+            version = '4.0.2'
 
             from components.java
         }

src/main/java/org/carlmontrobotics/lib199/DummySparkMaxAnswer.java

@@ -2,6 +2,7 @@
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxAbsoluteEncoder;
 import com.revrobotics.SparkMaxAnalogSensor;
 import com.revrobotics.SparkMaxLimitSwitch;
 import com.revrobotics.SparkMaxPIDController;
@@ -23,6 +24,8 @@ public class DummySparkMaxAnswer extends REVLibErrorAnswer {
     public static final SparkMaxAnalogSensor DUMMY_ANALOG_SENSOR = Mocks.mock(SparkMaxAnalogSensor.class, REVLibErrorAnswer.ANSWER);
     public static final SparkMaxLimitSwitch DUMMY_LIMIT_SWITCH = Mocks.mock(SparkMaxLimitSwitch.class, REVLibErrorAnswer.ANSWER);
     public static final SparkMaxPIDController DUMMY_PID_CONTROLLER = Mocks.mock(SparkMaxPIDController.class, ANSWER);
+    public static final SparkMaxAbsoluteEncoder DUMMY_ABSOLUTE_ENCODER = Mocks.mock(SparkMaxAbsoluteEncoder.class, ANSWER);
+
 
     @Override
     public Object answer(InvocationOnMock invocation) throws Throwable {
@@ -41,6 +44,8 @@ public Object answer(InvocationOnMock invocation) throws Throwable {
             return IdleMode.kBrake;
         } else if(returnType == AccelStrategy.class) {
             return AccelStrategy.kTrapezoidal;
+        } else if(returnType == SparkMaxAbsoluteEncoder.class) {
+            return DUMMY_ABSOLUTE_ENCODER;
         }
         return super.answer(invocation);
     }

src/main/java/org/carlmontrobotics/lib199/path/SwerveDriveInterface.java

@@ -21,7 +21,6 @@
 import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 
 public interface SwerveDriveInterface extends DrivetrainInterface {
@@ -113,22 +112,16 @@ public default Command createPPAutoCommand(PathPlannerTrajectory trajectory, Has
      * @return PPSwerveControllerCommand
      */
     public default Command createPPAutoCommand(List<PathPlannerTrajectory> trajectory, HashMap<String, Command> eventMap) {
-        Subsystem[] requirements = eventMap.values().stream().flatMap(command -> command.getRequirements().stream())
-                .toArray(Subsystem[]::new);
-        requirements = Arrays.copyOf(requirements, requirements.length + 1);
-        requirements[requirements.length - 1] = this;
-        requirements = Arrays.stream(requirements).distinct().toArray(Subsystem[]::new);
-        final Subsystem[] finalRequirements = requirements; // Make this final so it can be used in the anonymous inner class
         PIDController[] pidControllers = Arrays.stream(getPIDConstants()).map(constants -> new PIDController(constants[0], constants[1], constants[2])).toArray(PIDController[]::new);
         pidControllers[2].enableContinuousInput(-Math.PI, Math.PI);
         // Use SwerveAutoBuilder because required argument for base auto builder "DrivetrainType" is protected
         return new SwerveAutoBuilder(this::getPose, this::setPose, null, null, null, null, eventMap, true) {
             @Override
             public CommandBase followPath(PathPlannerTrajectory trajectory) {
                 // AutoBuilder will convert this to work with events
-                return new PPSwerveControllerCommand(trajectory, poseSupplier, getKinematics(), pidControllers[0], pidControllers[1], pidControllers[2], SwerveDriveInterface.this::drive, true, finalRequirements /* This will propagate the requirements to the final command via SequentialCommandGroup */);
+                return new PPSwerveControllerCommand(trajectory, poseSupplier, getKinematics(), pidControllers[0], pidControllers[1], pidControllers[2], SwerveDriveInterface.this::drive, true, SwerveDriveInterface.this);
             };
-        }.followPathGroupWithEvents(trajectory);
+        }.fullAuto(trajectory);
     }
 
 }

src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java

@@ -40,7 +40,7 @@ public enum ModuleType {FL, FR, BL, BR};
     private boolean reversed;
     private Timer timer;
     private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward;
-    private double lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff;
+    private double desiredSpeed, lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff;
 
     public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCoder turnEncoder,
                         int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
@@ -126,23 +126,40 @@ public ModuleType getType() {
 
     private double prevTurnVelocity = 0;
     public void periodic() {
-        double measuredAngleDegs = getModuleAngle();
-        TrapezoidProfile.State goal = turnPIDController.getGoal();
-        goal = new TrapezoidProfile.State(goal.position, goal.velocity);
-
-        double period = turnPIDController.getPeriod();
-        double optimalTurnVelocityDps = Math.abs(MathUtil.inputModulus(goal.position-measuredAngleDegs, -180, 180))/period;
-        setMaxTurnVelocity(Math.min(maxAchievableTurnVelocityDps, optimalTurnVelocityDps));
-
-        double turnSpeedCorrectionDps = turnPIDController.calculate(measuredAngleDegs) * turnSimpleMotorFeedforward.maxAchievableVelocity(12,0);
-        TrapezoidProfile.State state = turnPIDController.getSetpoint();
-        double turnVolts = turnSimpleMotorFeedforward.calculate(prevTurnVelocity+turnSpeedCorrectionDps, (state.velocity-prevTurnVelocity) / period);
-        if (!turnPIDController.atGoal()) {
-            turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0));
-        } else {
-            turn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity));
+
+        // Drive Control
+        {
+            double actualSpeed = getCurrentSpeed();
+            double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF :
+                                    backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration);
+
+            // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc.
+            // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed)
+            targetVoltage += drivePIDController.calculate(actualSpeed, desiredSpeed);
+            double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12);
+            drive.setVoltage(appliedVoltage);
+        }
+
+        // Turn Control
+        {
+            double measuredAngleDegs = getModuleAngle();
+            TrapezoidProfile.State goal = turnPIDController.getGoal();
+            goal = new TrapezoidProfile.State(goal.position, goal.velocity);
+
+            double period = turnPIDController.getPeriod();
+            double optimalTurnVelocityDps = Math.abs(MathUtil.inputModulus(goal.position-measuredAngleDegs, -180, 180))/period;
+            setMaxTurnVelocity(Math.min(maxAchievableTurnVelocityDps, optimalTurnVelocityDps));
+
+            double turnSpeedCorrectionDps = turnPIDController.calculate(measuredAngleDegs) * turnSimpleMotorFeedforward.maxAchievableVelocity(12,0);
+            TrapezoidProfile.State state = turnPIDController.getSetpoint();
+            double turnVolts = turnSimpleMotorFeedforward.calculate(prevTurnVelocity+turnSpeedCorrectionDps, (state.velocity-prevTurnVelocity) / period);
+            if (!turnPIDController.atGoal()) {
+                turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0));
+            } else {
+                turn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity));
+            }
+            prevTurnVelocity = state.velocity;
         }
-        prevTurnVelocity = state.velocity;
     }
 
     /**
@@ -177,22 +194,13 @@ private double calculateAntiGravitationalA(Float gyroPitchDeg, Float gyroRollDeg
         SmartDashboard.putNumber("AntiGravitational accelration", antiGravitationalA);
         return antiGravitationalA;
     }
+
     /**
      * Sets the speed for the drive motor controller.
      * @param speedMps     The desired speed in meters per second.
      */
     private void setSpeed(double speedMps) {
-        // Compute desired and actual speeds in m/s
-        double desiredSpeed = speedMps * driveModifier;
-        double actualSpeed = getCurrentSpeed();
-        double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF :
-                                 backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration);
-
-        // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc.
-        // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed)
-        targetVoltage += drivePIDController.calculate(actualSpeed, desiredSpeed);
-        double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12);
-        drive.setVoltage(appliedVoltage);
+        desiredSpeed = speedMps * driveModifier;
     }
 
     /**