Week 0 changes, drive pid intake tweaking

chopshop-166 · Feb 15, 2025 · 5f8f982 · 5f8f982
1 parent 0af6f70
commit 5f8f982
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -148,14 +148,14 @@ public void configureButtonBindings() {
                 .onFalse(commandSequences.scoreL4());
         copilotController.back().onTrue(elevator.resetCmd());
         copilotController.start().onTrue(elevator.zero());
-
-        driveController.rightBumper().whileTrue(drive.aimAtReefCenter());
         copilotController.getPovButton(POVDirection.RIGHT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL2))
                 .onFalse(elevator.safeStateCmd());
         copilotController.getPovButton(POVDirection.UP).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL3))
                 .onFalse(elevator.safeStateCmd());
         copilotController.getPovButton(POVDirection.LEFT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL1))
                 .onFalse(elevator.safeStateCmd());
+        copilotController.getPovButton(POVDirection.DOWN).whileTrue(elevator.moveTo(ElevatorPresets.STOW))
+                .onFalse(elevator.safeStateCmd());
 
         copilotController.rightBumper().whileTrue(coralManip.feed());
     }

diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -71,7 +71,8 @@ public SwerveDriveMap getDriveMap() {
 
         // Configuration for MK4i with L2 speeds
         Configuration MK4i_L2 = new Configuration(SDSSwerveModule.MK4_V2.gearRatio,
-                SDSSwerveModule.MK4_V2.wheelDiameter, new PIDValues(0.011, 0.00, 0.0002));
+                SDSSwerveModule.MK4_V2.wheelDiameter, new PIDValues(0.011, 0.00, 0.0002),
+                new PIDValues(0.05, 0.0, 0.0, 0.21));
 
         // All Distances are in Meters
         // Front Left Module
@@ -96,7 +97,7 @@ public SwerveDriveMap getDriveMap() {
 
         final double maxDriveSpeedMetersPerSecond = Units.feetToMeters(15);
 
-        final double maxRotationRadianPerSecond = Math.PI;
+        final double maxRotationRadianPerSecond = 2 * Math.PI;
 
         RobotConfig config = new RobotConfig(68, 58, new ModuleConfig(
                 0.1016, 6000, 1.0, DCMotor.getNeoVortex(1), 50, 1),
@@ -144,8 +145,8 @@ public ElevatorMap getElevatorMap() {
         ElevatorFeedforward feedForward = new ElevatorFeedforward(0, 0.01, 0);
 
         return new ElevatorMap(leftMotor, leftMotor.getEncoder(),
-                new ElevatorMap.ElevatorPresetValues(16, 5, 14, 29.5, 56, 57.5, 1),
-                new ValueRange(0, 57.5), new ValueRange(3, 53), pid, feedForward);
+                new ElevatorMap.ElevatorPresetValues(16.6, 5, 14, 29.5, 56, 57.5, 1),
+                new ValueRange(0, 57.5), new ValueRange(6, 53), pid, feedForward);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java
@@ -15,7 +15,8 @@ public class CoralManip extends LoggedSubsystem<Data, CoralManipMap> {
     private final double INTAKE_SPEED = -0.3;
     private final double RELEASE_DELAY = 1;
     // private final double SLOW_SPEED = .
-    private final double DELAY = 0.0;
+    private final double DELAY = 0.5;
+    private final double HOLD_SPEED = -0.05;
 
     public CoralManip(CoralManipMap coralManipMap) {
         super(new Data(), coralManipMap);
@@ -53,7 +54,10 @@ public Command intake() {
         return run(() -> {
             getData().leftMotor.setpoint = INTAKE_SPEED;
             getData().rightMotor.setpoint = INTAKE_SPEED;
-        }).until(() -> getData().gamePieceDetected).andThen(safeStateCmd());
+        }).until(() -> getData().gamePieceDetected).andThen(waitSeconds(DELAY), runOnce(() -> {
+            getData().leftMotor.setpoint = HOLD_SPEED;
+            getData().rightMotor.setpoint = HOLD_SPEED;
+        }));
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -20,7 +20,7 @@ public class Elevator extends LoggedSubsystem<Data, ElevatorMap> {
 
     final ProfiledPIDController pid;
     final double RAISE_SPEED = 1.0;
-    final double MANUAL_LOWER_SPEED_COEF = 0.5;
+    final double MANUAL_LOWER_SPEED_COEF = 1.0;
     final double SLOW_DOWN_COEF = 0.5;
     final double LOWER_SPEED = -0.15;
     final double ZEROING_SPEED = -0.1;