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

Commit

Permalink
spelling corrections and remove unused imports
Browse files Browse the repository at this point in the history
  • Loading branch information
Jacob1010-h committed Nov 23, 2023
1 parent 1006c31 commit 30e2a84
Show file tree
Hide file tree
Showing 5 changed files with 18 additions and 23 deletions.
19 changes: 10 additions & 9 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@
import frc.robot.subsystems.ArduinoController;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.Claw;
import frc.robot.subsystems.PhotonCameraUtil;
import frc.robot.subsystems.Swerve;
import frc.robot.subsystems.camera.LimelightCamera;
import frc.robot.util.PatriBoxController;
import frc.robot.util.Constants.AutoConstants;
import frc.robot.util.Constants.DriveConstants;
Expand All @@ -40,7 +40,7 @@ public class RobotContainer {
private final Swerve swerve;
private final Arm arm;
private final Claw claw;
private final PhotonCameraUtil photonVision;
private final LimelightCamera limelight;
private final AutoAlignment autoAlignment;
private final ArduinoController arduinoController;
@SuppressWarnings("unused")
Expand All @@ -59,9 +59,9 @@ public RobotContainer() {
swerve = new Swerve();
arm = new Arm();
claw = new Claw();
photonVision = new PhotonCameraUtil();
limelight = new LimelightCamera();
autoPathStorage = new AutoPathStorage();
autoAlignment = new AutoAlignment(swerve, photonVision);
autoAlignment = new AutoAlignment(swerve, limelight);
arduinoController = new ArduinoController();

swerve.setDefaultCommand(new Drive(
Expand All @@ -74,7 +74,8 @@ public RobotContainer() {
() -> (driver.y().getAsBoolean() && FieldConstants.ALLIANCE == Alliance.Blue)
));

photonVision.setDefaultCommand(autoAlignment.calibrateOdometry());
limelight.setDefaultCommand(
autoAlignment.calibrateOdometry());

incinerateMotors();
configureButtonBindings();
Expand Down Expand Up @@ -124,7 +125,7 @@ private void configureButtonBindings() {
setDriveSpeed(FieldConstants.ALIGNMENT_SPEED),
autoAlignment.setNearestValuesCommand(),
swerve.resetHDC(),
swerve.setAlignemntSpeed(),
swerve.setAlignmentSpeed(),
// Run a method to get our desired speeds autonomously
// combine that with the driver axis X
// then run a swerve drive command with that
Expand Down Expand Up @@ -202,7 +203,7 @@ private void configureButtonBindings() {
operator.leftBumper().whileTrue(claw.setDesiredSpeedCommand(() -> PlacementConstants.CLAW_STOPPED_SPEED));

operator.rightBumper().or(operator.rightStick()).and(arm::getAtPlacementPosition)
.onTrue(claw.outTakeforXSeconds(() -> PlacementConstants.CONE_MODE ? 0.1 : 0.3)
.onTrue(claw.outTakeForXSeconds(() -> PlacementConstants.CONE_MODE ? 0.1 : 0.3)
.alongWith(arduinoController.setLEDStateCommand(() -> LEDConstants.BELLY_PAN_BLACK))
.alongWith(arduinoController.setLEDStateCommand(
() -> PlacementConstants.CONE_MODE ?
Expand Down Expand Up @@ -236,7 +237,7 @@ private void configureButtonBindings() {
PlacementConstants.CONE_MODE
? LEDConstants.BELLY_PAN_YELLOW
: LEDConstants.BELLY_PAN_PURPLE))
.and(claw::justAquiredGameElement)
.and(claw::justAcquiredGameElement)
.onTrue(arduinoController.setLEDStateCommand(() ->
PlacementConstants.CONE_MODE
? LEDConstants.BELLY_PAN_YELLOW_BLINK
Expand Down Expand Up @@ -270,7 +271,7 @@ public void generateEventMap() {

AutoConstants.EVENT_MAP.put("PlacePiece",
arm.finishPlacmentCommand()
.andThen(claw.outTakeforXSeconds(() -> 0.15))
.andThen(claw.outTakeForXSeconds(() -> 0.15))
.andThen(arm.getAutoStowCommand())
);

Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/commands/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,8 @@
import java.util.function.Supplier;

import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Swerve;
import frc.robot.util.Constants.FieldConstants;

public class Drive extends CommandBase {

Expand Down
3 changes: 0 additions & 3 deletions src/main/java/frc/robot/commands/auto/AutoPathStorage.java
Original file line number Diff line number Diff line change
@@ -1,10 +1,7 @@
package frc.robot.commands.auto;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPoint;

import frc.robot.DriverUI;
import frc.robot.util.Constants.AutoConstants;

Expand Down
13 changes: 6 additions & 7 deletions src/main/java/frc/robot/subsystems/Claw.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.DriverUI;
import frc.robot.util.Constants.ClawConstants;
import frc.robot.util.Constants.FieldConstants;
Expand Down Expand Up @@ -53,17 +52,17 @@ public void periodic() {
* Output generally is -0.25 < appliedOutput < 0
* When grabbygrab
* Current is generally 15 < current < 30
* Which is a big range unfortunatly.
* I am not sure where the curernt > 30 came from
* Which is a big range unfortunately.
* I am not sure where the current > 30 came from
* I am having a hard time getting that read again on the graph anymore
* Let's just go with > 15 for now
*
* Laslty, desiredSpeed is ALWAYS > 0.45 when we are intaking
* Lastly, desiredSpeed is ALWAYS > 0.45 when we are intaking
* Which is a good indicator of finding if the claw is just getting up
* to speed or if it is stalled
*
* The problem with having a boolean this sophisticated
* Is that there are too many independant variables to check
* Is that there are too many independent variables to check
* And if a single one is off, the boolean will be false
*
* Idea: use a startedIntakingTimestamp to check
Expand Down Expand Up @@ -156,7 +155,7 @@ public double getDesiredSpeed() {
return this.desiredSpeed;
}

public Command outTakeforXSeconds(DoubleSupplier X) {
public Command outTakeForXSeconds(DoubleSupplier X) {
return runOnce(() -> {
setDesiredSpeed(PlacementConstants.CLAW_OUTTAKE_SPEED_CONE);
}).andThen(Commands.waitSeconds(X.getAsDouble())
Expand All @@ -173,7 +172,7 @@ public boolean hasGameElement() {
return hasGameElement;
}

public boolean justAquiredGameElement() {
public boolean justAcquiredGameElement() {
return !hasGameElementOneLoopBehind && hasGameElement;
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/subsystems/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -328,7 +328,7 @@ public void driveInAuto(ChassisSpeeds speeds) {
*/
/**
* Credit: WPIlib 2024
* Discretizes a continuous-time chassis speed.
* Discretize a continuous-time chassis speed.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
Expand Down Expand Up @@ -491,7 +491,7 @@ public void setSpeedMultiplier(double speedMultiplier) {
this.speedMultiplier = speedMultiplier;
}

public Command setAlignemntSpeed() {
public Command setAlignmentSpeed() {
return runOnce(() -> {
DriveConstants.MAX_SPEED_METERS_PER_SECOND = FieldConstants.ALIGNMENT_SPEED;
});
Expand Down

0 comments on commit 30e2a84

Please sign in to comment.