Merge pull request #15 from grt192/notealign

Notealign
grt192 · Feb 20, 2024 · 3413b86 · 3413b86
2 parents 63922a0 + 7e08bbe
commit 3413b86
Show file tree

Hide file tree

Showing 16 changed files with 310 additions and 57 deletions.
diff --git a/src/main/deploy/weezer.chrp b/src/main/deploy/weezer.chrp
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -79,6 +79,10 @@ public static class SwerveConstants { //ADD 90 degrees to all
     public static final Translation2d FR_POS = new Translation2d(MODULE_DIST, -MODULE_DIST);
     public static final Translation2d BL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST);
     public static final Translation2d BR_POS = new Translation2d(-MODULE_DIST, -MODULE_DIST);
+
+    public static final Translation2d BLUE_SPEAKER_POS = new Translation2d(Units.inchesToMeters(-1.50 + 17.5), Units.inchesToMeters(218.42));
+    public static final Translation2d RED_SPEAKER_POS = new Translation2d(Units.inchesToMeters(652.73 - 17.5), Units.inchesToMeters(218.42));
+    public static final double SPEAKER_TO_SPEAKER = Units.inchesToMeters(651.23);
   }
 
   public static class IntakeConstants {
@@ -162,12 +166,15 @@ public static class LEDConstants {
   }
 
   public static final class VisionConstants {
-    public static final String POSE_TABLE_KEY = "PoseEstimates";
+    public static final String VISION_TABLE_KEY = "Vision";
 
     public static final PhotonCamera FRONT_CAMERA = new PhotonCamera("Arducam_OV9281_USB_Camera_2");
     public static final Transform3d FRONT_CAMERA_POSE = new Transform3d(
       new Translation3d(Units.inchesToMeters(+8), Units.inchesToMeters(4), Units.inchesToMeters(+44)),
       new Rotation3d(Math.PI, Units.degreesToRadians(16), 0)
     );
+
+    public static final PhotonCamera NOTE_CAMERA = new PhotonCamera("Microsoft_LifeCam_HD-3000");
+    public static final Transform3d NOTE_CAMERA_POSE = new Transform3d();
   }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -24,12 +24,14 @@
 import frc.robot.commands.intake.roller.IntakeRollerFeedCommand;
 import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand;
 import frc.robot.commands.intake.roller.IntakeRollerOutakeCommand;
+import frc.robot.commands.sequences.AutoIntakeSequence;
 import frc.robot.commands.sequences.ShootModeSequence;
 import frc.robot.commands.shooter.feed.ShooterFeedLoadCommand;
 import frc.robot.commands.shooter.feed.ShooterFeedShootCommand;
 import frc.robot.commands.shooter.pivot.ShooterPivotSetAngleCommand;
 import frc.robot.commands.shooter.pivot.ShooterPivotVerticalCommand;
 import frc.robot.commands.swerve.AlignCommand;
+import frc.robot.commands.swerve.NoteAlignCommand;
 import frc.robot.commands.swerve.SwerveStopCommand;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
@@ -38,9 +40,13 @@
 import frc.robot.controllers.XboxDriveController;
 import frc.robot.subsystems.swerve.BaseSwerveSubsystem;
 import frc.robot.subsystems.swerve.SingleModuleSwerveSubsystem;
+import frc.robot.subsystems.swerve.SwerveModule;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.subsystems.swerve.TestSingleModuleSwerveSubsystem;
 import frc.robot.util.ConditionalWaitCommand;
+import frc.robot.vision.NoteDetectionWrapper;
+
+import static frc.robot.Constants.VisionConstants.NOTE_CAMERA;
 
 import java.sql.Driver;
 import java.util.function.BooleanSupplier;
@@ -52,13 +58,15 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
@@ -85,6 +93,8 @@ public class RobotContainer {
     private final ElevatorSubsystem elevatorSubsystem;
 
     private final LEDSubsystem ledSubsystem = new LEDSubsystem();
+
+    private final NoteDetectionWrapper noteDetector;  
 
 
     // Configure the trigger bindings
@@ -110,58 +120,59 @@ public class RobotContainer {
     private PIDController xPID;
     private PIDController yPID;
 
-    private final JoystickButton
-      LBumper = new JoystickButton(mechController, XboxController.Button.kLeftBumper.value),
-      RBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value),
-      AButton = new JoystickButton(mechController, XboxController.Button.kA.value);
 
     private final GenericEntry xError, yError;
 
     private final ShuffleboardTab swerveCrauton;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
-      //construct Test
-      // module = new SwerveModule(6, 7, 0);
-      // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
-      baseSwerveSubsystem = new SwerveSubsystem();
-      intakePivotSubsystem = new IntakePivotSubsystem();
-      shooterFeederSubsystem = new ShooterFeederSubsystem();
+        //construct Test
+        // module = new SwerveModule(6, 7, 0);
+        // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
+        baseSwerveSubsystem = new SwerveSubsystem();
+        intakePivotSubsystem = new IntakePivotSubsystem();
+        shooterFeederSubsystem = new ShooterFeederSubsystem();
 
-      shooterPivotSubsystem = new ShooterPivotSubsystem(false);
-      shooterFlywheelSubsystem = new ShooterFlywheelSubsystem();
+        shooterPivotSubsystem = new ShooterPivotSubsystem(false);
+        shooterFlywheelSubsystem = new ShooterFlywheelSubsystem();
 
-      climbSubsystem = new ClimbSubsystem();
-
-      elevatorSubsystem = new ElevatorSubsystem();
+        climbSubsystem = new ClimbSubsystem();
+        
+        elevatorSubsystem = new ElevatorSubsystem();
 
-      xPID = new PIDController(4, 0, 0);
-      yPID = new PIDController(4, 0, 0);
+        xPID = new PIDController(4, 0, 0);
+        yPID = new PIDController(4, 0, 0);
 
-      traj = Choreo.getTrajectory("2mLine");
+        traj = Choreo.getTrajectory("2mLine");
 
-      swerveCrauton = Shuffleboard.getTab("Auton");
+        swerveCrauton = Shuffleboard.getTab("Auton");
 
-    xError = swerveCrauton.add("Xerror", 0).withPosition(8, 0).getEntry();
-    yError = swerveCrauton.add("Yerror", 0).withPosition(9, 0).getEntry();
-    if(DriverStation.getJoystickName(0).equals("Cyborg V.1")){
+        xError = swerveCrauton.add("Xerror", 0).withPosition(8, 0).getEntry();
+        yError = swerveCrauton.add("Yerror", 0).withPosition(9, 0).getEntry();
+        if(DriverStation.getJoystickName(0).equals("Cyborg V.1")){
             driveController = new DualJoystickDriveController();
         } else {
             driveController = new XboxDriveController();
         }
-        // Configure the trigger bindings
-        configureBindings();
         // private final SwerveModule module;
         //construct Test
         // module = new SwerveModule(6, 7, 0);
         // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
 
+
+        noteDetector = new NoteDetectionWrapper(NOTE_CAMERA);
+
+
         camera1 = new UsbCamera("camera1", 0);
         camera1.setFPS(60);
         camera1.setBrightness(45);
         camera1.setResolution(176, 144);
         mjpgserver1 = new MjpegServer("m1", 1181);
         mjpgserver1.setSource(camera1);
+
+        // Configure the trigger bindings
+        configureBindings();
     }
 
 
@@ -199,14 +210,27 @@ private void configureBindings() {
                 ledSubsystem.setDriverHeading(driveController.getRelativeMode() ? 0 : -swerveSubsystem.getDriverHeading().getRadians());
             }, ledSubsystem));
 
-            driveController.getAmpAlign().onTrue(AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE, swerveSubsystem));
+            driveController.getAmpAlign().onTrue(new ParallelRaceGroup(
+                AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE, swerveSubsystem),
+                new ConditionalWaitCommand(() -> !driveController.getAmpAlign().getAsBoolean())
+            ));
+            driveController.getNoteAlign().onTrue(new ParallelRaceGroup(
+                new AutoIntakeSequence(elevatorSubsystem, intakeRollerSubsystem, swerveSubsystem, noteDetector)
+                .unless(() -> noteDetector.getNote().isEmpty()),
+                new ConditionalWaitCommand(() -> !driveController.getNoteAlign().getAsBoolean())
+            ));
             driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));
 
             swerveSubsystem.setDefaultCommand(new RunCommand(() -> {
                 if(driveController.getRelativeMode()){
                     swerveSubsystem.setRobotRelativeDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower());
                 } else {
+                  if(driveController.getTurnMode()){
+                    swerveSubsystem.setAimMode(driveController.getForwardPower(), driveController.getLeftPower());
+                  }
+                  else{
                     swerveSubsystem.setDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower());
+                  }
                 }
                 // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition());
                 xError.setValue(xPID.getPositionError());
@@ -246,10 +270,6 @@ private void configureBindings() {
           swerveSubsystem.setDrivePowers(driveController.getForwardPower(), driveController.getLeftPower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()));
         }
         , swerveSubsystem));
-
-          driveController.getFieldResetButton().onTrue(new InstantCommand(() -> {
-            swerveSubsystem.toggletoRun();
-          }));
 
         }
 

diff --git a/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java b/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java
@@ -5,7 +5,7 @@
 
 public class DriveForwardCommand extends Command{
     private final SwerveSubsystem swerve;
-    private int xpower;
+    private double xpower = .3;
 
 
     public DriveForwardCommand(SwerveSubsystem swerve){
@@ -14,7 +14,7 @@ public DriveForwardCommand(SwerveSubsystem swerve){
     }
 
     @Override
-    public void initialize() {
+    public void execute() {
         swerve.setRobotRelativeDrivePowers(xpower, 0, 0);
     }
 

diff --git a/src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java b/src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java
@@ -0,0 +1,30 @@
+package frc.robot.commands.sequences;
+
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.auton.DriveForwardCommand;
+import frc.robot.commands.elevator.ElevatorToGroundCommand;
+import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand;
+import frc.robot.commands.swerve.NoteAlignCommand;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.vision.NoteDetectionWrapper;
+
+public class AutoIntakeSequence extends SequentialCommandGroup {
+
+
+    public AutoIntakeSequence(
+        ElevatorSubsystem elevatorSubsystem, 
+        IntakeRollersSubsystem intakeRollersSubsystem,
+        SwerveSubsystem swerveSubsystem,
+        NoteDetectionWrapper noteDetector
+    ){
+        addCommands(new ElevatorToGroundCommand(elevatorSubsystem)
+        .andThen(new NoteAlignCommand(swerveSubsystem, noteDetector))
+        .andThen(new ParallelDeadlineGroup(
+            new IntakeRollerIntakeCommand(intakeRollersSubsystem).withTimeout(3),
+            new DriveForwardCommand(swerveSubsystem)))
+        );
+    }
+}
diff --git a/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java
@@ -0,0 +1,63 @@
+package frc.robot.commands.swerve;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.NoSuchElementException;
+
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.vision.NoteDetectionWrapper;
+
+public class NoteAlignCommand extends Command{
+    private final SwerveSubsystem swerveSubsystem;
+    private final NoteDetectionWrapper noteDetector;
+
+    private double noteYawOffsetDegrees;
+
+    public NoteAlignCommand(SwerveSubsystem swerveSubsystem, NoteDetectionWrapper noteDetector) {
+        this.swerveSubsystem = swerveSubsystem;
+        this.noteDetector = noteDetector;
+
+        this.addRequirements(swerveSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        System.out.println("Started NoteAlignCommand");
+
+        this.noteYawOffsetDegrees = 0;
+        try {
+            noteYawOffsetDegrees = noteDetector.getNote().get().getYaw();
+        } catch(NoSuchElementException e) {
+            System.out.println("Tried to align to a note, but none was detected.");
+            this.end(true);
+        }
+
+        System.out.println("Rotating to note at offset " + this.noteYawOffsetDegrees + " degrees");
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(noteYawOffsetDegrees) < 1;
+    }
+
+    @Override
+    public void execute() {
+        try {
+            noteYawOffsetDegrees = noteDetector.getNote().get().getYaw();
+        } catch(NoSuchElementException e) {
+
+        }
+
+        swerveSubsystem.setRobotRelativeDrivePowers(0.,0., -MathUtil.clamp(noteYawOffsetDegrees * .008, -.3, .3));
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerveSubsystem.setRobotRelativeDrivePowers(0, 0, 0);
+        System.out.println("Ended NoteAlignCommand");
+    }
+
+
+}
diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java
@@ -31,5 +31,9 @@ public abstract class BaseDriveController {
 
     public abstract JoystickButton getAmpAlign();
 
+    public abstract JoystickButton getNoteAlign();
+
     public abstract JoystickButton getSwerveStop();
+
+    public abstract Boolean getTurnMode();
 }
diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
@@ -21,8 +21,8 @@ public class DualJoystickDriveController extends BaseDriveController {
     private final Joystick rightJoystick = new Joystick(1);
     private final JoystickButton
         rightTrigger = new JoystickButton(rightJoystick, 1),
-        right2 = new JoystickButton(rightJoystick, 2),
-        rightTopLeftButton = new JoystickButton(rightJoystick, 3),
+        rightBottomButton = new JoystickButton(rightJoystick, 2),
+        rightTopButton = new JoystickButton(rightJoystick, 3),
         rightTopRightButton = new JoystickButton(rightJoystick, 4),
         rightMiddleLeftButton = new JoystickButton(rightJoystick, 5),
         rightMiddleRightButton = new JoystickButton(rightJoystick, 6),
@@ -66,7 +66,7 @@ private double getTurnScaling() {
     }
 
     public JoystickButton getFieldResetButton() {
-        return right2;
+        return rightTopButton;
     }
 
     public JoystickButton getRightBumper(){
@@ -82,10 +82,18 @@ public Boolean getRelativeMode(){
     }
 
     public JoystickButton getAmpAlign(){
-        return leftTrigger;
+        return leftTopLeftButton;
     }
 
-    public JoystickButton getSwerveStop(){
+    public JoystickButton getNoteAlign() {
         return leftMiddleButton;
     }
+
+    public JoystickButton getSwerveStop(){
+        return leftTrigger;
+    }
+    public Boolean getTurnMode(){
+        return leftTopRightButton.getAsBoolean();
+    }
+
 }
diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java
@@ -56,7 +56,15 @@ public JoystickButton getAmpAlign() {
         return driveXButton;
     }
 
+    public JoystickButton getNoteAlign() {
+        return driveYButton;
+    }
+
     public JoystickButton getSwerveStop() {
         return driveBButton;
     }
+
+    public Boolean getTurnMode(){
+        return null;
+    }
 }