feat: Added named commands. should work

SaintsRobotics · Feb 27, 2024 · a2aaa24 · a2aaa24
2 parents fd73ad4 + fce6055
commit a2aaa24
Show file tree

Hide file tree

Showing 6 changed files with 141 additions and 49 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -100,9 +100,11 @@ public static final class IntakeConstants {
     public static final int kArmMotorID = 39;
     public static final int kArmEncoderChannel = 0;
 
+    // In degrees
     // In degrees
     public static final double kIntakeLoweredAngle = 0;
     public static final double kIntakeRaisedAngle = 194;
+    public static final double kIntakeAmpScoringAngle = 100;
 
     /** Encoder offset in rotations */
     public static final double kArmEncoderOffset = 0.6692;
@@ -126,8 +128,6 @@ public static class ClimberConstants {
 
         public final static double minPressure = 50.0;
         public final static double maxPressure = 120.0;
-
-
     }
   public static final class VisionConstants {
     // TODO: Update cam pose relative to center of bot

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,6 +13,8 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
@@ -22,14 +24,19 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
+import frc.robot.commands.IntakeArmPositionCommand;
+import frc.robot.commands.NoteIntakeCommand;
+import frc.robot.commands.NoteOuttakeCommand;
 import frc.robot.subsystems.ClimberSubsystem;
-import frc.robot.commands.IntakeCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
 
@@ -45,24 +52,26 @@ public class RobotContainer {
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
-
+private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
   private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
   private final SendableChooser<Command> autoChooser;
 
+
   /**
    * The container for the robot. Contains subsystems, IO devices, and commands.
    */
   public RobotContainer() {
     // Registering Named Commands for autonpaths
     // registerCommand is looking for XXXXXXCommand, so things may need to be
     // renamed.
-    NamedCommands.registerCommand("shoot", new InstantCommand(() -> m_shooterSubsystem
-        .shooterTimedRun(Constants.ShooterConstants.kTimeShoot, Constants.ShooterConstants.kShooterMotorSpeed)));
+    NamedCommands.registerCommand("shoot", new SequentialCommandGroup(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem), new WaitCommand(0.5), new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)));
 
-    NamedCommands.registerCommand("intake", new InstantCommand(
-        () -> m_intakeSubsystem.intakeTimedRun(Constants.IntakeConstants.kTimeIntake), m_intakeSubsystem));
+    NamedCommands.registerCommand("intake", new SequentialCommandGroup(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
+                                new NoteIntakeCommand(m_intakeSubsystem), 
+                                new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)));
 
     // All paths automatically
 
@@ -142,9 +151,22 @@ private void configureBindings() {
     new JoystickButton(m_operatorController, Button.kB.value)
         .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive));
 
+    new Trigger(() -> {
+      return m_driverController.getLeftTriggerAxis() > 0.5;
+    }).whileTrue(
+        new SequentialCommandGroup(
+            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
+            new NoteIntakeCommand(m_intakeSubsystem),
+            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)))
+        .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
+
     new Trigger(() -> {
       return m_driverController.getRightTriggerAxis() > 0.5;
-    }).whileTrue(new IntakeCommand(m_intakeSubsystem));
+    }).whileTrue(
+        new SequentialCommandGroup(
+            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp),
+            new NoteOuttakeCommand(m_intakeSubsystem)))
+        .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
   }
 
   // adding auton Path options

diff --git a/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java b/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java
@@ -0,0 +1,34 @@
+// 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.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
+
+public class IntakeArmPositionCommand extends Command {
+  private IntakeSubsystem m_intakeSubsystem;
+
+  private ArmPosition m_armPosition;
+
+  /** Creates a new intakeCommand. */
+  public IntakeArmPositionCommand(IntakeSubsystem subsystem, ArmPosition position) {
+    m_intakeSubsystem = subsystem;
+    m_armPosition = position;
+    addRequirements(m_intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_intakeSubsystem.setArmPosition(m_armPosition);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_intakeSubsystem.armAtSetpoint();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/NoteIntakeCommand.java b/src/main/java/frc/robot/commands/NoteIntakeCommand.java
@@ -0,0 +1,44 @@
+// 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.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class NoteIntakeCommand extends Command {
+  private IntakeSubsystem m_intakeSubsystem;
+
+  /** Creates a new intakeCommand. */
+  public NoteIntakeCommand(IntakeSubsystem subsystem) {
+    m_intakeSubsystem = subsystem;
+    addRequirements(m_intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_intakeSubsystem.intake();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_intakeSubsystem.haveNote()) {
+      m_intakeSubsystem.stopIntake();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_intakeSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_intakeSubsystem.haveNote();
+  }
+}
diff --git a/...ava/frc/robot/commands/IntakeCommand.java → ...rc/robot/commands/NoteOuttakeCommand.java b/...ava/frc/robot/commands/IntakeCommand.java → ...rc/robot/commands/NoteOuttakeCommand.java
@@ -7,20 +7,19 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;
 
-public class IntakeCommand extends Command {
+public class NoteOuttakeCommand extends Command {
   private IntakeSubsystem m_intakeSubsystem;
 
   /** Creates a new intakeCommand. */
-  public IntakeCommand(IntakeSubsystem subsystem) {
+  public NoteOuttakeCommand(IntakeSubsystem subsystem) {
     m_intakeSubsystem = subsystem;
     addRequirements(m_intakeSubsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_intakeSubsystem.armExtend();
-    m_intakeSubsystem.intake();
+    m_intakeSubsystem.outtake();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -32,10 +31,10 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_intakeSubsystem.armRetract();
     m_intakeSubsystem.stopIntake();
   }
 
+  // Returns true when the command should end.
   @Override
   public boolean isFinished() {
     return false;

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -22,7 +22,6 @@
 import frc.robot.Constants.IntakeConstants;
 
 public class IntakeSubsystem extends SubsystemBase {
-  private boolean deployed = false;
   private boolean haveNote = false;
 
   private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
@@ -35,7 +34,7 @@ public class IntakeSubsystem extends SubsystemBase {
   private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
 
   private double m_intakeSpeed = 0;
-  private double m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
+  private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
 
   /** Creates a new IntakeSubsystem */
   public IntakeSubsystem() {
@@ -50,37 +49,33 @@ public IntakeSubsystem() {
 
     // m_armPID.disableContinuousInput();
     m_armPID.setTolerance(0.05);
-
-    // TODO: See if this is needed
-    // m_distanceSensor.setAutomaticMode(true);
   }
 
-  public void armExtend() {
-    m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
-
-    stopIntake();
-
-    deployed = true;
+  public void setArmPosition(ArmPosition position) {
+    switch (position) {
+      case Amp:
+        m_armSetpoint = IntakeConstants.kIntakeAmpScoringAngle;
+        break;
+      case Extended:
+        m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
+        break;
+      case Retracted:
+        m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
+      default:
+        break;
+    }
   }
 
-  public void armRetract() {
-    m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
-
-    stopIntake();
-
-    deployed = false;
+  public boolean armAtSetpoint() {
+    return m_armPID.atSetpoint();
   }
 
   public void intake() {
-    if (deployed && !haveNote) {
-      m_intakeSpeed = IntakeConstants.kIntakeSpeed;
-    }
+    m_intakeSpeed = IntakeConstants.kIntakeSpeed;
   }
 
-  public void outake() {
-    if (!deployed && haveNote) {
-      m_intakeSpeed = -IntakeConstants.kIntakeSpeed;
-    }
+  public void outtake() {
+    m_intakeSpeed = -IntakeConstants.kIntakeSpeed;
   }
 
   public void stopIntake() {
@@ -100,7 +95,6 @@ public void intakeTimedRun(double time){
   }
   /**
    * Gets distance from Rev 2m sensor
-   * 
    */
   public double getDistanceSensor() {
     return m_distanceSensor.getRange();
@@ -110,21 +104,20 @@ public double getDistanceSensor() {
   public void periodic() {
     haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold;
 
-    // If we have a note and the arm is deployed, automatically bring it back in
-    if (haveNote && deployed) {
-      stopIntake();
-      armRetract();
-    }
-
     // m_armMotor.set(MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3));
-    // m_intakeMotor.set(m_intakeSpeed);
+    // m_intakeMotor.set((m_intakeSpeed >= 0 && haveNote) ? 0 : m_intakeSpeed);
 
     SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
-    SmartDashboard.putBoolean("Arm Deployed?", deployed);
     SmartDashboard.putBoolean("Have Note?", haveNote);
   }
 
-  public boolean readyToShoot() {
-    return haveNote && !deployed && m_armPID.atSetpoint();
+  public boolean haveNote() {
+    return haveNote;
+  }
+
+  public static enum ArmPosition {
+    Extended,
+    Retracted,
+    Amp
   }
 }