Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Changes from Battlecry #109

Merged
merged 9 commits into from
Jun 7, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/A to D.path
Original file line number Diff line number Diff line change
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 2.9000000000000004,
"y": 6.887856966597124
"x": 2.883271729453586,
"y": 6.722020177278586
},
"prevControl": null,
"nextControl": {
"x": 3.890268068741571,
"y": 7.027030067557189
"x": 3.8735397981951567,
"y": 6.861193278238651
},
"isLocked": false,
"linkedName": "A"
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/A to E.path
Original file line number Diff line number Diff line change
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 2.9000000000000004,
"y": 6.887856966597124
"x": 2.883271729453586,
"y": 6.722020177278586
},
"prevControl": null,
"nextControl": {
"x": 5.01984991243836,
"y": 6.971014628418452
"x": 5.003121641891946,
"y": 6.805177839099914
},
"isLocked": false,
"linkedName": "A"
16 changes: 8 additions & 8 deletions src/main/deploy/pathplanner/paths/B to A.path
Original file line number Diff line number Diff line change
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 2.9,
"y": 5.27
"x": 2.883271729453586,
"y": 5.059081387777612
},
"prevControl": null,
"nextControl": {
"x": 2.7306942929129034,
"y": 5.892387378210238
"x": 2.7139660223664896,
"y": 5.68146876598785
},
"isLocked": false,
"linkedName": "B"
},
{
"anchor": {
"x": 2.9000000000000004,
"y": 6.887856966597124
"x": 2.883271729453586,
"y": 6.722020177278586
},
"prevControl": {
"x": 2.7469122475281997,
"y": 6.452168507627614
"x": 2.7301839769817855,
"y": 6.286331718309076
},
"nextControl": null,
"isLocked": false,
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/B to F.path
Original file line number Diff line number Diff line change
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 2.9,
"y": 5.27
"x": 2.883271729453586,
"y": 5.059081387777612
},
"prevControl": null,
"nextControl": {
"x": 4.511078009317217,
"y": 5.211322262872771
"x": 4.494349738770803,
"y": 5.000403650650383
},
"isLocked": false,
"linkedName": "B"
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/C to B Close.path
Original file line number Diff line number Diff line change
@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 2.9,
"y": 5.27
"x": 2.883271729453586,
"y": 5.059081387777612
},
"prevControl": {
"x": 2.5981160273115327,
"y": 5.094712531987342
"x": 2.581387756765119,
"y": 4.883793919764954
},
"nextControl": null,
"isLocked": false,
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/C to B.path
Original file line number Diff line number Diff line change
@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 2.9,
"y": 5.27
"x": 2.883271729453586,
"y": 5.059081387777612
},
"prevControl": {
"x": 2.576402701523351,
"y": 4.817991630285256
"x": 2.559674430976937,
"y": 4.607073018062868
},
"nextControl": null,
"isLocked": false,
22 changes: 17 additions & 5 deletions src/main/deploy/pathplanner/paths/D to Ferry Shot.path
Original file line number Diff line number Diff line change
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 7.7,
"y": 7.529999999999999
"x": 7.930642280685416,
"y": 7.483224552754384
},
"prevControl": null,
"nextControl": {
"x": 6.782397412017047,
"y": 7.253301738161635
"x": 7.0130396927024625,
"y": 7.20652629091602
},
"isLocked": false,
"linkedName": "D Top Ferry"
@@ -29,7 +29,19 @@
}
],
"rotationTargets": [],
"constraintZones": [],
"constraintZones": [
{
"name": "Decrease Acceleration",
"minWaypointRelativePos": 0.65,
"maxWaypointRelativePos": 1.0,
"constraints": {
"maxVelocity": 4.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
}
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.5,
14 changes: 13 additions & 1 deletion src/main/deploy/pathplanner/paths/E to Ferry Shot.path
Original file line number Diff line number Diff line change
@@ -29,7 +29,19 @@
}
],
"rotationTargets": [],
"constraintZones": [],
"constraintZones": [
{
"name": "Decrease Acceleration",
"minWaypointRelativePos": 0.6,
"maxWaypointRelativePos": 1.0,
"constraints": {
"maxVelocity": 5.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
}
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
12 changes: 6 additions & 6 deletions src/main/deploy/pathplanner/paths/NTF Start To D.path
Original file line number Diff line number Diff line change
@@ -32,12 +32,12 @@
},
{
"anchor": {
"x": 7.7,
"y": 7.529999999999999
"x": 7.930642280685416,
"y": 7.483224552754384
},
"prevControl": {
"x": 5.922230165687515,
"y": 7.685534706860702
"x": 6.152872446372931,
"y": 7.638759259615087
},
"nextControl": null,
"isLocked": false,
@@ -54,11 +54,11 @@
"constraintZones": [
{
"name": "Speed Up",
"minWaypointRelativePos": 1.0,
"minWaypointRelativePos": 1.75,
"maxWaypointRelativePos": 2.0,
"constraints": {
"maxVelocity": 4.5,
"maxAcceleration": 9.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
}
8 changes: 4 additions & 4 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -322,8 +322,8 @@ public void configureAutons() {
AutonConfig ReroutableHGF_RED = new AutonConfig("4 HGF", ReroutableFourPieceHGF::new,
"Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)", "Rerouted H To G", "Rerouted G To F");

AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new,
"Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)");
AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new,
"Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)");
AutonConfig HGF_RED = new AutonConfig("4 HGF", FourPieceHGF::new,
"Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)");

@@ -403,8 +403,8 @@ public void configureAutons() {
CBF.registerBlue(autonChooser)
.registerRed(autonChooser);

ReroutableCBAEF.registerBlue(autonChooser)
.registerRed(autonChooser);
// ReroutableCBAEF.registerBlue(autonChooser)
// .registerRed(autonChooser);

GC.registerBlue(autonChooser);
GC_RED.registerRed(autonChooser);
Original file line number Diff line number Diff line change
@@ -37,7 +37,7 @@

public class AmpScoreRoutine extends SequentialCommandGroup {

private static final double SCORE_ALIGN_TIMEOUT = 1.25;
private static final double SCORE_ALIGN_TIMEOUT = 1.75; // 1.25

private static final double AMP_WALL_SETUP_X_TOLERANCE = Units.inchesToMeters(1.0);
private static final double AMP_WALL_SETUP_Y_TOLERANCE = Units.inchesToMeters(4.0);
@@ -62,6 +62,7 @@ public AmpScoreRoutine() {
new ParallelCommandGroup(
new WaitUntilCommand(() -> Amper.getInstance().hasNote()),
new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get()))
.withMaxSpeed(3.0)
.withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE)
.deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN))
),
Original file line number Diff line number Diff line change
@@ -72,6 +72,7 @@ public static SwerveDriveToPose speakerRelative(double angleToSpeaker) {
private double yTolerance;
private double thetaTolerance;
private double velocityTolerance;
private double maxSpeed;

private Pose2d targetPose;

@@ -86,6 +87,7 @@ public SwerveDriveToPose(Supplier<Pose2d> poseSupplier) {
this.poseSupplier = poseSupplier;

targetPose2d = odometry.getField().getObject("Target Pose");
maxSpeed = Swerve.MAX_MODULE_SPEED;

controller = new HolonomicController(
new PIDController(Translation.kP, Translation.kI, Translation.kD),
@@ -138,6 +140,11 @@ public SwerveDriveToPose withTolerance(Number x, Number y, Number theta) {
return this;
}

public SwerveDriveToPose withMaxSpeed(double speed) {
maxSpeed = speed;
return this;
}

@Override
public void initialize() {
targetPose = poseSupplier.get();
@@ -155,7 +162,7 @@ public void execute() {
controller.update(targetPose, odometry.getPose());

Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond)
.clamp(Swerve.MAX_MODULE_SPEED);
.clamp(maxSpeed);
double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get());

SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance());
6 changes: 3 additions & 3 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
Original file line number Diff line number Diff line change
@@ -326,9 +326,9 @@ public interface Shooter {
ShooterSpeeds REVERSE = new ShooterSpeeds(-3000, -3000);

ShooterSpeeds FERRY = new ShooterSpeeds(
new SmartNumber("Shooter/Ferry Shooter RPM", 5500),
new SmartNumber("Shooter/Ferry Shooter RPM", 6000), // 5500
500,
new SmartNumber("Shooter/Ferry Feeder RPM", 3000));
new SmartNumber("Shooter/Ferry Feeder RPM", 3500)); // 3000

ShooterSpeeds WING_FERRY = new ShooterSpeeds(2000, 2500);

@@ -405,7 +405,7 @@ public interface Alignment {
double PODIUM_SHOT_MAX_ANGLE = 80;

SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5));
SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.5)); // NOTE: remove "- 1.5" for battlecry
SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.75)); // NOTE: remove "- 1.5" for battlecry

SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(21.0));
SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0));
Original file line number Diff line number Diff line change
@@ -70,12 +70,9 @@ public void periodic() {
super.periodic();

outputs.clear();

boolean hasAnyData = false;


for (TheiaCamera camera : cameras) {
if (camera.getVisionData().isPresent())
hasAnyData = true;
SmartDashboard.putBoolean(camera.getName() + "/Has Data", camera.getVisionData().isPresent());

camera.getVisionData().ifPresent(
(VisionData data) -> {
@@ -84,7 +81,7 @@ public void periodic() {
});
}

SmartDashboard.putBoolean("Vision/Has Any Data", hasAnyData);
SmartDashboard.putBoolean("Vision/Has Any Data", outputs.size() > 0);
}

private void updateTelemetry(String prefix, VisionData data) {