Skip to content

Commit

Permalink
fix: Fix complex auto paths to function as intended
Browse files Browse the repository at this point in the history 
This commit should make the robot follow the paths as intended.
  • Loading branch information
@CameronMyhre
CameronMyhre committed Apr 4, 2024
1 parent fc26d90 commit 7cfdfb4
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 14 deletions.
15 changes: 6 additions & 9 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,21 +163,18 @@ public static final class VisionConstants {

// Camera settings
public static final String LIMELIGHT_NAME = "limelight";
public static final String ARDUCAM_OV9281_USB_CAMERA = "Arducam_OV9281_USB_Camera";
public static final String ARDUCAM_OV2311_USB_CAMERA = "Arducam_OV2311_USB_Camera";
public static final String COLOR_CAMERA_NAME = "USB_2M_GS_camera";
public static final String ARDUCAM_OV9281_USB_CAMERA_NAME = "Arducam_OV9281_USB_Camera";
public static final String ARDUCAM_OV2311_USB_CAMERA_NAME = "Arducam_OV2311_USB_Camera";
public static final String USB_2M_GS_CAMERA_NAME = "USB_2M_GS_camera";

// Camera offsets (NOTE: These values are placeholders and thus subject to change).
public static final Transform3d RED_OUTTAKE_CAMERA_OFFSET = new Transform3d(
public static final Transform3d ARDUCAM_OV9281_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d RED_INTAKE_CAMERA_OFFSET = new Transform3d(
public static final Transform3d ARDUCAM_OV2311_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d BLACK_OUTTAKE_CAMERA_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d BLACK_INTAKE_CAMERA_OFFSET = new Transform3d(
public static final Transform3d USB_2M_GS_CAMERA_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
}
Expand Down
12 changes: 7 additions & 5 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,9 @@ public class RobotContainer {

// The robot's subsystems
private final RobotVision robotVision = new RobotVision.Builder()
.addPhotonVisionCamera(VisionConstants.ARDUCAM_OV9281_USB_CAMERA, VisionConstants.RED_OUTTAKE_CAMERA_OFFSET,
.addPhotonVisionCamera(VisionConstants.ARDUCAM_OV2311_USB_CAMERA_NAME, VisionConstants.ARDUCAM_OV9281_OFFSET,
VisionConstants.APRIL_TAG_PIPELINE)
.addPhotonVisionCamera(VisionConstants.USB_2M_GS_CAMERA_NAME, VisionConstants.USB_2M_GS_CAMERA_OFFSET,
VisionConstants.APRIL_TAG_PIPELINE)
.build();
private final DriveSubsystem driveSubsystem = new DriveSubsystem(true, robotVision::estimateRobotPose);
Expand Down Expand Up @@ -251,7 +253,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption)
complexPath = new SequentialCommandGroup(
// Shoot note
new FollowTrajectoryCommand(driveSubsystem, // Drive up to the amp and then grab the note closest to the wall near the amp.
AutoConstants.BLUE_1_COMPLEX_POSITIONS.subList(0, 1)),
AutoConstants.BLUE_1_COMPLEX_POSITIONS.subList(0, 2)),
// Grab note
new FollowTrajectoryCommand(driveSubsystem, List.of(
AutoConstants.BLUE_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp
Expand Down Expand Up @@ -293,7 +295,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption)
complexPath = new SequentialCommandGroup(
// Score note
new FollowTrajectoryCommand(driveSubsystem,
AutoConstants.BLUE_3_COMPLEX_POSITIONS.subList(0, 1)),
AutoConstants.BLUE_3_COMPLEX_POSITIONS.subList(0, 2)),
// Grab note
new FollowTrajectoryCommand(driveSubsystem, List.of(
AutoConstants.BLUE_3_COMPLEX_POSITIONS.get(0),
Expand All @@ -316,7 +318,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption)
complexPath = new SequentialCommandGroup(
// Shoot note
new FollowTrajectoryCommand(driveSubsystem, // Drive up to the amp and then grab the note closest to the wall near the amp.
AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 1)),
AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 2)),
// Grab note
new FollowTrajectoryCommand(driveSubsystem, List.of(
AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp
Expand Down Expand Up @@ -357,7 +359,7 @@ private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption)
complexPath = new SequentialCommandGroup(
// Score note
new FollowTrajectoryCommand(driveSubsystem,
AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 1)),
AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 2)),
// Grab note
new FollowTrajectoryCommand(driveSubsystem, List.of(
AutoConstants.RED_1_COMPLEX_POSITIONS.get(0),
Expand Down

0 comments on commit 7cfdfb4

Please sign in to comment.