diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 46c52d3..fd37e10 100644 --- 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 index 039257f..024b547 100644 --- 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 index ab1c15e..9f0dcc7 100644 --- 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 { 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 index 286c492..e6269f3 100644 --- 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 { 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;