Skip to content
Open
Show file tree
Hide file tree
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
37 changes: 37 additions & 0 deletions src/main/deploy/pathplanner/autos/VisionSourceSide2ScoreN4.auto
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 0.73,
"y": 4.47
},
"rotation": 120.0
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "SubwooferLaunch"
}
},
{
"type": "path",
"data": {
"pathName": "LowSpeakerToFarN4ToLaunch"
}
},
{
"type": "named",
"data": {
"name": "VisionLaunch"
}
}
]
}
},
"folder": "Vision Autos",
"choreoAuto": false
}
144 changes: 144 additions & 0 deletions src/main/deploy/pathplanner/paths/LowSpeakerToFarN4ToLaunch.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,144 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.7275269251393202,
"y": 4.4671669970120575
},
"prevControl": null,
"nextControl": {
"x": 1.146100930190991,
"y": 3.4001885029736156
},
"isLocked": false,
"linkedName": "LowSpeaker"
},
{
"anchor": {
"x": 3.7844624260677096,
"y": 1.7444754889509784
},
"prevControl": {
"x": 2.076817112087623,
"y": 1.7678637978358573
},
"nextControl": {
"x": 4.556702651817908,
"y": 1.7338987065329259
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 8.44590590082081,
"y": 2.4960698570045796
},
"prevControl": {
"x": 7.976085063097927,
"y": 2.378058890871132
},
"nextControl": {
"x": 9.030376579271676,
"y": 2.642878903991426
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.690191951812475,
"y": 1.7444754889509784
},
"prevControl": {
"x": 7.053385240517052,
"y": 1.8806304206600526
},
"nextControl": {
"x": 4.326998663107897,
"y": 1.6083205572419041
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.9089022069531514,
"y": 1.9539173222406319
},
"prevControl": {
"x": 3.9750353786421866,
"y": 1.6125403679378794
},
"nextControl": null,
"isLocked": false,
"linkedName": "SourceVisionLaunch"
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.05,
"rotationDegrees": 120.0,
"rotateFast": false
},
{
"waypointRelativePos": 1.0,
"rotationDegrees": -171.13373619481098,
"rotateFast": false
},
{
"waypointRelativePos": 1.75,
"rotationDegrees": -167.33444776423735,
"rotateFast": false
},
{
"waypointRelativePos": 4.0,
"rotationDegrees": -164.3286881943862,
"rotateFast": false
},
{
"waypointRelativePos": 0.5,
"rotationDegrees": 159.12457737382582,
"rotateFast": false
}
],
"constraintZones": [],
"eventMarkers": [
{
"name": "intake",
"waypointRelativePos": 1.35,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Intake"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 120.0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 120.0,
"velocity": 0
},
"useDefaultConstraints": false
}
2 changes: 1 addition & 1 deletion src/main/deploy/swerve/modules/physicalproperties.json
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
},
"conversionFactor": {
"angle": 19.2,
"drive": 0.0696793458626792
"drive": 0.06658958893604663608562691131498
Copy link

@markpete markpete Jan 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Floats only have 7 significant digits (plus an exponent) and doubles have 16 decimal digits (plus an exponent) of precision, so this may be overkill.

https://en.wikipedia.org/wiki/Floating-point_arithmetic#IEEE_754:_floating_point_in_modern_computers

},
"rampRate": {
"drive": 0.25,
Expand Down
40 changes: 32 additions & 8 deletions src/main/java/frc/team2412/robot/Controls.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT;
import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT;
import static frc.team2412.robot.Subsystems.SubsystemConstants.APRILTAGS_ENABLED;
import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
import static frc.team2412.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED;
import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED;
Expand All @@ -11,6 +12,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
Expand Down Expand Up @@ -136,6 +138,25 @@ public Controls(Subsystems s) {
// this,
// codriveController.leftBumper()));
}
if (DRIVEBASE_ENABLED && APRILTAGS_ENABLED) {
new Trigger(s.rotateToSpeaker)
.whileTrue(
s.drivebaseSubsystem
.rotateToAngle(
() -> {
double currentTimestamp = Timer.getFPGATimestamp();
double robotToTagAngleTimestamp =
s.apriltagsProcessor.getLastRotatedAngleTimestamp();
var robotAngle = s.drivebaseSubsystem.getPose().getRotation();
var robotToTagAngle = s.apriltagsProcessor.getLastRotatedAngle();
if (currentTimestamp - robotToTagAngleTimestamp > 0.1) {
return robotAngle;
}
return robotAngle.plus(robotToTagAngle);
},
false)
.withName("RotateToSpeaker"));
}
}
// LED
private void bindLEDControls() {
Expand Down Expand Up @@ -211,22 +232,25 @@ private void bindLauncherControls() {
launcherLowerPresetButton.onTrue(
s.launcherSubsystem
.runOnce(s.launcherSubsystem::stopLauncher)
.andThen(new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE)));
.andThen(new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE))
.withTimeout(2.0));
launcherSubwooferPresetButton.onTrue(
new SetAngleLaunchCommand(
s.launcherSubsystem,
LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
LauncherSubsystem.SUBWOOFER_AIM_ANGLE));
s.launcherSubsystem,
LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
LauncherSubsystem.SUBWOOFER_AIM_ANGLE)
.withTimeout(2.0));
// launcherPodiumPresetButton.onTrue(
// new SetAngleLaunchCommand(
// s.launcherSubsystem,
// LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
// LauncherSubsystem.PODIUM_AIM_ANGLE));
launcherAmpPresetButton.onTrue(
new SetAngleAmpLaunchCommand(
s.launcherSubsystem,
LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
LauncherSubsystem.AMP_AIM_ANGLE));
s.launcherSubsystem,
LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
LauncherSubsystem.AMP_AIM_ANGLE)
.withTimeout(2.0));

// launcherTrapPresetButton.onTrue(
// TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
Expand All @@ -244,7 +268,7 @@ private void bindLauncherControls() {
// s.launcherSubsystem.runEnd(
// s.launcherSubsystem::launch, s.launcherSubsystem::stopLauncher));

driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(6500)));
driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch()));
}

private void bindSysIdControls() {
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/team2412/robot/Subsystems.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import frc.team2412.robot.subsystems.LauncherSubsystem;
import frc.team2412.robot.subsystems.LimelightSubsystem;
import frc.team2412.robot.util.DrivebaseWrapper;
import java.util.function.BooleanSupplier;

public class Subsystems {
public static class SubsystemConstants {
Expand All @@ -29,6 +30,7 @@ public static class SubsystemConstants {
public final IntakeSubsystem intakeSubsystem;
public final LEDSubsystem ledSubsystem;
public final AprilTagsProcessor apriltagsProcessor;
public final BooleanSupplier rotateToSpeaker;

public Subsystems() {
// initialize subsystems here (wow thats wild)
Expand All @@ -41,8 +43,10 @@ public Subsystems() {
}
if (APRILTAGS_ENABLED) {
apriltagsProcessor = new AprilTagsProcessor(drivebaseWrapper);
rotateToSpeaker = () -> apriltagsProcessor.shouldRotateToSpeaker();
} else {
apriltagsProcessor = null;
rotateToSpeaker = () -> false;
}
if (LAUNCHER_ENABLED) {
launcherSubsystem = new LauncherSubsystem();
Expand Down
Loading
Loading