moar tuning nd more setpoints and auto

BitBucketsFRC4183 · Mar 9, 2025 · 3b89e8a · 3b89e8a
1 parent 9445449
commit 3b89e8a
Show file tree

Hide file tree

Showing 11 changed files with 82 additions and 27 deletions.
diff --git a/src/main/java/frc/robot/OperatorInput.java b/src/main/java/frc/robot/OperatorInput.java
@@ -53,8 +53,8 @@ public class OperatorInput {
   final Trigger armElevatorL2 = operator.x();
   final Trigger armElevatorL3 = operator.y();
   final Trigger armElevatorL4 = operator.b();
-  final Trigger rollersIn = operator.rightBumper();
-  final Trigger rollersOut = operator.leftBumper();
+  final Trigger rollersIn = operator.leftBumper();
+  final Trigger rollersOut = operator.rightBumper();
 
   public static final Trigger mechanismLimitOverride = operator.back();
   final Trigger rollerPivotUp = operator.povUp();

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -202,9 +202,11 @@ public RobotContainer() {
 //    autoChooser.addRoutine("ThreeL4CoralBottom", AutoSubsystem::ThreeL4CoralBottomRoutine);
 //    autoChooser.addRoutine("ThreeL4CoralTop", AutoSubsystem::ThreeL4CoralTopRoutine);
 //    autoChooser.addRoutine("OneL4CoralMid", AutoSubsystem::OneL4CoralMidRoutine);
-    autoChooser.addCmd("Score1L4Coral", autoSubsystem::OneL4Score);
+    autoChooser.addCmd("Score1L3Coral", () -> autoSubsystem.OneL3Score(elevatorSubsystem,armSubsystem));
     autoChooser.addCmd("nothing", Commands::none);
     autoChooser.addCmd("TaxiBack", autoSubsystem::TaxiBack);
+    autoChooser.addCmd("TaxiBack", autoSubsystem::TaxiForward);
+
 //    autoChooser.addCmd("DriveSysIDQuasistaticForward", () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
 //    autoChooser.addCmd("DriveSysIDQuasistaticReverse", () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
 //    autoChooser.addCmd("DriveSysIDDynamicForward", () -> driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
@@ -258,8 +260,8 @@ void loadCommands() {
     operatorInput.manualElevator.whileTrue(new ManualElevatorCommand(elevatorSubsystem, operatorController::getLeftY));
 
     // rollers and pivot, algae intake stuff
-    operatorInput.rollersIn.whileTrue(new IntakeSetRollersCommand(intakeSubsystem, false));
-    operatorInput.rollersOut.whileTrue(new IntakeSetRollersCommand(intakeSubsystem, true));
+    operatorInput.rollersIn.whileTrue(new IntakeSetRollersCommand(intakeSubsystem, true));
+    operatorInput.rollersOut.whileTrue(new IntakeSetRollersCommand(intakeSubsystem, false));
     operatorInput.rollerPivotDown.whileTrue(new PivotDownCommand(intakeSubsystem));
     operatorInput.rollerPivotUp.whileTrue(new PivotUpCommand(intakeSubsystem));
 

diff --git a/src/main/java/frc/robot/commands/ArmCommands/ArmBendCommand.java b/src/main/java/frc/robot/commands/ArmCommands/ArmBendCommand.java
@@ -37,7 +37,7 @@ public void execute() {
         // timestampAverageBuffer.addValue(armSubsystem.getCurrentAngle(), Timer.getFPGATimestamp());
         Logger.recordOutput("ArmSubsystem/target_voltage", calculatedVolts);
         Logger.recordOutput("ArmSubsystem/desired_position", armSubsystem.armFeedback.getSetpoint().position);
-        this.armSubsystem.setArmVoltage(calculatedVolts);
+        this.armSubsystem.setArmVoltageCommandBypass(calculatedVolts);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/ArmCommands/ArmHoverCommand.java b/src/main/java/frc/robot/commands/ArmCommands/ArmHoverCommand.java
@@ -26,7 +26,7 @@ public void execute(){
             armSubsystem.hoverAngle = armSubsystem.getCurrentAngle();
             Logger.recordOutput("ArmSubsystem/target_voltage", appliedVolts);
             Logger.recordOutput("ArmSubsystem/target_Angle", armSubsystem.hoverAngle);
-            armSubsystem.setArmVoltage(appliedVolts);
+            armSubsystem.setArmVoltageCommandBypass(appliedVolts);
         }
 
     }

diff --git a/src/main/java/frc/robot/commands/ArmElevatorToOrigin.java b/src/main/java/frc/robot/commands/ArmElevatorToOrigin.java
@@ -18,16 +18,16 @@ public ArmElevatorToOrigin(ElevatorSubsystem elevator, ArmSubsystem arm) {
                 Commands.waitSeconds(Constants.commandTimeout + 1),
                 Commands.sequence(
                         Commands.deadline(
-                                Commands.waitSeconds(0.2),
+                                Commands.waitSeconds(0.3),
                                 new ArmBendCommand(arm, ArmConstants.MIN_ANGLE_RADS)
+                        )
                         ,
                         Commands.parallel(
                                 new ArmBendCommand(arm, ArmConstants.MIN_ANGLE_RADS),
-                                new ElevatorSetPointCommand(elevator, -0.05))
+                                new ElevatorSetPointCommand(elevator, -0.01))
                         )
 
                 )
-                )
 
         );
     }

diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorSetPointCommand.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorSetPointCommand.java
@@ -46,7 +46,7 @@ public void execute(){
         Logger.recordOutput("ElevatorSubsystem/current_velocity", elevator.getElevatorHeightSpeed());
 
 
-        this.elevator.setElevatorVoltage(calculatedVolts);
+        this.elevator.setElevatorVoltageCommandBypass(calculatedVolts);
 
     }
 

diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -17,8 +17,8 @@ public class ArmConstants {
     public static double kV = 1.5096;
     public static double kG = 0.18;
     public static double kA = 0.33715;
-    public static double kP = 0;
-    public static double kI = 0;
+    public static double kP = 3;
+    public static double kI = 0.2;
     public static double kD = 0;
     public static final double kArmToleranceRPS = 0;
     public static final double MAX_ANGLE_RADS = Units.degreesToRadians(76);
@@ -49,7 +49,7 @@ public class ArmConstants {
     public final static double armL1Angle = Units.degreesToRadians(25.5);
     public final static double armL2Angle = Units.degreesToRadians(25.5);
     public final static double armL3Angle = Units.degreesToRadians(56.4);
-    public final static double armL4Angle = Units.degreesToRadians(80);
+    public final static double armL4Angle = Units.degreesToRadians(77);
     public static InvertedValue arm1Inverted = InvertedValue.Clockwise_Positive;
     public static InvertedValue arm2Inverted = InvertedValue.CounterClockwise_Positive;
 }
diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -38,18 +38,18 @@ public class ElevatorConstants {
     public static double kV = 10.604;
     public static double kG = 0;
     public static double kA = 2.2017;
-    public static double kP = 75;
+    public static double kP = 110;
     public static double kI = 0;
     public static double kD = 0;
 
 
     public static double L1 = 0;
     public static double L2 = -0.244;
     public static double L3 = 0.0885;
-    public static double L4 = 0.60;
+    public static double L4 = 0.607;
     public static double carriageMass = 20.0;
-    public static double maxHeight = 0.62;
-    public static double minHeight = -0.240;
+    public static double maxHeight = 0.617;
+    public static double minHeight = -0.280;
     public static double maxVelocity = 1.5;
     public static double maxAcceleration = 1.5;
 

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmSubsystem.java
@@ -84,7 +84,7 @@ public void setArmVoltage(double volts){
             outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.07;
         } else if (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS) {
             outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.07;
-        } else if (getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS + Units.degreesToRadians(10)) {
+        } else if (getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS + Units.degreesToRadians(8)) {
             outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.33;
         } else if (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS - Units.degreesToRadians(8)) {
             outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.25;
@@ -94,6 +94,23 @@ public void setArmVoltage(double volts){
         armIO.setArmMotorVoltage(outputVoltage);
     }
 
+    public void setArmVoltageCommandBypass(double volts){
+        double outputVoltage = volts;
+        if (OperatorInput.mechanismLimitOverride.getAsBoolean()) {
+            outputVoltage = volts;
+
+
+        } else if ((getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS)) {
+            // if mechanism exceeds limit basically set it to zero
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.07;
+        } else if (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.07;
+        }
+
+        Logger.recordOutput("ArmSubsystem/outputVoltageAdjusted", outputVoltage);
+        armIO.setArmMotorVoltage(outputVoltage);
+    }
+
     public void runCharacterization(double output) {
         // bypasses limits, be careful!!!
         armIO.setArmMotorVoltage(output);

diff --git a/src/main/java/frc/robot/subsystems/AutoSubsystem/AutoSubsystem.java b/src/main/java/frc/robot/subsystems/AutoSubsystem/AutoSubsystem.java
@@ -3,7 +3,9 @@
 import choreo.auto.AutoFactory;
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.*;
+import frc.robot.commands.ArmElevatorToOrigin;
 import frc.robot.commands.ArmElevatorToSetpoint;
 import frc.robot.commands.DriveCommands.RobotRelativeDriveCommand;
 import frc.robot.constants.ArmConstants;
@@ -419,13 +421,20 @@ public static AutoRoutine OneL4CoralMidRoutine() {
     }
 
 
-    public Command OneL4Score() {
-            double yValue = 0.2;
-            return Commands.sequence(
-                    new ArmElevatorToSetpoint(elevator, arm, ElevatorConstants.L4, ArmConstants.armL4Angle),
-                    Commands.waitSeconds(1),
-                    Commands.deadline(Commands.waitSeconds(0.1), new RobotRelativeDriveCommand(drive, () -> 0, () -> yValue, () -> 0))
-                    );
+    public Command OneL3Score(ElevatorSubsystem elevator, ArmSubsystem arm) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(4), new RobotRelativeDriveCommand(drive, () -> 0.2, () -> 0, () -> 0)),
+                Commands.waitSeconds(1),
+                Commands.deadline(Commands.waitSeconds(0.2), new RobotRelativeDriveCommand(drive, () -> -0.2, () -> 0, () -> 0)),
+                Commands.deadline(Commands.waitSeconds(3), new ArmElevatorToSetpoint(elevator, arm, ElevatorConstants.L3 + 0.005, ArmConstants.armL4Angle + Units.degreesToRadians(1))),
+                Commands.deadline(Commands.waitSeconds(0.05), new RobotRelativeDriveCommand(drive, () -> 0.1, () -> 0, () -> 0)),
+                Commands.parallel(
+                        new ArmElevatorToOrigin(elevator, arm),
+                        Commands.deadline(Commands.waitSeconds(0.2), new RobotRelativeDriveCommand(drive, () -> -0.1, () -> 0, () -> 0))
+                        )
+
+
+                );
     }
 
     public Command TaxiBack() {
@@ -437,6 +446,16 @@ public Command TaxiBack() {
                 )
         );
     };
+
+    public Command TaxiForward() {
+        return Commands.sequence(
+                Commands.waitSeconds(0),
+                Commands.deadline(
+                        Commands.waitSeconds(3),
+                        new RobotRelativeDriveCommand(drive, () -> 1, () -> 0, () -> 0)
+                )
+        );
+    };
 }
 
 

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorSubsystem.java
@@ -101,7 +101,7 @@ public void setElevatorVoltage(double volts) {
             outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : 0;
         } else if (getLoadHeight() >= ElevatorConstants.maxHeight) {
             outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : 0;
-        } else if ((getLoadHeight() <= ElevatorConstants.minHeight + 0.06)) {
+        } else if ((getLoadHeight() <= ElevatorConstants.minHeight + 0.05)) {
             outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.333;
         } else if (getLoadHeight() >= ElevatorConstants.maxHeight - 0.06) {
             outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.333;
@@ -110,6 +110,23 @@ public void setElevatorVoltage(double volts) {
         elevatorIO.setElevatorMotorVoltage(outputVoltage);
     }
 
+    public void setElevatorVoltageCommandBypass(double volts) {
+        double outputVoltage = volts;
+        if (OperatorInput.mechanismLimitOverride.getAsBoolean()) {
+            outputVoltage = volts;
+        } else if ((getLoadHeight() <= ElevatorConstants.minHeight)) {
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : 0;
+        } else if (getLoadHeight() >= ElevatorConstants.maxHeight) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : 0;
+        } else if ((getLoadHeight() <= ElevatorConstants.minHeight + 0.01)) {
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.333;
+        } else if (getLoadHeight() >= ElevatorConstants.maxHeight - 0.01) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.333;
+
+        }
+        elevatorIO.setElevatorMotorVoltage(outputVoltage);
+    }
+
     public void runCharacterization(double output) {
         // bypasses limits, be careful!!!
         elevatorIO.setElevatorMotorVoltage(output);
-Original file line number
+Diff line change
@@ Expand Up / @@ -46,7 +46,7 @@ public void execute(){ @@
             Logger.recordOutput("ElevatorSubsystem/current_velocity", elevator.getElevatorHeightSpeed());
-            this.elevator.setElevatorVoltage(calculatedVolts);
+            this.elevator.setElevatorVoltageCommandBypass(calculatedVolts);
         }
@@ Expand Down @@