Skip to content
This repository has been archived by the owner on Nov 13, 2024. It is now read-only.

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ernie3
Ernie3 committed Mar 2, 2024
1 parent 1124b2d commit cde4c36
Show file tree
Hide file tree
Showing 5 changed files with 68 additions and 73 deletions.
59 changes: 32 additions & 27 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,6 @@

public class RobotContainer {
private static RobotContainer instance;
public static AutoBuilder autoBuilder;

public static RobotContainer getInstance() {
if (instance == null) {
Expand All @@ -55,10 +54,15 @@ public static RobotContainer getInstance() {
return instance;
}

private static SendableChooser<Command> autoChooser;
private SendableChooser<Command> autoChooser;
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}

private AutoBuilder autoBuilder;
public AutoBuilder getAutoBuilder() {
return autoBuilder;
}

public static final AHRS navx = new AHRS(Port.kUSB);

Expand Down Expand Up @@ -100,6 +104,7 @@ public static double getRobotRoll() {
//public static final NewLEDStrip ledStrip = new NewLEDStrip();
public static final OldLEDStrip leds = new OldLEDStrip();

// Cameras
public static final NoteDetectorCamera frontNoteCamera = new NoteDetectorCamera("NoteCamera", Constants.FRONT_NOTE_CAMERA_OFFSET);
public static final AprilTagCamera frontAprilTagCamera = new AprilTagCamera("FrontCamera", Constants.FRONT_APRILTAG_CAMERA_OFFSET);
public static final AprilTagCamera rearAprilTagCamera = new AprilTagCamera("RearCamera", Constants.REAR_APRILTAG_CAMERA_OFFSET);
Expand All @@ -110,7 +115,7 @@ public static double getRobotRoll() {
private RobotContainer() {
this.configureSubsystemDefaultCommands();
this.configureBindings();
this.configureAutonmousChooser();
this.configureAutonmous();
}

/** Configures the subsystem's default commands. */
Expand Down Expand Up @@ -220,13 +225,13 @@ private void configureOperatorStickBindings() {
/**
* Configures the autonomous chooser over Network Tables (e.g. Smart Dashboard).
*/
private void configureAutonmousChooser() {
private void configureAutonmous() {
registerNamedCommands();

HolonomicPathFollowerConfig pathFollowerConfig = new HolonomicPathFollowerConfig(
var pathFollowerConfig = new HolonomicPathFollowerConfig(
new PIDConstants(4.0, 0.0, 0.3),
new PIDConstants(1.8, 0.0, 0.8),
3.5,
SwerveDrive.kMaxSpeedMetersPerSecond,
Constants.DRIVE_BASE_RADIUS_METERS,
new ReplanningConfig(true, true));

Expand All @@ -241,14 +246,31 @@ private void configureAutonmousChooser() {

autoBuilder = new AutoBuilder();
autoChooser = AutoBuilder.buildAutoChooser();

SmartDashboard.putData("Auto Chooser", autoChooser);
SmartDashboard.putData("field", field);

configureFieldLogging();
configurePathPlannerLogging();
}

private void registerNamedCommands() {
NamedCommands.registerCommand("DeployIntake", new SetIntakeState(IntakeState.kDown));
NamedCommands.registerCommand("RetractIntake", new SetIntakeState(IntakeState.kUp));
NamedCommands.registerCommand("SetArmForScore", new SetArmState(ArmState.kShoot));
NamedCommands.registerCommand("SetArmAndShooterForIntake", new SetShooterState(ShooterState.kIntake).alongWith(new SetArmState(ArmState.kIntake)));
NamedCommands.registerCommand("AutoPickupNote", new AutoPickupNote().withTimeout(5));

// TODO: this command is needed since we cannot lower the arm until the stops pop out by lifting the arm weight off them
NamedCommands.registerCommand("LiftArmToDeployDemonHorns", new PrintCommand("!!! LiftArmToDeployDemonHorns not yet implemented !!!"));

// TODO: create a LaunchSpeaker for auto that does not use the joystick controls on the drive train
NamedCommands.registerCommand("LaunchSpeaker", new PrintCommand("!!! LaunchSpeaker not yet implemented !!!"));

// TODO: get heading adjustment using vision as pass along to DoubleSupplier, or make a separate command AlignToSpeaker
NamedCommands.registerCommand("AlignToSpeaker", new PrintCommand("!!! AlignToSpeaker not yet implemented !!!"));
}

private void configureFieldLogging() {
private void configurePathPlannerLogging() {
SmartDashboard.putData("field", field);

var logPPCurrentPoseX = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/current/x");
var logPPCurrentPoseY = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/current/y");
var logPPCurrentPoseTheta = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/current/theta");
Expand Down Expand Up @@ -283,21 +305,4 @@ private void configureFieldLogging() {
field.getObject("path").setPoses(poses);
});
}

private void registerNamedCommands() {
NamedCommands.registerCommand("DeployIntake", new SetIntakeState(IntakeState.kDown));
NamedCommands.registerCommand("RetractIntake", new SetIntakeState(IntakeState.kUp));
NamedCommands.registerCommand("SetArmForScore", new SetArmState(ArmState.kShoot));
NamedCommands.registerCommand("SetArmAndShooterForIntake", new SetShooterState(ShooterState.kIntake).alongWith(new SetArmState(ArmState.kIntake)));
NamedCommands.registerCommand("AutoPickupNote", new AutoPickupNote().withTimeout(5));

// TODO: this command is needed since we cannot lower the arm until the stops pop out by lifting the arm weight off them
NamedCommands.registerCommand("LiftArmToDeployDemonHorns", new PrintCommand("!!! LiftArmToDeployDemonHorns not yet implemented !!!"));

// TODO: create a LaunchSpeaker for auto that does not use the joystick controls on the drive train
NamedCommands.registerCommand("LaunchSpeaker", new PrintCommand("!!! LaunchSpeaker not yet implemented !!!"));

// TODO: get heading adjustment using vision as pass along to DoubleSupplier, or make a separate command AlignToSpeaker
NamedCommands.registerCommand("AlignToSpeaker", new PrintCommand("!!! AlignToSpeaker not yet implemented !!!"));
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/arm/SetArmState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public SetArmState(ArmState armState) {
// Called when the command is initially scheduled.
@Override
public void initialize() {
RobotContainer.arm.setState(armState);;
RobotContainer.arm.setState(armState);
}

// Called every time the scheduler runs while the command is scheduled.
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/commands/drive/RotateToAngle.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,9 @@
import frc.robot.RobotContainer;

public class RotateToAngle extends Command {
private static final double kErrorInDegrees = 3;
private Timer timer = new Timer();
private static final double kErrorToleranceInDegrees = 3;
private static final double kTimeAtSetpointForCompletionInSeconds = 0.20;
private final Timer timer = new Timer();

private final DoubleSupplier angle;

Expand Down Expand Up @@ -46,13 +47,13 @@ public void end(boolean interrupted) {
// Returns true when the command should end.
@Override
public boolean isFinished() {
var atSetpoint = RobotContainer.swerveDrive.atSetpoint(kErrorInDegrees);
var atSetpoint = RobotContainer.swerveDrive.atSetpoint(kErrorToleranceInDegrees);
if (atSetpoint) {
timer.start();
} else {
timer.reset();
}

return atSetpoint && timer.hasElapsed(0.20);
return atSetpoint && timer.hasElapsed(kTimeAtSetpointForCompletionInSeconds);
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/shooter/RampShooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,9 @@
import frc.robot.RobotContainer;

public class RampShooter extends Command {
private static final double kVelocityTolerance = 1.0;
private static final double kTimeAtSetpointForCompletionInSeconds = 0.15;
private final Timer timer = new Timer();
private final double kTimeAtSetpointForCompletionInSeconds = 0.15;
private final double kVelocityTolerance = 1.0;

private final DoubleSupplier velocity;

Expand Down
67 changes: 28 additions & 39 deletions src/main/java/frc/robot/subsystems/drive/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import org.photonvision.targeting.PhotonTrackedTarget;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
Expand All @@ -24,6 +25,8 @@
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.datalog.DoubleLogEntry;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.RobotState;
Expand Down Expand Up @@ -161,10 +164,6 @@ public Rotation2d getAngle() {
return Rotation2d.fromDegrees((-RobotContainer.navx.getAngle() + offset) % 360);
}

public double getYaw() {
return normalizeAngle(-RobotContainer.navx.getYaw() + offset);
}

public void setOffset(double offset) {
SwerveDrive.offset = offset;
}
Expand Down Expand Up @@ -271,57 +270,51 @@ public Pose2d getPose() {

/**
* Updates the field relative position of the robot.
*
*/
public void updateOdometry() {
// traction
var modulePositions = getSwerveModulePositions();
poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngle(), modulePositions);

// vision
updateOdometryUsingFrontCamera();
updateOdometryUsingRearCamera();
}

private void updateOdometryUsingFrontCamera() {
Optional<EstimatedRobotPose> estimatedFrontPose = RobotContainer.frontAprilTagCamera.getEstimatedGlobalPose();
if (estimatedFrontPose.isPresent()) {
List<PhotonTrackedTarget> targetsUsed = estimatedFrontPose.get().targetsUsed;

poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.3));

boolean goodMeasurements = true;
for (PhotonTrackedTarget t : targetsUsed) {
if (t.getPoseAmbiguity() < 0.2) {
goodMeasurements = false;
break;
}
}

if (goodMeasurements) {
poseEstimator.addVisionMeasurement(estimatedFrontPose.get().estimatedPose.toPose2d(), estimatedFrontPose.get().timestampSeconds);
}
var stdDevs = VecBuilder.fill(0.9, 0.9, 0.3);
var ambiguity = 0.2;
updateOdometryUsingVisionMeasurement(estimatedFrontPose.get(), stdDevs, ambiguity);
}
}

private void updateOdometryUsingRearCamera() {
Optional<EstimatedRobotPose> estimatedRearPose = RobotContainer.rearAprilTagCamera.getEstimatedGlobalPose();
if (estimatedRearPose.isPresent()) {
List<PhotonTrackedTarget> targetsUsed = estimatedRearPose.get().targetsUsed;
var stdDevs = VecBuilder.fill(0.9, 0.9, 0.3);
var ambiguity = 0.2;
updateOdometryUsingVisionMeasurement(estimatedRearPose.get(), stdDevs, ambiguity);
}
}

poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.9, 0.9, 0.3));
private void updateOdometryUsingVisionMeasurement(EstimatedRobotPose ePose, Matrix<N3, N1> visionMeasurementStdDevs, double ambiguity) {
List<PhotonTrackedTarget> targetsUsed = ePose.targetsUsed;
poseEstimator.setVisionMeasurementStdDevs(visionMeasurementStdDevs);

boolean goodMeasurements = true;
for (PhotonTrackedTarget t : targetsUsed) {
if (t.getPoseAmbiguity() < 0.2) {
goodMeasurements = false;
break;
}
boolean goodMeasurements = true;
for (PhotonTrackedTarget t : targetsUsed) {
if (t.getPoseAmbiguity() < ambiguity) {
goodMeasurements = false;
break;
}
}

if (goodMeasurements) {
poseEstimator.addVisionMeasurement(estimatedRearPose.get().estimatedPose.toPose2d(), estimatedRearPose.get().timestampSeconds);
}
if (goodMeasurements) {
poseEstimator.addVisionMeasurement(ePose.estimatedPose.toPose2d(), ePose.timestampSeconds);
}
}


public ChassisSpeeds getChassisSpeeds() {
return this.kinematics.toChassisSpeeds(getSwerveModuleStates());
Expand Down Expand Up @@ -352,14 +345,16 @@ public void stop() {
rearLeft.stop();
}

/**
* Sets swerve modules to be all angled - useful when trying to stay still on an incline.
*/
public void lock() {
frontRight.setDesiredState(0, 45);
frontLeft.setDesiredState(0, -45);
rearRight.setDesiredState(0, -45);
rearLeft.setDesiredState(0, 45);
}


public void resetNavx() {
RobotContainer.navx.reset();
}
Expand All @@ -368,10 +363,6 @@ public void resetPid() {
anglePid.reset();
}

public boolean facingInfield() {
return (180 - Math.abs(getYaw())) < 10 || Math.abs(getYaw()) < 10;
}

private static double normalizeAngle(double angle) {
if (angle > 0) {
angle %= 360;
Expand Down Expand Up @@ -405,7 +396,6 @@ public void periodic() {
SmartDashboard.putNumber("Navx-Roll", roll);
SmartDashboard.putNumber("Navx-Pitch", pitch);


if (RobotState.isEnabled()) {
var chassis = getChassisSpeeds();
var speed = Math.hypot(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond);
Expand All @@ -420,6 +410,5 @@ public void periodic() {
LogNavxRoll.append(roll);
LogNavxPitch.append(pitch);
}

}
}

0 comments on commit cde4c36

Please sign in to comment.