Small vision additions

chopshop-166 · Feb 18, 2025 · d29342a · d29342a
1 parent 0815732
commit d29342a
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Vision.Branch;
 import frc.robot.maps.RobotMap;
 import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
 import frc.robot.subsystems.AlgaeDestage;
@@ -133,6 +134,9 @@ public void configureButtonBindings() {
         driveController.back().onTrue(commandSequences.resetAll());
         driveController.leftBumper()
                 .whileTrue(drive.robotCentricDrive());
+        driveController.rightBumper().whileTrue(drive.aimAtReefCenter());
+        driveController.rightTrigger().whileTrue(drive.alignToReefBranch(Branch.RIGHT_BRANCH));
+        driveController.leftTrigger().whileTrue(drive.alignToReefBranch(Branch.LEFT_BRANCH));
 
         copilotController.a().onTrue(commandSequences.intake());
 
@@ -149,7 +153,6 @@ public void configureButtonBindings() {
         copilotController.back().onTrue(elevator.resetCmd());
         copilotController.start().onTrue(elevator.zero());
 
-        driveController.rightBumper().whileTrue(drive.aimAtReefCenter());
         copilotController.getPovButton(POVDirection.RIGHT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL2))
                 .onFalse(elevator.safeStateCmd());
         copilotController.getPovButton(POVDirection.UP).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL3))

diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -7,8 +7,10 @@
 import com.chopshop166.chopshoplib.digital.CSDigitalInput;
 import com.chopshop166.chopshoplib.drive.SDSSwerveModule;
 import com.chopshop166.chopshoplib.drive.SDSSwerveModule.Configuration;
+import com.chopshop166.chopshoplib.maps.CameraSource;
 import com.chopshop166.chopshoplib.maps.RobotMapFor;
 import com.chopshop166.chopshoplib.maps.SwerveDriveMap;
+import com.chopshop166.chopshoplib.maps.VisionMap;
 import com.chopshop166.chopshoplib.motors.CSSparkFlex;
 import com.chopshop166.chopshoplib.motors.CSSparkMax;
 import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro2;
@@ -25,6 +27,7 @@
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
@@ -111,6 +114,13 @@ public SwerveDriveMap getDriveMap() {
                 maxRotationRadianPerSecond, pigeonGyro2, config, holonomicDrive);
     }
 
+    public VisionMap getVisionMap() {
+
+        return new VisionMap(
+                new CameraSource("FLCamera", new Transform3d()),
+                new CameraSource("FRCamera", new Transform3d()));
+    }
+
     @Override
     public ElevatorMap getElevatorMap() {
         CSSparkFlex leftMotor = new CSSparkFlex(11);