Merge pull request #24 from Pearadox/nazeerbranch5

Nazeerbranch5

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 java {

src/main/java/frc/robot/Constants.java

@@ -61,7 +61,7 @@ public static class ClimberConstants {
   public static class RollerClawConstants {
     public static final int rollerClawID = 7;
     public static final int rollerClawLimit = 45; // CIM, brushed
-
+    
     public static final double clawSpeed = 0.5;
     public static final double clawStallSpeed = 0.1;
     public static final double clawShootSpeed = -0.5;

src/main/java/frc/robot/RobotContainer.java

@@ -4,10 +4,13 @@
 
 package frc.robot;
 
-import frc.robot.Constants.LauncherConstants;
+// import frc.robot.Constants.LauncherConstants;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.*;
 import frc.robot.subsystems.*;
+// import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -25,21 +28,30 @@ public class RobotContainer {
   private final Drivetrain drivetrain = new Drivetrain();
   private final Launcher launcher = new Launcher();
   private final Climber climber = new Climber();
-  // private final RollerClaw rollerClaw = new RollerClaw();
+  private final RollerClaw rollerClaw = new RollerClaw();
+
+  private final SendableChooser<Command> chooser = new SendableChooser<Command>();
+  private final SendableChooser<Boolean> controllerChoose = new SendableChooser<Boolean>();
 
   private final CommandXboxController driverController =
       new CommandXboxController(OperatorConstants.kDriverControllerPort);
   private final CommandXboxController operatorController =
-      new CommandXboxController(OperatorConstants.kOperatorControllerPort);
+      new CommandXboxController(OperatorConstants.kOperatorControllerPort);  
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
-  public RobotContainer() {
-    // Configure the trigger bindings
-    configureBindings();
-
-    drivetrain.setDefaultCommand(
-      new Drive(drivetrain, driverController)
-    );
+  public RobotContainer() {  
+    drivetrain.setDefaultCommand(new Drive(drivetrain, driverController));
+
+    SmartDashboard.putData("Auton", chooser);
+    chooser.setDefaultOption("AutoSpin", new AutoSpin(drivetrain, 2));
+    chooser.addOption("Auto Cross And Spin", new AutoCrossAndSpin(drivetrain, launcher));
+    chooser.addOption("Launch Group", new LaunchGroup(launcher));    
+
+    SmartDashboard.putData("Number of Controllers", controllerChoose);
+    controllerChoose.setDefaultOption("Driver + Operator", true);
+    controllerChoose.addOption("Driver Only", false);
+
+    configureBindings();    
   }
 
   /**
@@ -51,25 +63,45 @@ public RobotContainer() {
    * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
    * joysticks}.
    */
-  private void configureBindings() {
-    // when operator holds roight bumper, run PrepareLaunch for 1 sec, then run LaunchNote
-    operatorController.rightBumper().whileTrue(new PrepearLaunch(launcher)
-      .withTimeout(LauncherConstants.launcherDelay)
-      .andThen(new LaunchNote(launcher))
-      .handleInterrupt(() -> launcher.stop()));
+  public void configureBindings() {
+    // DONE: bind all the operator buttons also on the driver controller (no need for conditionals, just duplicate all the bindings)
 
-    // intakes when operator holds left bumper
-    operatorController.leftBumper().whileTrue(new Intake(launcher));
-
-    // climbs up when operator holds dpad up
-    operatorController.povUp().whileTrue(new ClimbUp(climber));
-    // climbs down when operator holds dpad down
-    operatorController.povDown().whileTrue(new ClimbDown(climber));
-
-    // // intakes with roller when x button is pressed
-    // operatorController.x().whileTrue(new RollerIntake(rollerClaw));
-    // // shoots with roller when b button is pressed
-    // operatorController.b().whileTrue(new RollerLaunch(rollerClaw));
+    if (controllerChoose.getSelected()) {
+      // when operator holds roight bumper, run PrepareLaunch for 1 sec, then run LaunchNote
+      operatorController.rightBumper().whileTrue(new LaunchGroup(launcher));
+
+      // intakes when operator holds left bumper
+      operatorController.leftBumper().whileTrue(new Intake(launcher));
+
+      // climbs up when operator holds dpad up
+      operatorController.povUp().whileTrue(new ClimbUp(climber));
+      // climbs down when operator holds dpad down
+      operatorController.povDown().whileTrue(new ClimbDown(climber));
+
+      // intakes with roller when x button is pressed
+      operatorController.x().whileTrue(new RollerIntake(rollerClaw));
+      // shoots with roller when b button is pressed
+      operatorController.b().whileTrue(new RollerLaunch(rollerClaw));
+
+    } else { 
+      // must restart robot for changes to occur
+
+      // when driver holds roight bumper, run PrepareLaunch for 1 sec, then run LaunchNote
+      driverController.rightBumper().whileTrue(new LaunchGroup(launcher));
+
+      // intakes when driver holds left bumper
+      driverController.leftBumper().whileTrue(new Intake(launcher));
+
+      // climbs up when driver holds dpad up
+      driverController.povUp().whileTrue(new ClimbUp(climber));
+      // climbs down when driver holds dpad down
+      driverController.povDown().whileTrue(new ClimbDown(climber));
+
+      // intakes with roller when x button is pressed
+      driverController.x().whileTrue(new RollerIntake(rollerClaw));
+      // shoots with roller when b button is pressed
+      driverController.b().whileTrue(new RollerLaunch(rollerClaw));
+    }
 
   }
 
@@ -80,6 +112,6 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return new AutoCrossAndSpin(drivetrain);
+    return chooser.getSelected();
   }
 }

src/main/java/frc/robot/commands/AutoCrossAndSpin.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Launcher;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -16,12 +17,13 @@ public class AutoCrossAndSpin extends SequentialCommandGroup {
   // Drivetrain drivetrain;
 
   /** Creates a new AutoCrossAndSpin. */
-  public AutoCrossAndSpin(Drivetrain drivetrain) {
+  public AutoCrossAndSpin(Drivetrain drivetrain, Launcher launcher) {
 
     // this.drivetrain = drivetrain; 
 
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    addCommands(new AutoCrossTheLine(drivetrain), new WaitCommand(2), new AutoSpin(drivetrain));
+    addCommands(new AutoCrossTheLine(drivetrain, 3), new WaitCommand(1), 
+      new AutoSpin(drivetrain, 2), new WaitCommand(2), new LaunchGroup(launcher));
   }
 }

src/main/java/frc/robot/commands/AutoCrossTheLine.java

@@ -4,6 +4,7 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Drivetrain;
 // import edu.wpi.first.wpilibj.Timer;
@@ -14,26 +15,32 @@ public class AutoCrossTheLine extends Command {
 
   // initializes drivetrain object
   private Drivetrain drivetrain;
+  private long startTime;
+  private double stopTime;
 
   // intiializes timeSinceStart 
 
   /** Creates a new CrossTheLine. */
-  public AutoCrossTheLine(Drivetrain drivetrain) {
+  public AutoCrossTheLine(Drivetrain drivetrain, double stopTime) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.drivetrain = drivetrain;
     addRequirements(drivetrain);
+    this.stopTime = stopTime * 1000;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     // timeSinceStart.restart();
+    drivetrain.resetEncoders();
+    startTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    drivetrain.arcadeDrive(-0.6, 0.0);    
+    drivetrain.arcadeDrive(0.4, 0.0);    
+    SmartDashboard.putNumber("Encoder", drivetrain.getDistance());
   }
 
   // Called once the command ends or is interrupted.
@@ -45,6 +52,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {      
-    return (drivetrain.getDistance() >= 3);
+    return (System.currentTimeMillis() - startTime >= stopTime); //change 
   }
 }

src/main/java/frc/robot/commands/AutoSpin.java

@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Drivetrain;
-import edu.wpi.first.wpilibj.Timer;
+// import edu.wpi.first.wpilibj.Timer;
 
 public class AutoSpin extends Command {
 
@@ -15,12 +15,15 @@ public class AutoSpin extends Command {
 
   // intiializes timeSinceStart 
   private long startTime;
+  private double stopTime;
 
   /** Creates a new CrossTheLine. */
-  public AutoSpin(Drivetrain drivetrain) {
+  public AutoSpin(Drivetrain drivetrain, double stopTime) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.drivetrain = drivetrain;
     addRequirements(drivetrain);
+
+    this.stopTime = stopTime * 1000;
   }
 
   // Called when the command is initially scheduled.
@@ -32,7 +35,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    drivetrain.arcadeDrive(0.0, 0.6);
+    drivetrain.arcadeDrive(0.0, -0.4);
   }
 
   // Called once the command ends or is interrupted.
@@ -44,6 +47,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (System.currentTimeMillis() - startTime >= 2000);
+    return (System.currentTimeMillis() - startTime >= stopTime);
   }
 }

src/main/java/frc/robot/commands/LaunchGroup.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants.LauncherConstants;
+import frc.robot.subsystems.Launcher;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class LaunchGroup extends SequentialCommandGroup {
+  /** Creates a new LaunchGroup. */
+  public LaunchGroup(Launcher launcher) {
+    // Add your commands in the addCommands() call, e.g.
+
+    // TODO: Have you tested this fully in teleop?  This was structured in a way that it fit in robotContainer,
+    //       but it doesn't quite translate to a command group like this.
+
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(new PrepearLaunch(launcher)
+    .withTimeout(LauncherConstants.launcherDelay)
+    .andThen(new LaunchNote(launcher))
+    .handleInterrupt(() -> launcher.stop())
+    .withTimeout(2)); // change? to do: make constant
+  }
+}

src/main/java/frc/robot/commands/RollerIntake.java

@@ -2,41 +2,41 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-// package frc.robot.commands;
-
-// import edu.wpi.first.wpilibj2.command.Command;
-// import frc.robot.Constants.RollerClawConstants;
-// import frc.robot.subsystems.RollerClaw;
-
-
-// public class RollerIntake extends Command {
-//   private RollerClaw rollerClaw;
-//   /** Creates a new RollerIntake. */
-//   public RollerIntake(RollerClaw rollerClaw) {
-//     this.rollerClaw = rollerClaw;
-//     // Use addRequirements() here to declare subsystem dependencies.
-//     addRequirements(rollerClaw);
-//   }
-
-//   // Called when the command is initially scheduled.
-//   @Override
-//   public void initialize() {
-//     rollerClaw.setRollerClaw(RollerClawConstants.clawSpeed);
-//   }
-
-//   // Called every time the scheduler runs while the command is scheduled.
-//   @Override
-//   public void execute() {}
-
-//   // Called once the command ends or is interrupted.
-//   @Override
-//   public void end(boolean interrupted) {
-//     rollerClaw.stop();
-//   }
-
-//   // Returns true when the command should end.
-//   @Override
-//   public boolean isFinished() {
-//     return false;
-//   }
-// }
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.RollerClawConstants;
+import frc.robot.subsystems.RollerClaw;
+
+
+public class RollerIntake extends Command {
+  private RollerClaw rollerClaw;
+  /** Creates a new RollerIntake. */
+  public RollerIntake(RollerClaw rollerClaw) {
+    this.rollerClaw = rollerClaw;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(rollerClaw);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    rollerClaw.setRollerClaw(RollerClawConstants.clawSpeed);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    rollerClaw.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}