Merge pull request #34 from Operation-P-E-A-C-C-E-Robotics/needs-testing

Tested 12/7/23 - functional

.pathplanner/settings.json

@@ -0,0 +1,7 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": []
+}

src/main/deploy/pathplanner/paths/Example Path.path

@@ -15,16 +15,31 @@
     },
     {
       "anchor": {
-        "x": 1.2392921205890142,
-        "y": 0.10044234885605928
+        "x": 3.5718555916269823,
+        "y": 0.4317756615721482
       },
       "prevControl": {
-        "x": 1.234535433323163,
-        "y": -0.7795447953263174
+        "x": 3.9524999248504464,
+        "y": 1.6305212482908191
       },
       "nextControl": {
-        "x": 1.2440750895821604,
-        "y": 0.9852916125879988
+        "x": 3.333015305102551,
+        "y": -0.3203930019600163
+      },
+      "isLocked": false
+    },
+    {
+      "anchor": {
+        "x": 0.1290128165013187,
+        "y": 0.8578700644342354
+      },
+      "prevControl": {
+        "x": 0.4965892646287983,
+        "y": 1.4371615164210214
+      },
+      "nextControl": {
+        "x": -0.1873622304831094,
+        "y": 0.3592707627824723
       },
       "isLocked": false
     },
@@ -34,17 +49,17 @@
         "y": 0.0
       },
       "prevControl": {
-        "x": 0.40390930423865623,
-        "y": 0.23436548066413554
+        "x": 1.145487069346421,
+        "y": -1.4028170729397766e-16
       },
       "nextControl": null,
       "isLocked": false
     }
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 1.9,
-      "rotationDegrees": 180.0
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 90.0
     }
   ],
   "constraintZones": [],
@@ -53,11 +68,11 @@
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 360.0
+    "maxAngularAcceleration": 100.0
   },
   "goalEndState": {
     "velocity": 0.0,
-    "rotation": 180.0
+    "rotation": -0.22890635837495632
   },
   "reversed": false,
   "folder": null

src/main/java/frc/lib/swerve/PeaccyRequest.java

@@ -9,6 +9,7 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -56,6 +57,8 @@ public class PeaccyRequest implements SwerveRequest {
     private double holdHeadingAcceleration = 0;
 
     private Timer holdHeadingTrajectoryTimer = new Timer();
+    private final Timer robotMovingTimer = new Timer();
+    private final Timer robotNotMovingTimer = new Timer();
 
     //soft heading current limit parameters
     private DoubleSupplier totalDriveCurrent;
@@ -70,6 +73,7 @@ public class PeaccyRequest implements SwerveRequest {
 
 
     private final double CURRENT_LIMIT_THRESHOLD = 0.01; //percent of the current limit to start throttling at.
+    private final SlewRateLimiter currentLimitSmoother = new SlewRateLimiter(10); //limit the amps per second for the current to change, to help find equilibrium.
 
 
     /**
@@ -100,6 +104,8 @@ public PeaccyRequest(double maxAngularVelocity,
         this.getChassisSpeeds = chassisSpeedsSupplier;
         this.totalDriveCurrent = totalDriveCurrentSupplier;
         this.totalDriveCurrentLimit = softHeadingCurrentLimit;
+        robotMovingTimer.start();
+        robotNotMovingTimer.start();
     }
 
     @Override
@@ -110,10 +116,20 @@ public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule.
         //position correction only works for field centric :|
         if(IsFieldCentric) toApplyTranslation = applyPositionCorrection(toApplyTranslation, parameters.currentPose, parameters.updatePeriod);
 
+        if(toApplyTranslation.getNorm() < 0.1){
+            robotMovingTimer.reset();
+            robotMovingTimer.start();
+        } else {
+            robotNotMovingTimer.reset();
+            robotNotMovingTimer.start();
+        }
+
         //we only do auto heading if there is no manually requested rotational rate
         if(Math.abs(toApplyRotation) <= RotationalDeadband) {
             toApplyRotation = 0;
-            if (HoldHeading || SoftHoldHeading) toApplyRotation = applyAutoHeading(parameters);
+            if ((HoldHeading || SoftHoldHeading)) {
+                toApplyRotation = applyAutoHeading(parameters);
+            } 
         } else {
             //Update the set heading to the current heading. This means that when there is no rotational rate requested,
             //the robot will hold its current heading if HoldHeading or SoftHoldHeading is true,
@@ -303,11 +319,16 @@ private double applyAutoHeading(SwerveControlRequestParameters parameters) {
         var target = headingTrajectory.calculate(holdHeadingTrajectoryTimer.get() + parameters.updatePeriod);
         var error = target.position - currentHeading;
 
+        if(robotMovingTimer.get() < 0.3 && Math.abs(error) < 0.2){
+            return 0;
+        }
+
         SmartDashboard.putNumber("target heading",target.position);
         var feedforward = headingFeedforward.calculate(target.velocity);
         var pGain = error * holdHeadingkP * parameters.updatePeriod;
         SmartDashboard.putNumber("heading error", target.position - currentHeading);
-        if(SoftHoldHeading) pGain = pGain * compress(totalDriveCurrent.getAsDouble(), totalDriveCurrentLimit, CURRENT_LIMIT_THRESHOLD);
+        var currentDraw = currentLimitSmoother.calculate(totalDriveCurrent.getAsDouble());
+        if(SoftHoldHeading) pGain = pGain * compress(currentDraw, totalDriveCurrentLimit, CURRENT_LIMIT_THRESHOLD);
         var delta = pGain + feedforward;
 
         SmartDashboard.putNumber("holdheading gain", delta);
@@ -336,6 +357,8 @@ private Translation2d applyPositionCorrection(Translation2d requestedTranslation
             positionCorrectionRequestedVelocities.removeFirst();
         }
 
+        if(robotMovingTimer.get() < 0.05) return requestedTranslation;
+
         //integrate the requested velocities to get the requested position
         Translation2d requestedPositionDelta = new Translation2d(0, 0);
         for (Translation2d requestedVelocity : positionCorrectionRequestedVelocities) {

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

@@ -33,8 +33,11 @@ public static final class Swerve {
     //smoothing
     public static final double teleopLinearSpeedLimit = 5.0;
     public static final double teleopLinearAngleLimit = 2.0;
+    public static final double teleopNearLinearAngleLimit = 0.5;
     public static final double teleopAngularRateLimit = 3.0;
 
+    public static final double teleopNearLimitThreshold = 0.2; //how close to zero to use more extreme linear angle smoothing.
+
     //deadband
     public static final double teleopLinearSpeedDeadband = 0.1;
     public static final double teleopAngularVelocityDeadband = 0.13;
@@ -51,14 +54,14 @@ public static final class Swerve {
     public static final CANIDs rearLeftIDs =    new CANIDs(11,  13,   12); //module (tuner) 4 / (robot) 3
     public static final CANIDs rearRightIDs =   new CANIDs(2,   4,    3); //module (tuner) 1 / (robot) 0
 
-    public static final Gearing gearing = new Gearing(DriveGearRatios.SDSMK4i_L2, ((150.0 / 7.0) / 1.0), 2.0, 0);
+    public static final Gearing gearing = new Gearing(DriveGearRatios.SDSMK4i_L2, ((150.0 / 7.0) / 1.0), (3.85/2), 0);
     public static final EncoderOffsets offsets = new EncoderOffsets(-0.488770, -0.225342, -0.224609, -0.906738); //todo these offsets are very wrong.
 
     public static final Inversion inversion = new Inversion(false, true, true, false); //herra 4.5, 6
 
     //inertia only used for simulation
     public static final Physics physics = new Physics(0.01,1, 50, 10);
-    public static final double steerMotorCurrentLimit = 20; //amps
+    public static final double steerMotorCurrentLimit = 40; //amps
 
     public static final PidGains driveGains = new PidGains(0.35, 0, 0, 0.11, 0.3); 
     public static final PidGains angleGains = new PidGains(90, 0, 0.001, 0, 0);
@@ -89,7 +92,7 @@ public static final class Swerve {
       new PIDConstants(5, 0, 0), 
       measuredMaxVelocity, 
       dimensions.frontLeft.getNorm(), 
-      new ReplanningConfig(false, false),
+      new ReplanningConfig(true, true),
       period
     );
   }

src/main/java/frc/robot/RobotContainer.java

@@ -6,14 +6,16 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathPlannerPath;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.lib.sensors.LimelightHelper;
 import frc.robot.commands.PeaccyDrive;
-import frc.robot.commands.TestPeaccyRequest;
 import frc.robot.subsystems.DriveTrain;
 
 public class RobotContainer {
@@ -37,7 +39,7 @@ public class RobotContainer {
   private final JoystickButton closedLoopButton = new JoystickButton(driverController, 5);
 
   /* COMMANDS */
-  private final TestPeaccyRequest peaccyDrive = new TestPeaccyRequest(
+  private final PeaccyDrive peaccyDrive = new PeaccyDrive(
     () -> -driverController.getRawAxis(translationAxis),  //translation
     () -> -driverController.getRawAxis(strafeAxis),       //strafe
     () -> -driverController.getRawAxis(rotationAxis),     //rotation
@@ -59,6 +61,12 @@ public RobotContainer() {
 
   private void configureBindings() {
     driveTrain.setDefaultCommand(peaccyDrive);
+
+    // new JoystickButton(driverController,1).onTrue(new InstantCommand(
+    //   () -> {
+    //     Command test = driveTrain.driveToPose(new Pose2d());
+    //     test.schedule();
+    //   }, driveTrain));
   }
 
 

src/main/java/frc/robot/commands/TestPeaccyRequest.java → src/main/java/frc/robot/commands/CTRERequestPeaccyDrive.java

@@ -9,17 +9,18 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.lib.swerve.PeaccyRequest;
 import frc.lib.telemetry.SwerveTelemetry;
 import frc.robot.Constants;
 import frc.robot.subsystems.DriveTrain;
 
-public class TestPeaccyRequest extends Command {
+public class CTRERequestPeaccyDrive extends Command {
     private final DoubleSupplier xVelocitySup, yVelocitySup, angularVelocitySup, autoHeadingAngleSup;
     private final BooleanSupplier isAutoAngleSup, isFieldRelativeSup, isOpenLoopSup, isLockInSup;
     private final DriveTrain driveTrain;
 
-    private final PeaccyRequest help;
+    private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric();
+    private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric();
+    private final SwerveRequest.FieldCentricFacingAngle autoHeadingRequest = new SwerveRequest.FieldCentricFacingAngle();
     private final SwerveRequest.SwerveDriveBrake lockInRequest = new SwerveRequest.SwerveDriveBrake().withIsOpenLoop(false);
 
     private final SlewRateLimiter linearSpeedLimiter = new SlewRateLimiter(Constants.Swerve.teleopLinearSpeedLimit);
@@ -46,7 +47,7 @@ public class TestPeaccyRequest extends Command {
      * @param isZeroOdometry whether or not to zero the odometry
      * @param driveTrain the swerve subsystem
      */
-    public TestPeaccyRequest(DoubleSupplier xVelocitySup,
+    public CTRERequestPeaccyDrive(DoubleSupplier xVelocitySup,
                       DoubleSupplier yVelocitySup,
                       DoubleSupplier angularVelocitySup,
                       DoubleSupplier autoHeadingAngleSup,
@@ -67,19 +68,9 @@ public TestPeaccyRequest(DoubleSupplier xVelocitySup,
         this.isZeroOdometrySup = isZeroOdometry;
         this.driveTrain = driveTrain;
 
-        help  = new PeaccyRequest(
-            50, 
-            70,
-            2600, 
-            0.0, 
-            0.0, 
-            driveTrain::getChassisSpeeds, 
-            driveTrain::getTotalDriveCurrent, 
-            30
-        ).withRotationalDeadband(Constants.Swerve.teleopAngularVelocityDeadband)
-        .withSoftHoldHeading(false)
-        .withPositionCorrectionIterations(4)
-        .withPositionCorrectionWeight(1);
+        autoHeadingRequest.HeadingController.setP(Constants.Swerve.autoHeadingKP);
+        autoHeadingRequest.HeadingController.setI(Constants.Swerve.autoHeadingKI);
+        autoHeadingRequest.HeadingController.setD(Constants.Swerve.autoHeadingKD);
 
         addRequirements(driveTrain);
     }
@@ -97,10 +88,7 @@ public void execute() {
         boolean isLockIn = isLockInSup.getAsBoolean();
         boolean isZeroOdometry = isZeroOdometrySup.getAsBoolean();
 
-        if(isZeroOdometry) {
-            driveTrain.resetOdometry();
-            help.withHeading(driveTrain.getPose().getRotation().getRadians());
-        }
+        if(isZeroOdometry) driveTrain.resetOdometry();
 
         // handle smoothing and deadbanding
         Translation2d linearVelocity = new Translation2d(xVelocity, yVelocity);
@@ -130,18 +118,41 @@ public void execute() {
             }
         }
 
-       help.withVelocityX(linearVelocity.getX())
-            .withVelocityY(linearVelocity.getY())
-            .withRotationalRate(angularVelocity)
-            .withIsOpenLoop(true)
-            .withIsFieldCentric(isFieldRelative)
-            .withHoldHeading(true);
+        //handle auto angle
+        if (isAutoHeading) {
+            //convert angle to be -180 to 180
+            autoHeadingAngle = Math.IEEEremainder(autoHeadingAngle, 360);
+            if (autoHeadingAngle > 180) autoHeadingAngle -= 360;
+            if (autoHeadingAngle < -180) autoHeadingAngle += 360;
+
+            autoHeadingRequest.withIsOpenLoop(isOpenLoop)
+                            .withVelocityX(linearVelocity.getX())
+                            .withVelocityY(linearVelocity.getY())
+                            .withTargetDirection(Rotation2d.fromDegrees(autoHeadingAngle))
+                            .withRotationalDeadband(0.1); // todo do we need rotational deadband?
+
+            driveTrain.drive(autoHeadingRequest);
+            return;
+        }
+
+        //handle field relative
+        if (isFieldRelative) {
+            fieldCentricRequest.withIsOpenLoop(isOpenLoop)
+                                .withVelocityX(linearVelocity.getX())
+                                .withVelocityY(linearVelocity.getY())
+                                .withRotationalRate(angularVelocity);
 
-        if(isAutoHeading) {
-            help.withHeading(Rotation2d.fromDegrees(autoHeadingAngle).getRadians());
+            driveTrain.drive(fieldCentricRequest);
+            return;
         }
 
-        driveTrain.drive(help);
+        //handle robot relative
+        robotCentricRequest.withIsOpenLoop(isOpenLoop)
+                            .withVelocityX(linearVelocity.getX())
+                            .withVelocityY(linearVelocity.getY())
+                            .withRotationalRate(angularVelocity);
+
+        driveTrain.drive(robotCentricRequest);
     }
 
     private Translation2d smoothAndDeadband (Translation2d linearVelocity) {