Skip to content

Commit

Permalink
Merge pull request #16 from Team537/MobiliT
Browse files Browse the repository at this point in the history 
Feat: Add complex autonomous paths
  • Loading branch information
@CrumblzTheEgg
CrumblzTheEgg authored Apr 2, 2024
2 parents 1d7f45d + fc26d90 commit 60fc2d0
Show file tree
Hide file tree
Showing 19 changed files with 782 additions and 148 deletions.
5 changes: 4 additions & 1 deletion .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,5 +26,8 @@
},
],
"java.test.defaultConfig": "WPIlibUnitTests",
"java.debug.settings.onBuildFailureProceed": true
"java.debug.settings.onBuildFailureProceed": true,
"cSpell.words": [
"Odometry"
]
}
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.1.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
}

java {
Expand Down
103 changes: 77 additions & 26 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;

/**
* The Constants class provides a convenient place for teams to hold robot-wide
Expand All @@ -36,7 +37,7 @@ public final class Constants {
public static final class DriveConstants {
// Driving Parameters - Note that these are not the maximum capable speeds of
// the robot, rather the allowed maximum speeds
public static final double MAX_SPEED_METERS_PER_SECOND = 2; // 2 // meters per second
public static final double MAX_SPEED_METERS_PER_SECOND = 2.0; // 2 // meters per second
public static final double BOOST_MODE_MAX_SPEED_METERS_PER_SECOND = 4.8; // meters per second
public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // radians per second

Expand Down Expand Up @@ -162,21 +163,21 @@ public static final class VisionConstants {

// Camera settings
public static final String LIMELIGHT_NAME = "limelight";
public static final String BLACK_AND_WHITE_CAMERA_NAME = "Arducam_OV9281_USB_Camera";
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";

// Camera offsets (NOTE: These values are placeholders and thus subject to
// change).
public static final Transform3d FRONT_CAMERA_OFFSET = new Transform3d(
// Camera offsets (NOTE: These values are placeholders and thus subject to change).
public static final Transform3d RED_OUTTAKE_CAMERA_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d RIGHT_CAMERA_OFFSET = new Transform3d(
public static final Transform3d RED_INTAKE_CAMERA_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d LEFT_CAMERA_OFFSET = new Transform3d(
public static final Transform3d BLACK_OUTTAKE_CAMERA_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d BACK_CAMERA_OFFSET = new Transform3d(
public static final Transform3d BLACK_INTAKE_CAMERA_OFFSET = new Transform3d(
new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
}
Expand Down Expand Up @@ -207,43 +208,72 @@ public static final class AutoConstants {

// Constraint for the motion profiled robot angle controller
// Value is a float because xyz
public static final double TRAJECOTRY_THRESHOLD = 0.1; //the maximum distance away from a waypoint before the robot moves onto the next one (meters)
public static final double TRAJECTORY_THRESHOLD = 0.1; //the maximum distance away from a waypoint before the robot moves onto the next one (meters)
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED);

public static final Pose2d BLUE_1_STARTING_POSE = new Pose2d(1.665, 6.636, Rotation2d.fromRadians((1.0/3.0) * Math.PI));
public static final Pose2d BLUE_2_STARTING_POSE = new Pose2d(2.362, 5.484, Rotation2d.fromRadians(0));
public static final Pose2d BLUE_3_STARTING_POSE = new Pose2d(1.689, 4.451, Rotation2d.fromRadians((5.0/3.0) * Math.PI));
public static final Pose2d RED_1_STARTING_POSE = new Pose2d(16.238, 6.650, Rotation2d.fromRadians((2.0/3.0) * Math.PI ));
public static final Pose2d RED_2_STARTING_POSE = new Pose2d(15.481, 5.593, Rotation2d.fromRadians(Math.PI));
public static final Pose2d RED_3_STARTING_POSE = new Pose2d(16.322, 4.367, Rotation2d.fromRadians((4.0/3.0) * Math.PI));
public static final Pose2d BLUE_1_STARTING_POSE = new Pose2d(1.629, 6.457, Rotation2d.fromDegrees(0));
public static final Pose2d BLUE_2_STARTING_POSE = new Pose2d(1.278, 5.603, Rotation2d.fromDegrees(0));
public static final Pose2d BLUE_3_STARTING_POSE = new Pose2d(1.629, 4.749, Rotation2d.fromDegrees(0));
public static final Pose2d RED_1_STARTING_POSE = new Pose2d(14.884, 6.457, Rotation2d.fromDegrees(180));
public static final Pose2d RED_2_STARTING_POSE = new Pose2d(15.235, 5.603, Rotation2d.fromDegrees(180));
public static final Pose2d RED_3_STARTING_POSE = new Pose2d(14.884, 4.749, Rotation2d.fromDegrees(180));

// Old paths
public static final List<Pose2d> RED_1_TRAJECTORY = List.of(
new Pose2d(15.036,7.671, RED_1_STARTING_POSE.getRotation()),
new Pose2d(15.072,7.671, RED_1_STARTING_POSE.getRotation())
);
public static final List<Pose2d> RED_2_TRAJECTORY = List.of(
new Pose2d(12.429,5.713, RED_2_STARTING_POSE.getRotation())
);
public static final List<Pose2d> RED_3_TRAJECTORY = List.of(
new Pose2d(14.940,1.580, RED_3_STARTING_POSE.getRotation()),
new Pose2d(13.475,1.195, RED_3_STARTING_POSE.getRotation())
);
public static final List<Pose2d> BLUE_1_TRAJECTORY = List.of(
new Pose2d(2.926,7.563, BLUE_1_STARTING_POSE.getRotation()),
new Pose2d(5.497,7.563, BLUE_1_STARTING_POSE.getRotation())
);

public static final List<Pose2d> BLUE_2_TRAJECTORY = List.of(
new Pose2d(5.293,5.593, BLUE_2_STARTING_POSE.getRotation())
);

public static final List<Pose2d> BLUE_3_TRAJECTORY = List.of(
new Pose2d(3.503,0.931, BLUE_3_STARTING_POSE.getRotation()),
new Pose2d(4.440,0.931, BLUE_3_STARTING_POSE.getRotation())
);

public static final List<Pose2d> RED_1_TRAJECTORY = List.of(
new Pose2d(15.036,7.671, RED_1_STARTING_POSE.getRotation()),
new Pose2d(15.072,7.671, RED_1_STARTING_POSE.getRotation())
// New Paths
public static final List<Pose2d> BLUE_1_COMPLEX_POSITIONS = List.of(
new Pose2d(1.629, 7.807, Rotation2d.fromDegrees(0)), // Close to amp position
new Pose2d(7.717, 7.436, Rotation2d.fromDegrees(0)), // Topmost note
new Pose2d(7.717, 5.785, Rotation2d.fromDegrees(0)) // Second top note
);

public static final List<Pose2d> RED_2_TRAJECTORY = List.of(
new Pose2d(12.429,5.713, RED_2_STARTING_POSE.getRotation())
public static final List<Pose2d> BLUE_2_COMPLEX_POSITIONS = List.of(
new Pose2d(2.273, 4.088, Rotation2d.fromDegrees(0)), // Bottom Note
new Pose2d(2.273, 5.569, Rotation2d.fromDegrees(0)), // Middle Note
new Pose2d(2.273, 7.004, Rotation2d.fromDegrees(0)) // Top Note
);
public static final List<Pose2d> BLUE_3_COMPLEX_POSITIONS = List.of(
new Pose2d(4.726, 4.840, Rotation2d.fromDegrees(0)), // Position under triangle thingy
new Pose2d(7.717, 4.099, Rotation2d.fromDegrees(0)), // Center note in center of field.
new Pose2d(7.717, 2.425, Rotation2d.fromDegrees(0)) // Second to last note in center of field.
);

public static final List<Pose2d> RED_3_TRAJECTORY = List.of(
new Pose2d(14.940,1.580, RED_3_STARTING_POSE.getRotation()),
new Pose2d(13.475,1.195, RED_3_STARTING_POSE.getRotation())
public static final List<Pose2d> RED_1_COMPLEX_POSITIONS = List.of(
new Pose2d(14.884, 7.807, Rotation2d.fromDegrees(180)), // Close to amp position
new Pose2d(8.796, 7.436, Rotation2d.fromDegrees(180)), // Topmost note
new Pose2d(8.796, 5.785, Rotation2d.fromDegrees(180)) // Second top note
);
public static final List<Pose2d> RED_2_COMPLEX_POSITIONS = List.of(
new Pose2d(14.24, 4.088, Rotation2d.fromDegrees(180)), // Bottom Note
new Pose2d(14.24, 5.569, Rotation2d.fromDegrees(180)), // Middle Note
new Pose2d(14.24, 7.004, Rotation2d.fromDegrees(180)) // Top Note
);
public static final List<Pose2d> RED_3_COMPLEX_POSITIONS = List.of(
new Pose2d(11.787, 4.840, Rotation2d.fromDegrees(180)), // Position under triangle thingy
new Pose2d(8.796, 4.099, Rotation2d.fromDegrees(180)), // Center note in center of field.
new Pose2d(8.796, 2.425, Rotation2d.fromDegrees(180)) // Second to last note in center of field.
);
}

Expand All @@ -254,6 +284,27 @@ public static final class NeoMotorConstants {
public static final class FieldConstants {
public static final Pose2d SPEAKER_POSE = new Pose2d(0, 0, new Rotation2d());
public static final double SPEAKER_HEIGHT = 81;

// Speaker Scoring Locations
public static final Pose2d RED_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));
public static final Pose2d RED_ALLIANCE_SPEAKER_LEFT_SCORING_LOCATION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));
public static final Pose2d RED_ALLIANCE_SPEAKER_RIGHT_SCORING_LOCATION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));

public static final Pose2d BLUE_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION =
new Pose2d(new Translation2d(1.278, 5.603), Rotation2d.fromDegrees(0));
public static final Pose2d BLUE_ALLIANCE_SPEAKER_LEFT_SCORING_LOCATION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));
public static final Pose2d BLUE_ALLIANCE_SPEAKER_RIGHT_SCORING_LOCATION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));

// AMP Scoring locations
public static final Pose2d RED_AMP_SCORING_POSITION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));
public static final Pose2d BLUE_AMP_SCORING_POSITION =
new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0));
}

public static final class IntakeConstants {
Expand Down
7 changes: 3 additions & 4 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@

package frc.robot;

import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
Expand Down Expand Up @@ -37,8 +36,8 @@ public void robotInit() {
// Start the timer
snapshotTimer.start();

// Mak eit possible to view caeras on the driverstation.
CameraServer.startAutomaticCapture();
// Mak eit possible to view cameras on the driver station.
// CameraServer.startAutomaticCapture();

// Make it possible to view the photonvision dashboard over the internet
PortForwarder.add(5800, "photonvision.local", 5800);
Expand Down Expand Up @@ -100,7 +99,7 @@ public void autonomousPeriodic() {
public void teleopInit() {

// Update the robot's settings so that they match what was configured on SmartDashboard.
robotContainer.configureDriverPrefferences();
robotContainer.configureDriverPreferences();

// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
Expand Down
Loading

0 comments on commit 60fc2d0

Please sign in to comment.