diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 39715e9..94563e7 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -1,19 +1,7 @@ package org.sciborgs1155.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.autonomous; -import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.test; -import static org.sciborgs1155.robot.Constants.DEADBAND; import static org.sciborgs1155.robot.Constants.PERIOD; -import static org.sciborgs1155.robot.arm.ArmConstants.INTAKE_ANGLE; -import static org.sciborgs1155.robot.arm.ArmConstants.PROCESSOR_OUTTAKE_ANGLE; -import static org.sciborgs1155.robot.arm.ArmConstants.TROUGH_OUTTAKE_ANGLE; -import static org.sciborgs1155.robot.drive.DriveConstants.MAX_ANGULAR_ACCEL; -import static org.sciborgs1155.robot.drive.DriveConstants.MAX_SPEED; -import static org.sciborgs1155.robot.drive.DriveConstants.TELEOP_ANGULAR_SPEED; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -26,14 +14,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import java.util.Set; import monologue.Annotations.Log; import monologue.Logged; import monologue.Monologue; import org.littletonrobotics.urcl.URCL; import org.sciborgs1155.lib.CommandRobot; import org.sciborgs1155.lib.FaultLogger; -import org.sciborgs1155.lib.InputStream; import org.sciborgs1155.lib.Test; import org.sciborgs1155.robot.Constants.Field.Level; import org.sciborgs1155.robot.Ports.OI; @@ -113,88 +99,88 @@ private void configureGameBehavior() { /** Configures trigger -> command bindings. */ private void configureBindings() { - operator - .povDown() - .onTrue( - elevator - .scoreLevel(Level.L1) - .until(() -> elevator.atGoal()) - .andThen(scoral.outtake().until(() -> scoral.beambreak()))); - operator - .povLeft() - .onTrue( - elevator - .scoreLevel(Level.L2) - .until(() -> elevator.atGoal()) - .andThen(scoral.outtake().until(() -> scoral.beambreak()))); - operator - .povRight() - .onTrue( - elevator - .scoreLevel(Level.L3) - .until(() -> elevator.atGoal()) - .andThen(scoral.outtake().until(() -> scoral.beambreak()))); - operator - .povUp() - .onTrue( - elevator - .scoreLevel(Level.L4) - .until(() -> elevator.atGoal()) - .andThen(scoral.outtake().until(() -> scoral.beambreak()))); - operator.leftBumper().onTrue(arm.climbSetup()); - operator.leftTrigger().onTrue(arm.climbExecute()); - operator.rightBumper().onTrue(arm.goTo(TROUGH_OUTTAKE_ANGLE).alongWith(coroller.outtake())); - operator.rightTrigger().onTrue(arm.goTo(INTAKE_ANGLE).alongWith(coroller.intake())); - operator.a().onTrue(arm.goTo(PROCESSOR_OUTTAKE_ANGLE).alongWith(coroller.outtake())); - - // x and y are switched: we use joystick Y axis to control field x motion - InputStream x = InputStream.of(driver::getLeftY).negate(); - InputStream y = InputStream.of(driver::getLeftX).negate(); - - // Apply speed multiplier, deadband, square inputs, and scale translation to max speed - InputStream r = - InputStream.hypot(x, y) - .log("Robot/raw joystick") - .scale(() -> speedMultiplier) - .clamp(1.0) - .deadband(Constants.DEADBAND, 1.0) - .signedPow(2.0) - .log("Robot/processed joystick") - .scale(MAX_SPEED.in(MetersPerSecond)); - - InputStream theta = InputStream.atan(x, y); - - // Split x and y components of translation input - x = r.scale(theta.map(Math::cos)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond)); - y = r.scale(theta.map(Math::sin)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond)); - - // Apply speed multiplier, deadband, square inputs, and scale rotation to max teleop speed - InputStream omega = - InputStream.of(driver::getRightX) - .negate() - .scale(() -> speedMultiplier) - .clamp(1.0) - .deadband(DEADBAND, 1.0) - .signedPow(2.0) - .scale(TELEOP_ANGULAR_SPEED.in(RadiansPerSecond)) - .rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second))); - - drive.setDefaultCommand(drive.drive(x, y, omega)); - led.setDefaultCommand(led.scrolling()); - - autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy()); - autonomous().whileTrue(led.autos()); - - test().whileTrue(systemsCheck()); - - driver.b().whileTrue(drive.zeroHeading()); - driver - .leftBumper() - .or(driver.rightBumper()) - .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER)) - .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER)); - - driver.leftTrigger().onTrue(hopper.intake().alongWith(scoral.intake())); + // operator + // .povDown() + // .onTrue( + // elevator + // .scoreLevel(Level.L1) + // .until(() -> elevator.atGoal()) + // .andThen(scoral.outtake().until(() -> scoral.beambreak()))); + // operator + // .povLeft() + // .onTrue( + // elevator + // .scoreLevel(Level.L2) + // .until(() -> elevator.atGoal()) + // .andThen(scoral.outtake().until(() -> scoral.beambreak()))); + // operator + // .povRight() + // .onTrue( + // elevator + // .scoreLevel(Level.L3) + // .until(() -> elevator.atGoal()) + // .andThen(scoral.outtake().until(() -> scoral.beambreak()))); + // operator + // .povUp() + // .onTrue( + // elevator + // .scoreLevel(Level.L4) + // .until(() -> elevator.atGoal()) + // .andThen(scoral.outtake().until(() -> scoral.beambreak()))); + // operator.leftBumper().onTrue(arm.climbSetup()); + // operator.leftTrigger().onTrue(arm.climbExecute()); + // operator.rightBumper().onTrue(arm.goTo(TROUGH_OUTTAKE_ANGLE).alongWith(coroller.outtake())); + // operator.rightTrigger().onTrue(arm.goTo(INTAKE_ANGLE).alongWith(coroller.intake())); + // operator.a().onTrue(arm.goTo(PROCESSOR_OUTTAKE_ANGLE).alongWith(coroller.outtake())); + + // // x and y are switched: we use joystick Y axis to control field x motion + // InputStream x = InputStream.of(driver::getLeftY).negate(); + // InputStream y = InputStream.of(driver::getLeftX).negate(); + + // // Apply speed multiplier, deadband, square inputs, and scale translation to max speed + // InputStream r = + // InputStream.hypot(x, y) + // .log("Robot/raw joystick") + // .scale(() -> speedMultiplier) + // .clamp(1.0) + // .deadband(Constants.DEADBAND, 1.0) + // .signedPow(2.0) + // .log("Robot/processed joystick") + // .scale(MAX_SPEED.in(MetersPerSecond)); + + // InputStream theta = InputStream.atan(x, y); + + // // Split x and y components of translation input + // x = r.scale(theta.map(Math::cos)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond)); + // y = r.scale(theta.map(Math::sin)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond)); + + // // Apply speed multiplier, deadband, square inputs, and scale rotation to max teleop speed + // InputStream omega = + // InputStream.of(driver::getRightX) + // .negate() + // .scale(() -> speedMultiplier) + // .clamp(1.0) + // .deadband(DEADBAND, 1.0) + // .signedPow(2.0) + // .scale(TELEOP_ANGULAR_SPEED.in(RadiansPerSecond)) + // .rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second))); + + // drive.setDefaultCommand(drive.drive(x, y, omega)); + // led.setDefaultCommand(led.scrolling()); + + // autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy()); + // autonomous().whileTrue(led.autos()); + + // test().whileTrue(systemsCheck()); + + // driver.b().whileTrue(drive.zeroHeading()); + // driver + // .leftBumper() + // .or(driver.rightBumper()) + // .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER)) + // .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER)); + + // driver.leftTrigger().onTrue(hopper.intake().alongWith(scoral.intake())); } /** @@ -226,15 +212,6 @@ public Command systemsCheck() { .withName("Test Mechanisms"); } - // drive = Drive.create(); - // private final Vision vision = Vision.create(); - // private final LEDStrip led = new LEDStrip(); - // private final Elevator elevator = Elevator.create(); - // private final Scoral scoral = Scoral.create(); - // private final Hopper hopper = Hopper.create(); - // private final Arm arm = Arm.create(); - // private final Coroller coroller = Coroller - @Override public void close() { super.close();