diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19e02bc..66858c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,8 +109,8 @@ public class RobotContainer { drivetrain.setDefaultCommand( drivetrain.drive( - () -> Math.pow(driver.getLeftY(), 3), - () -> Math.pow(driver.getLeftX(), 3), + () -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3), + () -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3), driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3) () -> true ) @@ -143,7 +143,7 @@ public class RobotContainer { ); driver.leftTrigger().whileTrue( - manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25)) + manipulator.runUntilCollected(() -> 0.75) ); driver.start().and(driver.back()).onTrue( @@ -153,6 +153,8 @@ public class RobotContainer { driver.y().whileTrue(drivetrain.zeroHeading()); driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false)); + + driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true)); /* driver.rightBumper().whileTrue( @@ -201,26 +203,28 @@ public class RobotContainer { ) ); - operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5))); + operator.back().onTrue(elevator.homeCommand()); + + operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5))); operator.a().onTrue( - safeMoveManipulator(ElevatorConstants.kL1Position, 0.0) + safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition) ); operator.x().onTrue( safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) - .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) .until(() -> driver.a().getAsBoolean()) ); operator.b().onTrue( safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) - .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) .until(() -> driver.a().getAsBoolean()) ); operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) - .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) .until(() -> driver.a().getAsBoolean()) ); diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 5960e9e..c09a4c3 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -43,8 +43,8 @@ public class ElevatorConstants { public static final double kCoralIntakePosition = 0; public static final double kL1Position = 0; - public static final double kL2Position = 9; - public static final double kL3Position = 23.0; + public static final double kL2Position = 11; + public static final double kL3Position = 27; public static final double kL4Position = 50.5; public static final double kL4TransitionPosition = 40.0; public static final double kL2AlgaePosition = 23.0; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index 722e49c..8f21adb 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -29,26 +29,27 @@ public class ManipulatorPivotConstants { public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41 public static final double kFeedForwardV = 0.68; //calculated value 0.68 - public static final double kFFGravityOffset = Units.degreesToRadians(135.0); + public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90); public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100 public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168 - public static final double kEncoderOffset = 0.780; + public static final double kEncoderOffset = 0.78-0.25; - public static final double kCoralIntakePosition = Units.degreesToRadians(175.0); - public static final double kL1Position = Units.degreesToRadians(0.0); - public static final double kL2Position = Units.degreesToRadians(22.0); - public static final double kL3Position = Units.degreesToRadians(22.0); - public static final double kL4Position = Units.degreesToRadians(45.0); - public static final double kL2AlgaePosition = Units.degreesToRadians(175.0); - public static final double kL3AlgaePosition = Units.degreesToRadians(175.0); - public static final double kProcessorPosition = Units.degreesToRadians(175.0); - public static final double kNetPosition = Units.degreesToRadians(175.0); + public static final double kStartingPosition = Units.degreesToRadians(90); + public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90); + public static final double kL1Position = Units.degreesToRadians(0.0+90); + public static final double kL2Position = Units.degreesToRadians(22.0+90); + public static final double kL3Position = Units.degreesToRadians(22.0+90); + public static final double kL4Position = Units.degreesToRadians(45.0+90); + public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90); + public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90); + public static final double kProcessorPosition = Units.degreesToRadians(175.0+90); + public static final double kNetPosition = Units.degreesToRadians(175.0+90); /**The closest position to the elevator brace without hitting it */ - public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0); + public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90); /**The forward rotation limit of the pivot */ - public static final double kRotationLimit = Units.degreesToRadians(175.0); + public static final double kRotationLimit = Units.degreesToRadians(175.0+90); public static final double kSysIDRampRate = 1; public static final double kSysIDStepVolts = 7; diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index 7e22167..cd2be66 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -4,7 +4,7 @@ public class OIConstants { public static final int kDriverControllerPort = 0; public static final int kOperatorControllerPort = 1; - public static final double kDriveDeadband = 0.05; + public static final double kDriveDeadband = Math.pow(0.05, 3); public static final String kAutoTab = "Auto Tab"; public static final String kSensorsTab = "Sensors Tab"; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index d229ac9..6700502 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -174,6 +174,14 @@ public class Elevator extends SubsystemBase { } + public Command homeCommand(){ + return run(() -> { + elevatorMotor1.setVoltage(0.5); + }) + .until(() -> elevatorMotor1.getOutputCurrent() > 5) + .andThen(run(() -> encoder.setPosition(0))); + } + /** * Moves the elevator to a target destination (setpoint). * @@ -208,7 +216,7 @@ public class Elevator extends SubsystemBase { } }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()) - .andThen(runManualElevator(() -> -.1) + .andThen(runManualElevator(() -> -.5) .until(() -> encoder.getPosition() == 0)); } else { diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 75ca3c0..f1f26e3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase { ); pidController.setSetpoint(0); - pidController.enableContinuousInput(0, 180); + pidController.enableContinuousInput(0, 280); feedForward = new ArmFeedforward( ManipulatorPivotConstants.kFeedForwardS,