Skip to content

Commit

Permalink
Minor fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Jul 30, 2024
1 parent a213446 commit 4f0da97
Show file tree
Hide file tree
Showing 7 changed files with 74 additions and 93 deletions.
33 changes: 13 additions & 20 deletions src/main/deploy/pathplanner/autos/Center_CloseBottom_CloseMid_CloseTop_FarTop.auto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,38 +92,31 @@
"name": "auto shoot"
}
},
{
"type": "path",
"data": {
"pathName": "CloseTop_FarTop"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intake"
}
},
{
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "CloseTop_FarTop2"
}
},
{
"type": "path",
"data": {
"pathName": "FarTop_LeftStage"
}
}
]
"name": "auto intake"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "FarTop_LeftStage"
}
},
{
"type": "named",
"data": {
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/paths/CloseTop_FarTop.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
"maxAngularAcceleration": 2160.0
},
"goalEndState": {
"velocity": 0,
"velocity": 4.0,
"rotation": 8.652541791114864,
"rotateFast": true
},
Expand Down
58 changes: 29 additions & 29 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ public static class AutoNames {
}

public static class Drive {
public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(1.0), Units.Value.of(0.9));
public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(1.1), Units.Value.of(1.0));
public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(8.0, 0.0, 0.3, 0.0, 0.0);
public static final Measure<Dimensionless> DRIVE_SLIP_RATIO = Units.Percent.of(8.0);
public static final double DRIVE_TURN_SCALAR = 90.0;
Expand Down Expand Up @@ -168,40 +168,40 @@ public static class Shooter {
);
public static final TrapezoidProfile.Constraints ANGLE_MOTION_CONSTRAINT = new TrapezoidProfile.Constraints(
Units.DegreesPerSecond.of(360.0),
Units.DegreesPerSecond.of(360.0 * 6).per(Units.Second)
Units.DegreesPerSecond.of(360.0 * 3).per(Units.Second)
);
public static final List<Entry<Measure<Distance>,State>> SHOOTER_MAP = Arrays.asList(
Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(55.0))),
Map.entry(Units.Meters.of(1.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(55.0))),
Map.entry(Units.Meters.of(1.40), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(55.0))),
Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(53.0))),
Map.entry(Units.Meters.of(1.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(51.0))),
Map.entry(Units.Meters.of(1.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(49.0))),
Map.entry(Units.Meters.of(1.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(47.0))),
Map.entry(Units.Meters.of(1.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(46.0))),
Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(45.0))),
Map.entry(Units.Meters.of(2.10), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(44.0))),
Map.entry(Units.Meters.of(2.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(43.0))),
Map.entry(Units.Meters.of(2.30), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(42.0))),
Map.entry(Units.Meters.of(2.40), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(41.0))),
Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(40.0))),
Map.entry(Units.Meters.of(2.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(39.0))),
Map.entry(Units.Meters.of(2.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))),
Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.0))),
Map.entry(Units.Meters.of(2.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.0))),
Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.0))),
Map.entry(Units.Meters.of(3.10), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(35.0))),
Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(34.0))),
Map.entry(Units.Meters.of(3.30), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(33.0))),
Map.entry(Units.Meters.of(3.40), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))),
Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))),
Map.entry(Units.Meters.of(3.60), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(3.70), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(30.0))),
Map.entry(Units.Meters.of(3.80), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0))),
Map.entry(Units.Meters.of(3.90), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0))),
Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0))),
Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(28.0))),
Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(25.0)))
Map.entry(Units.Meters.of(1.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(52.0))),
Map.entry(Units.Meters.of(1.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(50.0))),
Map.entry(Units.Meters.of(1.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(48.0))),
Map.entry(Units.Meters.of(1.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(47.0))),
Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(46.0))),
Map.entry(Units.Meters.of(2.10), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(45.0))),
Map.entry(Units.Meters.of(2.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(44.0))),
Map.entry(Units.Meters.of(2.30), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(43.0))),
Map.entry(Units.Meters.of(2.40), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(42.0))),
Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(41.0))),
Map.entry(Units.Meters.of(2.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(40.0))),
Map.entry(Units.Meters.of(2.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(39.0))),
Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))),
Map.entry(Units.Meters.of(2.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))),
Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.0))),
Map.entry(Units.Meters.of(3.10), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(36.0))),
Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(35.0))),
Map.entry(Units.Meters.of(3.30), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(34.0))),
Map.entry(Units.Meters.of(3.40), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(34.0))),
Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(33.0))),
Map.entry(Units.Meters.of(3.60), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))),
Map.entry(Units.Meters.of(3.70), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(3.80), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(3.90), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(29.0))),
Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(26.0)))
);
}

Expand Down
39 changes: 21 additions & 18 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ public class RobotContainer {
IntakeSubsystem.initializeHardware(),
Constants.Intake.ROLLER_VELOCITY
);
//private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance();
private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance();

private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);

Expand All @@ -83,6 +83,8 @@ public RobotContainer() {
// Configure auto builder
DRIVE_SUBSYSTEM.configureAutoBuilder();

VISION_SUBSYSTEM.setPoseSupplier(() -> DRIVE_SUBSYSTEM.getPose());

// Register named commands
NamedCommands.registerCommand(Constants.NamedCommands.INTAKE_COMMAND_NAME, autoIntakeCommand().withTimeout(7));
NamedCommands.registerCommand(Constants.NamedCommands.PRELOAD_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(1.1));
Expand Down Expand Up @@ -173,6 +175,8 @@ private void configureBindings() {
() -> PRIMARY_CONTROLLER.getLeftX(),
() -> PRIMARY_CONTROLLER.getRightX()
));

PRIMARY_CONTROLLER.povLeft().whileTrue(aimAndIntakeObjectCommand());
}

/**
Expand Down Expand Up @@ -301,7 +305,7 @@ private Command feedThroughCommand() {
* @return Command to aim robot at object, drive, and intake a game object
*/
private Command aimAndIntakeObjectCommand() {
return Commands.sequence(
// return Commands.sequence(
// DRIVE_SUBSYSTEM.driveCommand(() -> 0, () -> 0, () -> 0).withTimeout(0.1),
// DRIVE_SUBSYSTEM.aimAtPointCommand(
// () -> VISION_SUBSYSTEM.getObjectLocation().isPresent() ? PRIMARY_CONTROLLER.getLeftY() : 0,
Expand All @@ -312,22 +316,21 @@ private Command aimAndIntakeObjectCommand() {
// },
// false,
// false).until(() -> VISION_SUBSYSTEM.shouldIntake()),
// Commands.parallel(
// DRIVE_SUBSYSTEM.aimAtPointCommand(
// () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos() * 0.75,
// () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin() * 0.75,
// () -> 0,
// () -> {
// return VISION_SUBSYSTEM.getObjectLocation().orElse(null);
// },
// false,
// false)

// INTAKE_SUBSYSTEM.intakeCommand(),
// SHOOTER_SUBSYSTEM.intakeCommand()
// )
// .until(() -> SHOOTER_SUBSYSTEM.isObjectPresent())
);
return Commands.parallel(
DRIVE_SUBSYSTEM.aimAtPointCommand(
() -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos() * 0.75,
() -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin() * 0.75,
() -> 0,
() -> {
return VISION_SUBSYSTEM.getObjectLocation().orElse(null);
},
false,
false),

INTAKE_SUBSYSTEM.intakeCommand(),
SHOOTER_SUBSYSTEM.intakeCommand()
)
.until(() -> SHOOTER_SUBSYSTEM.isObjectPresent());
}

/**
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,7 @@ public ShooterSubsystem(Hardware shooterHardware, SparkPIDConfig flywheelConfig,
// Set default command to track speaker angle
setDefaultCommand(run(() -> {
var state = getAutomaticState();
state = new State(SPINUP_SPEED, state.angle);
state = new State(ZERO_FLYWHEEL_SPEED, state.angle);
setState(state, true);
}));

Expand Down Expand Up @@ -325,7 +325,7 @@ private State normalizeState(State state) {
* Reset shooter state
*/
private void resetState() {
setState(new State(SPINUP_SPEED, m_desiredShooterState.angle), false);
setState(new State(ZERO_FLYWHEEL_SPEED, m_desiredShooterState.angle), false);
}

/**
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/vision/ObjectCamera.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units;
import frc.robot.Constants;
import frc.robot.subsystems.vision.AprilTagCamera.Resolution;
import org.lasarobotics.vision.AprilTagCamera.Resolution;

/**
* Camera that looks for rings on the ground
Expand Down
29 changes: 7 additions & 22 deletions src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.subsystems.vision.AprilTagCamera.AprilTagCameraResult;

public class VisionSubsystem extends SubsystemBase implements AutoCloseable {
Expand Down Expand Up @@ -92,18 +93,6 @@ private VisionSubsystem(Hardware visionHardware) {
// Set field layout for sim
m_sim.addAprilTags(m_fieldLayout);

// Setup camera pose estimation threads
this.m_cameraNotifier = (RobotBase.isReal())
? new Notifier(() -> {
for (var camera : m_apriltagCameras) camera.run();
updateEstimatedGlobalPoses();
})
: new Notifier(() -> {
if (m_poseSupplier != null) m_sim.update(m_poseSupplier.get());
for (var camera : m_apriltagCameras) camera.run();
updateEstimatedGlobalPoses();
});

// Set all cameras to primary pipeline
for (var camera : m_apriltagCameras) camera.setPipelineIndex(0);

Expand All @@ -115,10 +104,6 @@ private VisionSubsystem(Hardware visionHardware) {

// Add AprilTag cameras to sim
for (var camera : m_apriltagCameras) m_sim.addCamera(camera.getCameraSim(), camera.getTransform());

// Start camera thread
m_cameraNotifier.setName(getName());
m_cameraNotifier.startPeriodic(GlobalConstants.ROBOT_LOOP_PERIOD);
}

/**
Expand All @@ -127,12 +112,12 @@ private VisionSubsystem(Hardware visionHardware) {
*/
private static Hardware initializeHardware() {
Hardware visionHardware = new Hardware(
// new ObjectCamera(
// Constants.VisionHardware.CAMERA_OBJECT_NAME,
// Constants.VisionHardware.CAMERA_OBJECT_LOCATION,
// Constants.VisionHardware.CAMERA_OBJECT_RESOLUTION,
// Constants.VisionHardware.CAMERA_OBJECT_FOV
// ),
new ObjectCamera(
Constants.VisionHardware.CAMERA_OBJECT_NAME,
Constants.VisionHardware.CAMERA_OBJECT_LOCATION,
Constants.VisionHardware.CAMERA_OBJECT_RESOLUTION,
Constants.VisionHardware.CAMERA_OBJECT_FOV
)
// new AprilTagCamera(
// Constants.VisionHardware.CAMERA_A_NAME,
// Constants.VisionHardware.CAMERA_A_LOCATION,
Expand Down

0 comments on commit 4f0da97

Please sign in to comment.