From d29342a90f9342ee2fefcb4c77ae6b7612978011 Mon Sep 17 00:00:00 2001 From: Timothy Bevis Date: Tue, 18 Feb 2025 18:49:36 -0500 Subject: [PATCH] Small vision additions --- src/main/java/frc/robot/Robot.java | 5 ++++- src/main/java/frc/robot/maps/ColdStart.java | 10 ++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 46c52d3..b6eef40 100644 --- 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 index 039257f..663da38 100644 --- 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);