diff --git a/src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto b/src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto new file mode 100644 index 0000000..3d32169 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Deploy_Intake_Command" + } + }, + { + "type": "path", + "data": { + "pathName": "Starting to HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "path", + "data": { + "pathName": "HP to Hub" + } + }, + { + "type": "named", + "data": { + "name": "Auto_Aim_To_Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot_Fuel_Command" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "Kill_All" + } + }, + { + "type": "named", + "data": { + "name": "PathPlan_To_Climb_Right_Offsetted" + } + }, + { + "type": "named", + "data": { + "name": "PathPlan_Into_Climb_Right" + } + }, + { + "type": "named", + "data": { + "name": "Lift_Robot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto b/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto index 83f6076..1db36d4 100644 --- a/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto +++ b/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto @@ -34,6 +34,12 @@ "name": "Shoot_Fuel_Command" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "named", "data": { @@ -43,7 +49,7 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 2.0 } }, { @@ -64,12 +70,6 @@ "pathName": "Side into Climb" } }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/HP to Hub.path b/src/main/deploy/pathplanner/paths/HP to Hub.path index ffc5e11..70c66d8 100644 --- a/src/main/deploy/pathplanner/paths/HP to Hub.path +++ b/src/main/deploy/pathplanner/paths/HP to Hub.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.5, - "y": 0.65 + "x": 0.447, + "y": 0.634 }, "prevControl": null, "nextControl": { - "x": 0.8535533905932737, - "y": 1.0035533905932739 + "x": 0.8005533905932738, + "y": 0.9875533905932738 }, "isLocked": false, "linkedName": null @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 38.99099404250546 + "rotation": 27.207 }, "reversed": false, "folder": "Start to HP to Hub to Climb", diff --git a/src/main/deploy/pathplanner/paths/Hub to side of Climb.path b/src/main/deploy/pathplanner/paths/Hub to side of Climb.path index 1cc25eb..ff4a8e6 100644 --- a/src/main/deploy/pathplanner/paths/Hub to side of Climb.path +++ b/src/main/deploy/pathplanner/paths/Hub to side of Climb.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.08, + "x": 1.25, "y": 2.0457840909090907 }, "prevControl": { - "x": 1.5498463103929543, + "x": 1.7198463103929542, "y": 2.2167941625719254 }, "nextControl": null, @@ -48,7 +48,7 @@ "folder": "Start to HP to Hub to Climb", "idealStartingState": { "velocity": 0, - "rotation": 22.013232308372665 + "rotation": 27.207 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Side into Climb.path b/src/main/deploy/pathplanner/paths/Side into Climb.path index 75e2d29..fb59d7b 100644 --- a/src/main/deploy/pathplanner/paths/Side into Climb.path +++ b/src/main/deploy/pathplanner/paths/Side into Climb.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 1.08, + "x": 1.25, "y": 2.046 }, "prevControl": null, "nextControl": { - "x": 1.0861477272727271, - "y": 2.5817386363636365 + "x": 1.302590909090909, + "y": 2.6126590909090908 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.25, + "x": 1.37, "y": 3.5 }, "prevControl": { - "x": 1.256147727272727, + "x": 1.3761477272727272, "y": 3.190795454545455 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Starting to HP.path b/src/main/deploy/pathplanner/paths/Starting to HP.path index ea68986..123cd2c 100644 --- a/src/main/deploy/pathplanner/paths/Starting to HP.path +++ b/src/main/deploy/pathplanner/paths/Starting to HP.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 0.4471249999999998, + "x": 0.457431818181818, "y": 0.63375 }, "prevControl": { - "x": 0.999027242432462, + "x": 1.0093340606142802, "y": 0.6337499999999999 }, "nextControl": null, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8213d76..ed69bb0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,8 @@ package frc.robot; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; +import com.pathplanner.lib.path.PathConstraints; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; @@ -98,7 +100,7 @@ public final class Constants { * .withWidget(BuiltInWidgets.kNumberBar).getEntry(); */ - public static double INDEXER_AND_RAMP_MOTOR_RPM = 20000; + public static double INDEXER_AND_RAMP_MOTOR_RPM = 30000; // this method called in robot periodic so values updated in elastic are // constantly read and applied to RAMP_MOTOR_SPEED @@ -128,16 +130,16 @@ public final class Constants { public static final int INTAKE_ROTATOR_MOTOR_ID = 51; public static class IntakeRotatorPID { - public static final double INTAKE_ROTATOR_P = .1; - public static final double INTAKE_ROTATOR_I = 0.0; - public static final double INTAKE_ROTATOR_D = 0.; + public static final double INTAKE_ROTATOR_P = .08; + public static final double INTAKE_ROTATOR_I = 0; + public static final double INTAKE_ROTATOR_D = 0.01; } public static final double INTAKE_MOTOR_P = 0.0001; public static final double INTAKE_MOTOR_I = 0; public static final double INTAKE_MOTOR_D = 0; - public static final double INTAKE_COLLECT_ENCODER_VALUE = -0.2; + public static final double INTAKE_COLLECT_ENCODER_VALUE = 1.4; public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3; public static final double INTAKE_RETRACT_ENCODER_VALUE = -5.5; @@ -175,6 +177,8 @@ public final class Constants { public static Rotation2d HUB_ROTATION_BLUE; public static Rotation2d HUB_ROTATION_RED; + public static Pose2d allianceHubPose = new Pose2d(); + public static final double HUB_X_POSE_BLUE = 4.625; public static final double HUB_Y_POSE_BLUE = 4.03; @@ -182,10 +186,12 @@ public final class Constants { public static final double HUB_X_POSE_RED = 11.915; public static final double HUB_Y_POSE_RED = 4.03; + public static PathConstraints DRIVE_INTO_CLIMB_CONSTRAINTS; + public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d( new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25), Units.inchesToMeters(13.1875)), - new Rotation3d(0, 0, Math.toRadians(205))); + new Rotation3d(0, 0, Math.toRadians(195))); public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d( new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)), diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9efa539..27c4874 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ package frc.robot; import java.util.logging.Logger; +import org.photonvision.PhotonUtils; import org.photonvision.targeting.PhotonPipelineResult; import com.pathplanner.lib.auto.AutoBuilder; @@ -107,7 +108,7 @@ public class Robot extends TimedRobot { TargetingSubsystems.updateShooterRPM(m_robotContainer.getSwerveDrive().getPose()); SmartDashboard.putNumber("Estimated Shooter RPM", Constants.ShooterConstants.SHOOTER_RPM); - + SmartDashboard.putNumber("Distance From Hub: ", PhotonUtils.getDistanceToPose(m_robotContainer.getSwerveDrive().getPose(), Constants.TargetingConstants.allianceHubPose)); //Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); //Constants.IntakeConstants.updateIntakeWheelsRPM(); //Constants.ShooterConstants.updateShooterRPM(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78de8c3..842e492 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,13 +143,19 @@ public class RobotContainer { // Create the NamedCommands that will be used in PathPlanner NamedCommands.registerCommand("test", Commands.print("I EXIST")); NamedCommands.registerCommand("Shoot_Fuel_Command", - m_ShooterSubsystem.shootFuelCommand() + m_ShooterSubsystem.setShooterMotorsRPMAutoCommand() .andThen(m_IntakeSubsystem.startIntakeMotorCommand())); NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand()); NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()); NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand()); NamedCommands.registerCommand("Assist_Shooter", m_IntakeSubsystem.assistFuelIntakeCommand()); NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand()); + NamedCommands.registerCommand("Kill_All", killAllCommand()); + NamedCommands.registerCommand("Auto_Aim_To_Hub", m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); + NamedCommands.registerCommand("PathPlan_To_Climb_Right_Offsetted", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED)); + NamedCommands.registerCommand("PathPlan_To_Climb_Left_Offsetted", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED)); + NamedCommands.registerCommand("PathPlan_Into_Climb_Right", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE)); + NamedCommands.registerCommand("PathPlan_Into_Climb_Left", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE)); // Have the autoChooser pull in all PathPlanner autos as options @@ -230,12 +236,12 @@ public class RobotContainer { operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand()) .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()); - topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -1)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0)); + topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -5)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0)); topButtons.axisGreaterThan(1, -0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0)); topButtons.button(3).onTrue(killAllCommand()); topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); - topButtons.button(1).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED)); - topButtons.button(2).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED)); + topButtons.button(1).onTrue(drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED)); + topButtons.button(2).onTrue(drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED)); //topButtons.button(1).onTrue(drivebase.driveToPose(Constants.)) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 60b89ec..5810914 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -121,6 +121,18 @@ public class ShooterSubsystem extends SubsystemBase { rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); } + public void setShooterMotorsRPMAuto(){ + centerShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity); + leftShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity); + rightShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity); + } + + public Command setShooterMotorsRPMAutoCommand() + { + return runOnce(()-> setShooterMotorsRPMAuto()).andThen(new WaitCommand(1.5)) + .andThen(() -> setIndexerAndRampMotorRPM()); + } + // test individual motor code public void setLeftShooterMotorRPM() { leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); @@ -206,7 +218,7 @@ public class ShooterSubsystem extends SubsystemBase { public Command shootFuelCommand() { - return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) + return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1.5)) .andThen(() -> setIndexerAndRampMotorRPM()); }; diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java index b37119d..5dbe141 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java @@ -55,7 +55,6 @@ public class TargetingSubsystems extends SubsystemBase { PIDController photonAimPIDController = new PIDController(3, 0.01, 0); - static Pose2d allianceHubPose = new Pose2d(); public static Rotation2d hubThetaPose = new Rotation2d(); public static Optional alliance = DriverStation.getAlliance(); private static ShuffleboardTab cameras; @@ -117,9 +116,9 @@ public class TargetingSubsystems extends SubsystemBase { // Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose, - allianceHubPose); + Constants.TargetingConstants.allianceHubPose); - double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), allianceHubPose.getRotation().getRadians()); + double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), Constants.TargetingConstants.allianceHubPose.getRotation().getRadians()); angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0); @@ -150,7 +149,7 @@ public class TargetingSubsystems extends SubsystemBase { hubThetaPose = new Rotation2d( Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX())); - allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, + Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose); } @@ -158,7 +157,7 @@ public class TargetingSubsystems extends SubsystemBase { hubThetaPose = new Rotation2d( Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX())); - allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, + Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose); } } @@ -168,11 +167,12 @@ public class TargetingSubsystems extends SubsystemBase { public static void updateShooterRPM(Pose2d currentRobotPose) { double distance = PhotonUtils.getDistanceToPose(currentRobotPose, - allianceHubPose); - Constants.ShooterConstants.SHOOTER_RPM = -300 * Math.pow(distance, 3) - + 1221.475 * Math.pow(distance, 2) - - 1955.00131 * distance - - 1630.07168; + Constants.TargetingConstants.allianceHubPose); + Constants.ShooterConstants.SHOOTER_RPM = + (-293.84123 * Math.pow(distance, 3)) + + (1360.01497 * Math.pow(distance, 2)) + - (2391.17127 * distance) + - 1249.22704; } @@ -181,10 +181,10 @@ public class TargetingSubsystems extends SubsystemBase { public void periodic() { alliance = DriverStation.getAlliance(); SmartDashboard.putString("Target Hub Pose", - allianceHubPose.getX() + " " + allianceHubPose.getY() + " " + allianceHubPose.getRotation()); + Constants.TargetingConstants.allianceHubPose.getX() + " " + Constants.TargetingConstants.allianceHubPose.getY() + " " + Constants.TargetingConstants.allianceHubPose.getRotation()); - SmartDashboard.putString("Hub Pose", "x: " + allianceHubPose.getMeasureX() + " y: " + allianceHubPose.getY() - + " angle: " + allianceHubPose.getRotation()); + SmartDashboard.putString("Hub Pose", "x: " + Constants.TargetingConstants.allianceHubPose.getMeasureX() + " y: " + Constants.TargetingConstants.allianceHubPose.getY() + + " angle: " + Constants.TargetingConstants.allianceHubPose.getRotation()); /* * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ee84c9b..bbaf78c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -69,6 +69,10 @@ public class SwerveSubsystem extends SubsystemBase { */ private final boolean visionDriveTest = true; + + + + /** * PhotonVision class to keep an accurate odometry. */ @@ -120,10 +124,13 @@ public class SwerveSubsystem extends SubsystemBase { setupPhotonVision(); // Stop the odometry thread if we are using vision that way we can synchronize // updates better. - swerveDrive.stopOdometryThread(); + //swerveDrive.stopOdometryThread(); } setupPathPlanner(); SmartDashboard.putData("Rebuilt Field", rebuiltField); + Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS = new PathConstraints(1, 4.0, + swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(360)); + } /** @@ -286,7 +293,7 @@ public class SwerveSubsystem extends SubsystemBase { ); } - public Command driveToClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) { + public Command driveToClimbPoseOffsetted(Pose2d blueAlliancePose, Pose2d redAlliancePose) { Pose2d goal; Optional alliance = DriverStation.getAlliance(); @@ -308,6 +315,26 @@ public class SwerveSubsystem extends SubsystemBase { ); } + public Command driveIntoClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) { + + Pose2d goal; + Optional alliance = DriverStation.getAlliance(); + if (alliance.get() == Alliance.Blue) { + goal = blueAlliancePose; + } else { + goal = redAlliancePose; + } + // Create the constraints to use while pathfinding + + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + goal, + Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS, + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec + ); + } + /** * Drive with {@link SwerveSetpointGenerator} from 254, implemented by * PathPlanner. diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 3b4d08d..1a5f8d8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -319,14 +319,14 @@ public class Vision { BACK_RIGHT_CAM("Rear Right Camera", Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(), Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), /** * Front Left Camera */ - /*FRONT_LEFT_CAM("Front Left Camera", + FRONT_LEFT_CAM("Front Left Camera", Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(), Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/ + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); /**