feat: merged main into branch :)

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();
     }

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(){

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);
+    }
+
+
+}