From 54710dcc9825bb2ec5fea68d6b644810a52b8f48 Mon Sep 17 00:00:00 2001 From: Luna Date: Fri, 3 May 2024 17:19:40 -0700 Subject: [PATCH] feat: merged main into branch :) --- .../java/org/team1540/robot2024/Robot.java | 7 ++-- .../team1540/robot2024/RobotContainer.java | 10 +++--- .../drivetrain/KidModeSwerveDriveCommand.java | 33 +++++++++++++++++++ 3 files changed, 41 insertions(+), 9 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/drivetrain/KidModeSwerveDriveCommand.java diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 2a639f84..8ec3ed9c 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -1,7 +1,6 @@ package org.team1540.robot2024; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -10,9 +9,10 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.team1540.robot2024.commands.drivetrain.KidModeSwerveDriveCommand; import org.team1540.robot2024.subsystems.led.*; import org.team1540.robot2024.subsystems.led.patterns.*; -import org.team1540.robot2024.util.LoggedTunableNumber; + import org.team1540.robot2024.util.MechanismVisualiser; /** @@ -168,7 +168,8 @@ public void teleopPeriodic() { */ @Override public void testInit() { - robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,new LedPatternTuneColor()); + robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternRainbow(10)); + robotContainer.drivetrain.setDefaultCommand(new KidModeSwerveDriveCommand(robotContainer.drivetrain, robotContainer.kidPilot)); // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index e949a208..4f6352f6 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,6 +1,5 @@ package org.team1540.robot2024; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.*; @@ -9,7 +8,6 @@ import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.climb.ClimbSequence; -import org.team1540.robot2024.commands.climb.ScoreInTrap; import org.team1540.robot2024.commands.climb.TrapAndClimbSequence; import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; import org.team1540.robot2024.commands.drivetrain.SwerveDriveCommand; @@ -25,7 +23,6 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; import org.team1540.robot2024.subsystems.shooter.*; import org.team1540.robot2024.subsystems.tramp.Tramp; @@ -52,6 +49,7 @@ public class RobotContainer { // Controller public final CommandXboxController driver = new CommandXboxController(0); public final CommandXboxController copilot = new CommandXboxController(1); + public final CommandXboxController kidPilot = new CommandXboxController(2); public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS); @@ -144,7 +142,6 @@ private void configureButtonBindings() { driver.a().whileTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); - copilot.rightBumper().whileTrue(new IntakeCommand(indexer, tramp::isNoteStaged, 1)); copilot.povDown().onTrue(indexer.commandRunIntake(-1)); copilot.povUp().whileTrue(new ClimbSequence(drivetrain, elevator, null)); @@ -162,6 +159,9 @@ private void configureButtonBindings() { // copilot.leftTrigger(0.5).whileTrue(new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB)); copilot.leftBumper().whileTrue(new TrampScoreSequence(tramp, indexer, elevator)); + kidPilot.leftBumper().or(kidPilot.leftTrigger()).whileTrue(new IntakeCommand(indexer, tramp::isNoteStaged, 1)); + kidPilot.rightBumper().or(kidPilot.rightTrigger()).whileTrue(new ShootSequence(shooter, indexer)); + new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd( () -> { elevator.setBrakeMode(false); @@ -177,8 +177,6 @@ private void configureButtonBindings() { } )); - - } private void configureAutoRoutines(){ diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/KidModeSwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/KidModeSwerveDriveCommand.java new file mode 100644 index 00000000..3b745017 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/KidModeSwerveDriveCommand.java @@ -0,0 +1,33 @@ +package org.team1540.robot2024.commands.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.util.LoggedTunableNumber; + +public class KidModeSwerveDriveCommand extends Command { + private final Drivetrain drivetrain; + private final CommandXboxController controller; + private double maxSpeedPercentage; + private double maxRotsPercentage; + + public KidModeSwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller){ + this.drivetrain = drivetrain; + this.controller = controller; + } + + public void execute() { + //TODO: check deadbands bc of diff controllers + double xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); + double yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); + double rotPercent = MathUtil.applyDeadband((-controller.getRightX()), 0.1); + + LoggedTunableNumber kidModeSpeedPercent = new LoggedTunableNumber("kidMode/maxSpeedPercentage", 0.3); + LoggedTunableNumber kidModeRotsPercent = new LoggedTunableNumber("kidMode/maxRotsPercentage", 0.5); + + drivetrain.drivePercent(xPercent * kidModeSpeedPercent.get(), yPercent * kidModeSpeedPercent.get(), rotPercent * kidModeRotsPercent.get(), true); + } + + +}