diff --git a/src/main/java/frc/robot/OperatorInput.java b/src/main/java/frc/robot/OperatorInput.java index 5970b5a..149a026 100644 --- 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 index 66c8891..76f98fb 100644 --- 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 index da2562b..b9ff044 100644 --- 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 index 7d1a085..40034ef 100644 --- 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 index af52a75..f19946a 100644 --- 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 index 07a635e..133aad7 100644 --- 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 index e86649b..d648c20 100644 --- 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; } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 315ea5f..e5cfaa2 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -38,7 +38,7 @@ 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; @@ -46,10 +46,10 @@ public class ElevatorConstants { 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 index c37414c..38cbaa8 100644 --- 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 index 15ede62..d6e686a 100644 --- 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 index 329eb14..6c05e6f 100644 --- 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);