From 4ac2442f1ed7547a75c846ac8bc0450ccf058203 Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Thu, 26 Oct 2023 08:55:34 -0500 Subject: [PATCH 1/9] start big ol cleanup of RobotContainer - quinn --- src/main/java/frc/robot/RobotContainer.java | 122 ++++++++++---------- 1 file changed, 60 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index abfa568..a5f8f04 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,7 +55,7 @@ import frc.robot.subsystems.arm.ArmShoulder; import frc.robot.subsystems.arm.CompilationArm; -@SuppressWarnings("unused") +//@SuppressWarnings("unused") let me see unused imports >:( /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -63,46 +63,58 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - /* Messy, ugly commands and subsystems section - * TODO: organize later + + /* =============================================================== + * SUBSYSTEMS + * =============================================================== */ - /* Autotasks are mostly commented out in here for now because I don't care that they exist */ + + /* LIMELIGHT */ private final Limelight limelightBack = new Limelight("limelight-back"); private final Limelight limelightFront = new Limelight("limelight-front"); private final LimelightManager limelightManager = new LimelightManager(limelightBack, limelightFront); - private final Drive driveSubsystem = new Drive(limelightManager); - /* BIG CHUNGUS ARM CODE */ - //private final I2CManager I2CManager = new I2CManager(); - //private final ArmBase armBase = new ArmBase(); - //private final ArmShoulder armShoulder = new ArmShoulder(); - //private final ArmExtender armExtender = new ArmExtender(I2CManager); - //private final ArmClaw armClaw = new ArmClaw(); + /* CAMERA */ private final Camera aimCamera = new Camera(Constants.Camera.PORT_CAMERA_AIM); - //private final CompilationArm compilationArm = - // new CompilationArm(armBase, armClaw, armExtender, armShoulder); - //private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); - //private RetractArm retractArmCommand = new RetractArm(armExtender); - private final Plunger plunger = new Plunger(); - private final DeployPlunger deployPlunger = new DeployPlunger(plunger); + /* DRIVE */ + private final Drive driveSubsystem = new Drive(limelightManager); + /* ARM */ + private final I2CManager I2CManager = new I2CManager(); + private final ArmBase armBase = new ArmBase(); + private final ArmShoulder armShoulder = new ArmShoulder(); + private final ArmExtender armExtender = new ArmExtender(I2CManager); + private final ArmClaw armClaw = new ArmClaw(); + private final CompilationArm compilationArm = + new CompilationArm(armBase, armClaw, armExtender, armShoulder); + private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); + private RetractArm retractArmCommand = new RetractArm(armExtender); + + /* =============================================================== + * COMMANDS + * =============================================================== + */ + + /* AUTO COMMANDS */ private final StartLeavingCommunity startLeavingCommunity = new StartLeavingCommunity(driveSubsystem); private final StopLeavingCommunity stopLeavingCommunity = new StopLeavingCommunity(driveSubsystem); - private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); - private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); + /* CAMERA COMMAND */ private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera); + /* BALANCE COMMAND */ private final Balance balance = new Balance(driveSubsystem); + /* DRIVE COMMANDS*/ private final DriveArcade driveRO = new DriveArcade(driveSubsystem); private final DriveTank driveFO = new DriveTank(driveSubsystem); - //private final Command openClaw = armClaw.manualOpenClawCommand(); - //private final Command closeClaw = armClaw.manualCloseClawCommand(); - //private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); - //private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); - //private final ExtendArm extend = new ExtendArm(armExtender); - //private final RetractArm retract = new RetractArm(armExtender); - //private final TaskScheduler taskScheduler = new TaskScheduler(); + private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); + private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); + /* ARM COMMANDS */ + private final Command openClaw = armClaw.manualOpenClawCommand(); + private final Command closeClaw = armClaw.manualCloseClawCommand(); + private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); + private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); + private final ExtendArm extend = new ExtendArm(armExtender); + private final RetractArm retract = new RetractArm(armExtender); + /* TRAJECTORY COMMAND */ private final TrackTrajectory demoTrajectoryTrackingCommand = new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); - private final TestDrivePID testDrivePID = new TestDrivePID(driveSubsystem); - + // Replace with CommandPS4Controller or CommandJoystick if needed public static CommandXboxController controller = new CommandXboxController(OperatorConstants.CONTROLLER_NUMBER); @@ -116,9 +128,9 @@ public RobotContainer() { /* TODO: add ResetDrivePose into auto*/ autoChooser = new SendableChooser<>(); - //autoChooser.setDefaultOption("Fling cube + mobility bonus", - // Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); - //autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); + autoChooser.setDefaultOption("Fling cube + mobility bonus", + Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); + autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); SmartDashboard.putData("Choose auto", autoChooser); @@ -127,9 +139,7 @@ public RobotContainer() { } public Pose containerGetArmPose() { - - //return compilationArm.getArmPose(); - return new Pose(); + return compilationArm.getArmPose(); } /** @@ -143,31 +153,22 @@ public Pose containerGetArmPose() { */ private void configureBindings() { - // controller.y().whileTrue(deployPlunger); - controller.rightBumper().whileTrue(balance); controller.y().toggleOnTrue(driveFO); - controller.b().whileTrue(deployPlunger); - controller.x().whileTrue(demoTrajectoryTrackingCommand); - - controller.a().whileTrue(testDrivePID); - //joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase, - //armShoulder, armExtender, compilationArm)); + joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase, + armShoulder, armExtender, compilationArm)); + joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); + joystick.povUp().whileTrue(extend); + joystick.povDown().whileTrue(retract); - //joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); - - //joystick.povUp().whileTrue(extend); - //joystick.povDown().whileTrue(retract); - - //joystick.button(11).whileTrue(openClaw); + joystick.button(11).whileTrue(openClaw); /* TODO: get better buttons with Gabriel */ - /* Or just get color sensor working */ - //joystick.trigger().whileTrue(closeClaw); - //joystick.button(2).whileTrue(closeClawCube); + joystick.trigger().whileTrue(closeClaw); + joystick.button(2).whileTrue(closeClawCube); /*joystick .button(2) @@ -235,11 +236,10 @@ private void configureBindings() { compilationArm)); joystick .button(1) - .onTrue(armClaw.manualCloseClawCommand());*/ - } - - private void verifyAutoTasks() { + .onTrue(armClaw.manualCloseClawCommand()); + */ } + /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -252,15 +252,13 @@ public Command getAutonomousCommand() { } public Command getResetCommand() { - //return MoveToPose.retractingMoveToPose( - // Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm) - // .alongWith(armClaw.openClawCommand()); - return new InstantCommand(); + return MoveToPose.retractingMoveToPose( + Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm) + .alongWith(armClaw.openClawCommand()); } public Command getRetractCommand() { - //return retractArmCommand; - return new InstantCommand(); + return retractArmCommand; } public Command getDisplayAimVideoCommand() { From da0f43fb69f82955b5b7cde49cc84aabc4d57a0a Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Thu, 26 Oct 2023 09:03:59 -0500 Subject: [PATCH 2/9] prune unused files (bye autotasks) :( that hurt -quinn --- .vscode/settings.json | 1 + src/main/java/frc/robot/RobotContainer.java | 13 +- .../frc/robot/commands/DeployPlunger.java | 47 ---- .../robot/commands/autotasks/AutoTask.java | 230 ------------------ .../java/frc/robot/subsystems/Plunger.java | 30 --- .../frc/robot/subsystems/TaskScheduler.java | 112 --------- 6 files changed, 5 insertions(+), 428 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DeployPlunger.java delete mode 100644 src/main/java/frc/robot/commands/autotasks/AutoTask.java delete mode 100644 src/main/java/frc/robot/subsystems/Plunger.java delete mode 100644 src/main/java/frc/robot/subsystems/TaskScheduler.java diff --git a/.vscode/settings.json b/.vscode/settings.json index f579099..fe6a736 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -30,4 +30,5 @@ "editor.detectIndentation": false, "editor.tabSize": 2, "editor.insertSpaces": true, + "java.format.settings.url": "https://raw.githubusercontent.com/google/styleguide/gh-pages/eclipse-java-google-style.xmleclipse-formatter.xml", } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5f8f04..a5bb619 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,31 +24,26 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; import frc.robot.commands.Balance; -import frc.robot.subsystems.Drive; -import frc.robot.subsystems.Limelight; -import frc.robot.subsystems.LimelightManager; -import frc.robot.subsystems.TaskScheduler; import frc.robot.commands.CloseClawCone; import frc.robot.commands.CloseClawCube; -import frc.robot.commands.DeployPlunger; -import frc.robot.commands.DriveTank; +import frc.robot.commands.DriveArcade; import frc.robot.commands.DriveForwards; import frc.robot.commands.DriveReverse; -import frc.robot.commands.DriveArcade; +import frc.robot.commands.DriveTank; import frc.robot.commands.ExtendArm; import frc.robot.commands.ManualArm; import frc.robot.commands.ResetDrivePose; import frc.robot.commands.RetractArm; import frc.robot.commands.StartLeavingCommunity; import frc.robot.commands.StopLeavingCommunity; -import frc.robot.commands.TestDrivePID; import frc.robot.commands.TrackTrajectory; import frc.robot.commands.moveToPose.MoveToPose; import frc.robot.positioning.Pose; import frc.robot.subsystems.Camera; import frc.robot.subsystems.Drive; import frc.robot.subsystems.I2CManager; -import frc.robot.subsystems.Plunger; +import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.LimelightManager; import frc.robot.subsystems.arm.ArmBase; import frc.robot.subsystems.arm.ArmClaw; import frc.robot.subsystems.arm.ArmExtender; diff --git a/src/main/java/frc/robot/commands/DeployPlunger.java b/src/main/java/frc/robot/commands/DeployPlunger.java deleted file mode 100644 index 7be933a..0000000 --- a/src/main/java/frc/robot/commands/DeployPlunger.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Plunger; - -/** Command to deploy Plunger */ -public class DeployPlunger extends CommandBase { - private Plunger plunger; - /** - * Creates instance of DeployPlunger - * - * @param plunger takes plunger subsystem for dependency injection - */ - public DeployPlunger(Plunger plunger) { - this.plunger = plunger; - addRequirements(plunger); - } - - /** Called when the command starts/button is presse - starts the motor */ - @Override - public void initialize() { - plunger.deployPlunger(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - /** called when the command end/button is realeased - stops the motor from spinning */ - @Override - public void end(boolean interrupted) { - plunger.stop(); - } - - /** - * Returns true when the command should end. Will never return true due to because it's designed - * to be used with whenHeld - */ - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/autotasks/AutoTask.java b/src/main/java/frc/robot/commands/autotasks/AutoTask.java deleted file mode 100644 index 5b1feca..0000000 --- a/src/main/java/frc/robot/commands/autotasks/AutoTask.java +++ /dev/null @@ -1,230 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.autotasks; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.positioning.Pose; -// TODO: Add fallback commands -import java.util.ArrayList; -import java.util.Stack; - -/** Base class for autotasks. If you want to create an autotask extend this class. */ -public abstract class AutoTask { - private boolean initialized = false; - private boolean arrived = false; - private boolean ended = false; - private boolean verified = false; - private Pose taskPos; - private ArrayList commands = new ArrayList(); - private Stack initCommands = new Stack(); - private Stack arrivedCommands = new Stack(); - private CommandBase runningCommand; - - /** - * Creates a new AutoTask. Dont create instances of commands. Each command should be a parameter - * instead of created - */ - public AutoTask() {} - - /** - * Ran when the command is started. Allow basic movements before the PositionSystem moves the - * robot. - * - *

Make sure to put the {@link #setTaskPosition(Position) setTaskPosition} method in init to - * set where the robot will go. - * - *

Not running taskPosition will cause a NullPointerException. - */ - public abstract void initTask(); - - /** - * Checks if the initTask method is finished. Return true when the method finishes. - * - * @return the state of the initTask method. - */ - public boolean initFinished() { - if (initCommands.empty() & runningCommand.isFinished()) { - return true; - } else { - return false; - } - } - - /** Ran when the AutoTask arrives at the defined position. */ - public abstract void arrived(); - - /** - * Checks if the Arrived method has finished and allows the next queued command to run. - * - * @return the state of the arrived method - */ - public boolean isFinished() { - if (arrivedCommands.empty() & runningCommand.isFinished()) { - return true; - } else { - return false; - } - } - - /** - * Ran if the bot cant get to the position it needs. - * - * @param position The bots current position. - */ - public abstract void fallback(Pose position); - - /** Returns the acive command. */ - public CommandBase getActiveCommand() { - return runningCommand; - } - - /** - * Like periodic but gives position arg - * - * @param position The current position of the robot - */ - public void updateTask(Pose position) { - updateInit(); - updateArrived(); - update(position); - } - - /* - * Update the init sequence of the task. If the task is initialized the command - * is bypassed. - */ - private void updateInit() { - /* Checks if the task has finished init sequence */ - if (!initialized) { - /* - * If task is not initalised, queue command if current running one is finished - */ - if (runningCommand.isFinished() & !initCommands.isEmpty()) { - runningCommand = initCommands.pop(); - /* - * If currently running command is finished and there are no more init - * Commands, initalize - */ - } else if (initCommands.isEmpty() & runningCommand.isFinished()) { - initialized = true; - } - /* Prevents current command from getting ran multiple times without intention */ - if (!runningCommand.isScheduled() & !runningCommand.isFinished()) { - runningCommand.schedule(); - } - } - } - - /* - * Update the arrived sequence of the task. If the task hasent arrived at the - * desination the command is bypassed. - */ - private void updateArrived() { - /* Checks if the task has finished arrived sequence */ - if (!arrived) { - /* - * If task is not initalised, queue command if current running one is finished - */ - if (runningCommand.isFinished() & !arrivedCommands.isEmpty()) { - runningCommand = arrivedCommands.pop(); - /* - * If currently running command is finished and there are no more init - * Commands, end the command - */ - } else if (arrivedCommands.isEmpty() & runningCommand.isFinished()) { - ended = true; - } - /* Prevents current command from getting ran multiple times without intention */ - if (!runningCommand.isScheduled() & !runningCommand.isFinished()) { - runningCommand.schedule(); - } - } - } - - /** - * Returns if the task has ended or not. - * - * @return The status of the command - */ - public boolean ended() { - return ended; - } - - /** - * Use instead of execute. Functions as execute but with a position arguemnt. - * - * @param position the current position of the robot when update is ran. - */ - protected abstract void update(Pose position); - - /** - * Sets the position the bot will go to for the task. - * - *

Not running this in {@link #initTask initTask} will cause a - * NullPointerException - * - * @param position The position the bot will go to for the AutoTask - */ - protected void setTaskPosition(Pose position) { - taskPos = position; - } - - /** Ran if the task needs to be ended. */ - public void end() { - if (!runningCommand.isFinished()) { - runningCommand.end(true); - } - } - - /** - * Adds command requirements for TaskScheduler (such as ending commands) - * - * @param command The command/s to add - */ - private void addCommandRequirement(CommandBase... command) { - for (CommandBase cb : command) { - commands.add(cb); - } - } - - /** - * Sets the command to run on autotask init. - * - * @param command The command to run. - */ - protected void addInitCommand(CommandBase command) { - addCommandRequirement(command); - initCommands.push(command); - } - - /** - * Sets the command to run on autotask arrival. - * - * @param command The command to run. - */ - protected void addArrivedCommand(CommandBase command) { - addCommandRequirement(command); - arrivedCommands.push(command); - } - - /** Runs checks on the autotasks to make sure the tasks are valid */ - public void verify() { - System.out.println(); - System.out.println("================================================================"); - System.out.println("Checking if taskPosition was ran in " + this.getClass().getName()); - if (taskPos == null) { // checks if taskpos was instantiated and if not throw an error - System.out.println( - "Taskpos was not ran. In order for a AutoTask to be valid taskpos HAS to be ran in the" - + " constructor"); - throw new UnsupportedOperationException( - "Verification failed because taskpos was not ran in the constructor."); - - } else { - verified = true; - System.out.println("Taskpos was run. Task verified."); - } - System.out.println("================================================================"); - } -} diff --git a/src/main/java/frc/robot/subsystems/Plunger.java b/src/main/java/frc/robot/subsystems/Plunger.java deleted file mode 100644 index b3ccb48..0000000 --- a/src/main/java/frc/robot/subsystems/Plunger.java +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class Plunger extends SubsystemBase { - private Spark plunger; - /** Creates a new Plunger. */ - public Plunger() { - this.plunger = new Spark(Constants.Plunger.ID_PWMMOTOR_PLUNGER); - } - - public void deployPlunger() { - plunger.set(Constants.Plunger.SPEED_PLUNGER_DEPLOY); - } - - public void stop() { - plunger.set(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/TaskScheduler.java b/src/main/java/frc/robot/subsystems/TaskScheduler.java deleted file mode 100644 index faab819..0000000 --- a/src/main/java/frc/robot/subsystems/TaskScheduler.java +++ /dev/null @@ -1,112 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.autotasks.AutoTask; -import frc.robot.positioning.Pose; -import java.util.Stack; - -// TODO: autogenerated tests for each task -public class TaskScheduler extends SubsystemBase { - /** Creates a new TaskController. */ - private Stack taskqueue; - - private AutoTask currentTask; - private Pose botPosition; - - public TaskScheduler() {} - - @Override - public void periodic() { - /* - * If the current task has ended it does not update it. - * Then grabs the next task if the current one has ended. - */ - if (!currentTask.ended()) { - currentTask.updateTask(botPosition); - } else { - nextTask(); - } - } - - /** - * Adds a task to the end of the task queue. - * - * @param task The autotask to queue - */ - public void queueTask(AutoTask task) { - taskqueue.push(task); - } - - /** - * Cancels the current task to run another one. - * - * @param task The task to run instantly - */ - public void runTaskNow(AutoTask task) { - stopTask(); - currentTask = task; - initTask(); - } - - /** - * Removes a task from the task queue. - * - *

If you are trying to skip the current running task, use {@link #skipTask()} instead. - * - * @throws NullPointerException if task is not found in the queue. - * @param task The task to remove from the taskqueue - */ - public void removeTask(AutoTask task) throws NullPointerException { - if (taskqueue.contains(task)) { - taskqueue.remove(task); - } else { - throw new NullPointerException("Cant find task in queue"); - } - } - - /** Skips the currently scheduled class. */ - public void skipTask() { - stopTask(); - nextTask(); - } - - /** - * Clears the task queue. If there are any tasks queued they will be removed. - * - *

Doesnt stop the current task (if one is running). - */ - public void clearQueue() { - taskqueue.clear(); - } - - /** Gets the next queued task. */ - private void nextTask() { - if (!taskqueue.isEmpty()) { - currentTask = taskqueue.pop(); - initTask(); - } - } - - /** Stops the current running task. Will not pop the next command. */ - private void stopTask() { - currentTask.end(); - } - - /** Inits a command. */ - private void initTask() { - currentTask.initTask(); - } - - /** - * Sets the current bot position. - * - * @param position - */ - public void setBotPosition(Pose position) { - this.botPosition = position; - } -} From 8190154ed3c23eb8c51b9f03b4dc312b5eaa87f3 Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Thu, 26 Oct 2023 09:06:58 -0500 Subject: [PATCH 3/9] remove deprecated method --- src/main/java/frc/robot/commands/DriveForwards.java | 2 +- src/main/java/frc/robot/commands/DriveReverse.java | 2 +- src/main/java/frc/robot/subsystems/Drive.java | 11 +---------- 3 files changed, 3 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveForwards.java b/src/main/java/frc/robot/commands/DriveForwards.java index 04ae803..b9d984e 100644 --- a/src/main/java/frc/robot/commands/DriveForwards.java +++ b/src/main/java/frc/robot/commands/DriveForwards.java @@ -26,7 +26,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drivetrain.moveSimple(0.5, 0.5); + drivetrain.moveTank(0.5, 0.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/DriveReverse.java b/src/main/java/frc/robot/commands/DriveReverse.java index ec2929c..bad8a6d 100644 --- a/src/main/java/frc/robot/commands/DriveReverse.java +++ b/src/main/java/frc/robot/commands/DriveReverse.java @@ -26,7 +26,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drivetrain.moveSimple(-0.5, -0.5); + drivetrain.moveTank(-0.5, -0.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 9e28615..d28b508 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -234,16 +234,7 @@ public void moveArcade(double x, double z) { public void moveTank(double left, double right) { drivetrain.tankDrive(left, right); } - - /** - * @deprecated Use {@link #moveTank} - */ - @Deprecated(forRemoval = true) - public void moveSimple(double leftSpeed, double rightSpeed) { - left.set(ControlMode.PercentOutput, leftSpeed); - right.set(ControlMode.PercentOutput, rightSpeed); - } - + /** * Sets the velocity setpoint of the left and right sides of the drivetrain. * From e0876a1e7029644c2d07ec31c0b39e857952b15c Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Thu, 26 Oct 2023 09:13:02 -0500 Subject: [PATCH 4/9] format, all! - quinn --- src/main/java/frc/robot/Constants.java | 39 ++++++-- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 53 +++++++---- src/main/java/frc/robot/commands/Autos.java | 20 +++-- src/main/java/frc/robot/commands/Balance.java | 14 ++- .../frc/robot/commands/CloseClawCone.java | 2 +- .../frc/robot/commands/CloseClawCube.java | 2 +- .../java/frc/robot/commands/ExtendArm.java | 3 +- .../frc/robot/commands/MoveBaseDegrees.java | 5 +- .../robot/commands/MoveShoulderDegrees.java | 5 +- .../java/frc/robot/commands/RetractArm.java | 3 +- .../java/frc/robot/commands/TestDrivePID.java | 4 +- .../commands/homingRoutine/HomeBase.java | 7 +- .../commands/homingRoutine/HomeShoulder.java | 7 +- .../commands/moveToPose/MoveBaseToPose.java | 4 +- .../moveToPose/MoveExtenderToPose.java | 4 +- .../moveToPose/MoveShoulderToPose.java | 4 +- .../java/frc/robot/positioning/AStar.java | 30 +++---- src/main/java/frc/robot/positioning/Path.java | 2 +- .../java/frc/robot/subsystems/Camera.java | 3 +- src/main/java/frc/robot/subsystems/Drive.java | 90 +++++++++++-------- .../java/frc/robot/subsystems/I2CManager.java | 6 +- .../java/frc/robot/subsystems/Limelight.java | 13 ++- .../robot/subsystems/LimelightManager.java | 14 +-- .../frc/robot/subsystems/arm/ArmBase.java | 6 +- .../frc/robot/subsystems/arm/ArmClaw.java | 7 +- .../frc/robot/subsystems/arm/ArmExtender.java | 4 +- .../frc/robot/subsystems/arm/ArmShoulder.java | 4 +- .../robot/subsystems/arm/CompilationArm.java | 8 +- .../java/frc/robot/subsystems/yourMom.java | 2 + 30 files changed, 221 insertions(+), 145 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fa3efa2..89f9271 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,11 +4,6 @@ package frc.robot; -import frc.robot.positioning.Pose; - -import java.util.ArrayList; -import java.util.List; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -16,6 +11,9 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.util.Units; +import frc.robot.positioning.Pose; +import java.util.ArrayList; +import java.util.List; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -35,8 +33,10 @@ public static class OperatorConstants { public static class RobotDimensions { /** The pose of the robot's center */ public static final Pose CENTER_POSE = new Pose(0, 0, 8.5); + /** The front/back length of the robot's frame in inches */ public static final double FRAME_LENGTH = 29.0; + /** The left/right width of the robot's frame in inches */ public static final double FRAME_WIDTH = 29.0; } @@ -74,7 +74,8 @@ public static class Drive { public static final double WHEEL_DIAMETER_INCHES = 8; // I THINK they're 8" public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(WHEEL_DIAMETER_INCHES); - public static final double TEST_PID_SETPOINT = 4096; // This will no longer work after I fix the set method + public static final double TEST_PID_SETPOINT = + 4096; // This will no longer work after I fix the set method public static class DrivePIDYAY { public static final double P = 0.13939; @@ -111,9 +112,12 @@ public static class Constraints { public static class Trajectories { private static final Pose2d startPose = new Pose2d(5.9, -2.8, new Rotation2d()); - private static final ArrayList interiorWaypoints = new ArrayList<>(List.of(new Translation2d(5.5, -2.9))); + private static final ArrayList interiorWaypoints = + new ArrayList<>(List.of(new Translation2d(5.5, -2.9))); private static final Pose2d endPose = new Pose2d(4.7, -2.3, new Rotation2d()); - public static final Trajectory DEMO_TRAJECTORY = TrajectoryGenerator.generateTrajectory(startPose, interiorWaypoints, endPose, new TrajectoryConfig(5.0, 3.0)); + public static final Trajectory DEMO_TRAJECTORY = + TrajectoryGenerator.generateTrajectory( + startPose, interiorWaypoints, endPose, new TrajectoryConfig(5.0, 3.0)); } } @@ -146,10 +150,13 @@ public static class DriveConstants { public static class Arm { /** The pose of the arm's base in robot oriented coordinates */ public static final Pose BASE_POSE = new Pose(0, 0, 8.5); + /** The distance between the floor and the arm base in inches */ public static final double BASE_DISTANCE_TO_FLOOR = 8.5; + /** Length in inches from the base of the arm to the shoulder joint */ public static final double BASE_TO_SHOULDER_LENGTH = 19; + /** * How high above the arm base should we keep the end effector to stop the arm from stabbing the * robot? This only matters inside the frame perimeter. Distance in inches. @@ -231,8 +238,10 @@ public static class I2C { public static class Limits { /** Maximum horizontal extension over the frame in inches */ public static final double MAX_FRAME_EXTENSION = 48.0; + /** Maximum extended robot height in inches */ public static final double MAX_EXTENDED_HEIGHT = 78.0; + /** Minimum distance from overextending we want to keep in inches */ public static final double OVEREXTENSION_DANGER_DISTANCE = 4.0; } @@ -245,10 +254,13 @@ public static class ConeNode { public static class Top { /** Height of the node in feet */ public static final double HEIGHT = 3.8333; + /** Horizontal distance from the bumper to the node in feet */ public static final double DISTANCE = 3.3125; + /** Height of the reflective tape off the ground in feet */ public static final double TAPE_HEIGHT = 3.4896; + /** The distance from the top of the tape to the top of the node */ public static final double ABOVE_TAPE = 0.0156; } @@ -256,10 +268,13 @@ public static class Top { public static class Middle { /** Height of the node in feet */ public static final double HEIGHT = 2.8333; + /** Horizontal distance from the bumper to the node in feet */ public static final double DISTANCE = 1.8958; + /** Height of the reflective tape off the ground in feet */ public static final double TAPE_HEIGHT = 1.8438; + /** The distance from the top of the tape to the top of the node */ public static final double ABOVE_TAPE = 0.6667; } @@ -268,6 +283,7 @@ public static class CubeNode { public static class Top { /** Height of the node in feet */ public static final double HEIGHT = 2.9583; + /** Horizontal distance from the bumper to the middle of the node in feet */ public static final double DISTANCE = 3.3125; } @@ -275,6 +291,7 @@ public static class Top { public static class Middle { /** Height of the node in feet */ public static final double HEIGHT = 1.9583; + /** Horizontal distance from the bumper to the middle of the node in feet */ public static final double DISTANCE = 1.8958; } @@ -286,14 +303,19 @@ public static class Objects { public static class Cone { /** Height in inches */ public static final double HEIGHT = 12.8125; + /** Width of the square base in inches */ public static final double WIDTH = 8.375; + /** Diameter of the bottom of the cone */ public static final double BOTTOM_DIAMETER = 6.625; + /** Diameter of top bottom of the cone */ public static final double TOP_DIAMETER = 1.75; + /** Average diameter of the cone's grabbable area */ public static final double AVG_DIAMETER = 4.1875; + /** The pressure reading required to securely hold the cone */ public static final double PRESSURE_TO_HOLD = -1; } @@ -301,6 +323,7 @@ public static class Cone { public static class Cube { /** Side length of a properly inflated cube in inches */ public static final double LENGTH = 9.5; + /** The pressure reading required to securely hold the cube */ public static final double PRESSURE_TO_HOLD = -1; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d8b3aba..f070516 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,4 +1,3 @@ - // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5bb619..148eaef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,7 +50,7 @@ import frc.robot.subsystems.arm.ArmShoulder; import frc.robot.subsystems.arm.CompilationArm; -//@SuppressWarnings("unused") let me see unused imports >:( +// @SuppressWarnings("unused") let me see unused imports >:( /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -67,7 +67,8 @@ public class RobotContainer { /* LIMELIGHT */ private final Limelight limelightBack = new Limelight("limelight-back"); private final Limelight limelightFront = new Limelight("limelight-front"); - private final LimelightManager limelightManager = new LimelightManager(limelightBack, limelightFront); + private final LimelightManager limelightManager = + new LimelightManager(limelightBack, limelightFront); /* CAMERA */ private final Camera aimCamera = new Camera(Constants.Camera.PORT_CAMERA_AIM); /* DRIVE */ @@ -81,7 +82,7 @@ public class RobotContainer { private final CompilationArm compilationArm = new CompilationArm(armBase, armClaw, armExtender, armShoulder); private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); - private RetractArm retractArmCommand = new RetractArm(armExtender); + private final RetractArm retractArmCommand = new RetractArm(armExtender); /* =============================================================== * COMMANDS @@ -89,10 +90,13 @@ public class RobotContainer { */ /* AUTO COMMANDS */ - private final StartLeavingCommunity startLeavingCommunity = new StartLeavingCommunity(driveSubsystem); - private final StopLeavingCommunity stopLeavingCommunity = new StopLeavingCommunity(driveSubsystem); + private final StartLeavingCommunity startLeavingCommunity = + new StartLeavingCommunity(driveSubsystem); + private final StopLeavingCommunity stopLeavingCommunity = + new StopLeavingCommunity(driveSubsystem); /* CAMERA COMMAND */ - private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera); + private final InstantCommand displayAimVideo = + new InstantCommand(aimCamera::startCamera, aimCamera); /* BALANCE COMMAND */ private final Balance balance = new Balance(driveSubsystem); /* DRIVE COMMANDS*/ @@ -101,15 +105,16 @@ public class RobotContainer { private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); /* ARM COMMANDS */ - private final Command openClaw = armClaw.manualOpenClawCommand(); + private final Command openClaw = armClaw.manualOpenClawCommand(); private final Command closeClaw = armClaw.manualCloseClawCommand(); private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); private final ExtendArm extend = new ExtendArm(armExtender); private final RetractArm retract = new RetractArm(armExtender); /* TRAJECTORY COMMAND */ - private final TrackTrajectory demoTrajectoryTrackingCommand = new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); - + private final TrackTrajectory demoTrajectoryTrackingCommand = + new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); + // Replace with CommandPS4Controller or CommandJoystick if needed public static CommandXboxController controller = new CommandXboxController(OperatorConstants.CONTROLLER_NUMBER); @@ -117,19 +122,27 @@ public class RobotContainer { public static CommandJoystick joystick = new CommandJoystick(OperatorConstants.JOYSTICK_NUMBER); private final SendableChooser autoChooser; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureBindings(); /* TODO: add ResetDrivePose into auto*/ autoChooser = new SendableChooser<>(); - autoChooser.setDefaultOption("Fling cube + mobility bonus", - Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); - autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); + autoChooser.setDefaultOption( + "Fling cube + mobility bonus", + Commands.sequence( + Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), + new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), + new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), + new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); + autoChooser.addOption( + "Home arm (DOES NOT MOVE)", + Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); SmartDashboard.putData("Choose auto", autoChooser); - //compilationArm.setDefaultCommand(manualArm); + // compilationArm.setDefaultCommand(manualArm); driveSubsystem.setDefaultCommand(driveRO); } @@ -154,9 +167,14 @@ private void configureBindings() { controller.x().whileTrue(demoTrajectoryTrackingCommand); - joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase, - armShoulder, armExtender, compilationArm)); - joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); + joystick + .button(9) + .whileTrue( + MoveToPose.extendingMoveToPose( + Constants.Arm.Poses.PICKUP, armBase, armShoulder, armExtender, compilationArm)); + joystick + .button(4) + .whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); joystick.povUp().whileTrue(extend); joystick.povDown().whileTrue(retract); @@ -234,7 +252,6 @@ private void configureBindings() { .onTrue(armClaw.manualCloseClawCommand()); */ } - /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -297,7 +314,7 @@ public static double getScaledControllerRightYAxis() { } public static double getJoystickXAxis() { - return joystick.getX()* -1.0; + return joystick.getX() * -1.0; } public static double getScaledJoystickXAxis() { diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 5b2cbdd..16425b7 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -23,14 +23,22 @@ public static CommandBase homingRoutine( ArmClaw armClawSubsystem, CompilationArm compilationArmSubsystem) { return Commands.sequence( - armExtenderSubsystem.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), - new HomeShoulder(armShoulderSubsystem, compilationArmSubsystem), - new HomeBase(armBaseSubsystem, compilationArmSubsystem), - armClawSubsystem.openClawCommand()).withTimeout(5.5); + armExtenderSubsystem.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), + new HomeShoulder(armShoulderSubsystem, compilationArmSubsystem), + new HomeBase(armBaseSubsystem, compilationArmSubsystem), + armClawSubsystem.openClawCommand()) + .withTimeout(5.5); } - public static CommandBase homingNoOpenClaw(ArmShoulder armShoulder, ArmBase armBase, ArmExtender armExtender, CompilationArm compilationArm) { - return Commands.sequence(armExtender.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), new HomeShoulder(armShoulder, compilationArm), new HomeBase(armBase, compilationArm)); + public static CommandBase homingNoOpenClaw( + ArmShoulder armShoulder, + ArmBase armBase, + ArmExtender armExtender, + CompilationArm compilationArm) { + return Commands.sequence( + armExtender.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), + new HomeShoulder(armShoulder, compilationArm), + new HomeBase(armBase, compilationArm)); } private Autos() { diff --git a/src/main/java/frc/robot/commands/Balance.java b/src/main/java/frc/robot/commands/Balance.java index 70c619b..abf111f 100644 --- a/src/main/java/frc/robot/commands/Balance.java +++ b/src/main/java/frc/robot/commands/Balance.java @@ -30,19 +30,15 @@ public void initialize() {} @Override public void execute() { double pitchAngleDegrees = drive.getPitch(); // forward back - + if (pitchAngleDegrees > BalanceConstants.OFF_ANGLE_THRESHOLD) { - this.forwards.execute(); + this.forwards.execute(); } else if (pitchAngleDegrees < -BalanceConstants.OFF_ANGLE_THRESHOLD) { - this.reverse.execute(); + this.reverse.execute(); } else { - this.reverse.end(true); - this.forwards.end(true); + this.reverse.end(true); + this.forwards.end(true); } - - - - } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/CloseClawCone.java b/src/main/java/frc/robot/commands/CloseClawCone.java index 303d1af..3a3a36d 100644 --- a/src/main/java/frc/robot/commands/CloseClawCone.java +++ b/src/main/java/frc/robot/commands/CloseClawCone.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.arm.ArmClaw; public class CloseClawCone extends CommandBase { - private ArmClaw armClaw; + private final ArmClaw armClaw; /** Creates a new CloseClawCone. */ public CloseClawCone(ArmClaw armClaw) { diff --git a/src/main/java/frc/robot/commands/CloseClawCube.java b/src/main/java/frc/robot/commands/CloseClawCube.java index 479697b..8d08bf4 100644 --- a/src/main/java/frc/robot/commands/CloseClawCube.java +++ b/src/main/java/frc/robot/commands/CloseClawCube.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.arm.ArmClaw; public class CloseClawCube extends CommandBase { - private ArmClaw armClaw; + private final ArmClaw armClaw; /** Creates a new CloseClawCube. */ public CloseClawCube(ArmClaw armClaw) { diff --git a/src/main/java/frc/robot/commands/ExtendArm.java b/src/main/java/frc/robot/commands/ExtendArm.java index 53b795d..f3fb5f8 100644 --- a/src/main/java/frc/robot/commands/ExtendArm.java +++ b/src/main/java/frc/robot/commands/ExtendArm.java @@ -8,7 +8,8 @@ import frc.robot.subsystems.arm.ArmExtender; public class ExtendArm extends CommandBase { - private ArmExtender armExtender; + private final ArmExtender armExtender; + /** Creates a new RetractArm. */ public ExtendArm(ArmExtender armExtender) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/MoveBaseDegrees.java b/src/main/java/frc/robot/commands/MoveBaseDegrees.java index 2c21078..ee0b509 100644 --- a/src/main/java/frc/robot/commands/MoveBaseDegrees.java +++ b/src/main/java/frc/robot/commands/MoveBaseDegrees.java @@ -9,8 +9,9 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveBaseDegrees extends CommandBase { - private int degrees; - private ArmBase armBase; + private final int degrees; + private final ArmBase armBase; + /** Creates a new MoveBaseDegrees. */ public MoveBaseDegrees(int degrees, ArmBase armBase, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/MoveShoulderDegrees.java b/src/main/java/frc/robot/commands/MoveShoulderDegrees.java index 25fa760..7adfb0a 100644 --- a/src/main/java/frc/robot/commands/MoveShoulderDegrees.java +++ b/src/main/java/frc/robot/commands/MoveShoulderDegrees.java @@ -9,8 +9,9 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveShoulderDegrees extends CommandBase { - private int degrees; - private ArmShoulder armShoulder; + private final int degrees; + private final ArmShoulder armShoulder; + /** Creates a new MoveBaseDegrees. */ public MoveShoulderDegrees(int degrees, ArmShoulder armShoulder, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/RetractArm.java b/src/main/java/frc/robot/commands/RetractArm.java index 093b883..1fecd16 100644 --- a/src/main/java/frc/robot/commands/RetractArm.java +++ b/src/main/java/frc/robot/commands/RetractArm.java @@ -8,7 +8,8 @@ import frc.robot.subsystems.arm.ArmExtender; public class RetractArm extends CommandBase { - private ArmExtender armExtender; + private final ArmExtender armExtender; + /** Creates a new RetractArm. */ public RetractArm(ArmExtender armExtender) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/TestDrivePID.java b/src/main/java/frc/robot/commands/TestDrivePID.java index 5463ec6..b06c9a2 100644 --- a/src/main/java/frc/robot/commands/TestDrivePID.java +++ b/src/main/java/frc/robot/commands/TestDrivePID.java @@ -5,11 +5,11 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drive; import frc.robot.Constants; +import frc.robot.subsystems.Drive; public class TestDrivePID extends CommandBase { - private Drive drive; + private final Drive drive; /** Creates a new TestDrivePID. */ public TestDrivePID(Drive drive) { diff --git a/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java b/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java index c6139c9..09ffe49 100644 --- a/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java +++ b/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java @@ -11,6 +11,7 @@ public class HomeBase extends CommandBase { private final ArmBase armBase; + /** Creates a new HomeShoulder. */ public HomeBase(ArmBase armShoulder, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. @@ -38,10 +39,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (armBase.baseLimitSwitch()) { - return true; - } else { - return false; - } + return armBase.baseLimitSwitch(); } } diff --git a/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java b/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java index 1dd48a7..cfc7aee 100644 --- a/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java +++ b/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java @@ -11,6 +11,7 @@ public class HomeShoulder extends CommandBase { private final ArmShoulder armShoulder; + /** Creates a new HomeShoulder. */ public HomeShoulder(ArmShoulder armShoulder, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. @@ -38,10 +39,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (armShoulder.shoulderLimitSwitch()) { - return true; - } else { - return false; - } + return armShoulder.shoulderLimitSwitch(); } } diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java index adfd1b1..cc08aa5 100644 --- a/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java +++ b/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveBaseToPose extends CommandBase { - private ArmBase armBase; - private Pose pose; + private final ArmBase armBase; + private final Pose pose; /** Creates a new moveBaseToPose. */ public MoveBaseToPose(Pose pose, ArmBase armBase, CompilationArm compilationArm) { diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java index 8ab0cea..36fe799 100644 --- a/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java +++ b/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveExtenderToPose extends CommandBase { - private ArmExtender armExtender; - private Pose pose; + private final ArmExtender armExtender; + private final Pose pose; /** Creates a new moveBaseToPose. */ public MoveExtenderToPose(Pose pose, ArmExtender armExtender, CompilationArm compilationArm) { diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java index 288a495..a16f2e1 100644 --- a/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java +++ b/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveShoulderToPose extends CommandBase { - private ArmShoulder armShoulder; - private Pose pose; + private final ArmShoulder armShoulder; + private final Pose pose; /** Creates a new moveBaseToPose. */ public MoveShoulderToPose(Pose pose, ArmShoulder armShoulder, CompilationArm compilationArm) { diff --git a/src/main/java/frc/robot/positioning/AStar.java b/src/main/java/frc/robot/positioning/AStar.java index 37d6ed4..47cdaf5 100644 --- a/src/main/java/frc/robot/positioning/AStar.java +++ b/src/main/java/frc/robot/positioning/AStar.java @@ -22,11 +22,11 @@ */ public class AStar { private static Node[][] cell; - private ArrayList pathList = new ArrayList<>(); - private ArrayList closedList = new ArrayList<>(); - private static boolean[][] grid = + private final ArrayList pathList = new ArrayList<>(); + private final ArrayList closedList = new ArrayList<>(); + private static final boolean[][] grid = new boolean[Constants.AStar.FIELD_X * 2][Constants.AStar.FIELD_Y * 2]; - private AtomicReference currPath = new AtomicReference(new Path()); + private final AtomicReference currPath = new AtomicReference(new Path()); private Thread genThread; public int startY; public int startX; @@ -100,7 +100,7 @@ public AtomicReference generateAStarPath() { ArrayList pathListInMeters = new ArrayList(); for (Node node : pathList) { - Double tempNode[] = {(double) node.getX() / 100, (double) node.getY() / 100}; + Double[] tempNode = {(double) node.getX() / 100, (double) node.getY() / 100}; pathListInMeters.add(tempNode); } @@ -124,15 +124,15 @@ public AtomicReference generateAStarPath() { * @param d Cost between 2 cells located Diagonally next to each other */ private ArrayList generateHValue( - boolean matrix[][], - int startY, - int startX, - int endY, - int endX, - int width, - int length, - int v, - int d) { + boolean[][] matrix, + int startY, + int startX, + int endY, + int endX, + int width, + int length, + int v, + int d) { for (int i = 0; i < matrix.length; i++) { for (int j = 0; j < matrix.length; j++) { @@ -164,7 +164,7 @@ private ArrayList generateHValue( * path */ private ArrayList generatePath( - Node hValue[][], int startY, int startX, int endY, int endX, int x, int y, int v, int d) { + Node[][] hValue, int startY, int startX, int endY, int endX, int x, int y, int v, int d) { // Creation of a PriorityQueue and the declaration of the Comparator PriorityQueue openList = diff --git a/src/main/java/frc/robot/positioning/Path.java b/src/main/java/frc/robot/positioning/Path.java index d67ccbb..6110503 100644 --- a/src/main/java/frc/robot/positioning/Path.java +++ b/src/main/java/frc/robot/positioning/Path.java @@ -5,7 +5,7 @@ /** Return object for {@link positioning.Path.generateAStarPath} */ public class Path { private ArrayList pathList; - private boolean generatedPath; + private final boolean generatedPath; public ArrayList getPathList() { return this.pathList; diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java index 413a017..ff73b38 100644 --- a/src/main/java/frc/robot/subsystems/Camera.java +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -10,8 +10,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Camera extends SubsystemBase { - private int port; + private final int port; private UsbCamera camera; + /** Creates a new Camera. */ public Camera(int port) { this.port = port; diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index d28b508..2ad5ce8 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -26,19 +26,22 @@ public class Drive extends SubsystemBase { /* Motor controllers */ - private WPI_TalonSRX left, right, frontLeft, frontRight; + private WPI_TalonSRX left; + private WPI_TalonSRX right; + private final WPI_TalonSRX frontLeft; + private final WPI_TalonSRX frontRight; - private DifferentialDriveKinematics kinematics; + private final DifferentialDriveKinematics kinematics; - private DifferentialDrivePoseEstimator whereTheHeckAreWe; + private final DifferentialDrivePoseEstimator whereTheHeckAreWe; - private RamseteController ramseteController; + private final RamseteController ramseteController; - private DifferentialDrive drivetrain; + private final DifferentialDrive drivetrain; - private AHRS gyroscope; + private final AHRS gyroscope; - private LimelightManager limelightManager; + private final LimelightManager limelightManager; /** Creates a new drivetrain using IDs from {@link Constants.Drive}. */ public Drive(LimelightManager limelightManager) { @@ -84,7 +87,7 @@ public Drive(LimelightManager limelightManager) { /* Instantiates the gyroscope. */ gyroscope = new AHRS(SPI.Port.kMXP); - /* Gives us an instance of the Limelight Manager, which chooses which Limelight we pull data + /* Gives us an instance of the Limelight Manager, which chooses which Limelight we pull data * from, since there are two on the bot */ this.limelightManager = limelightManager; @@ -106,23 +109,34 @@ public Drive(LimelightManager limelightManager) { ramseteController = new RamseteController(); /* Configure PID values for both sides */ - left = configureTalonPID(left, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); - right = configureTalonPID(right, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); - //frontLeft = configureTalonPID(frontLeft, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); - //frontRight = configureTalonPID(frontRight, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); - + left = + configureTalonPID( + left, + Constants.Drive.DrivePIDYAY.P, + Constants.Drive.DrivePIDYAY.I, + Constants.Drive.DrivePIDYAY.D); + right = + configureTalonPID( + right, + Constants.Drive.DrivePIDYAY.P, + Constants.Drive.DrivePIDYAY.I, + Constants.Drive.DrivePIDYAY.D); + // frontLeft = configureTalonPID(frontLeft, Constants.Drive.DrivePIDYAY.P, + // Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); + // frontRight = configureTalonPID(frontRight, Constants.Drive.DrivePIDYAY.P, + // Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); + /*SmartDashboard.putString("REMEMBER TO WRITE DOWN YOUR PID VALUES", "I MEAN IT, WRITE THEM DOWN"); SmartDashboard.putNumber("Drive P", 0); SmartDashboard.putNumber("Drive I", 0); SmartDashboard.putNumber("Drive D", 0);*/ } - /** - * Class to handle converting m/s across the ground to encoder ticks/100ms - */ + /** Class to handle converting m/s across the ground to encoder ticks/100ms */ private static class EvilUnitConverter { /** * Convert m/s across the ground to encoder ticks/100ms (around the axle) + * * @param metersPerSecond m/s across the ground * @return Encoder ticks/100ms (angular momentum around the axle) */ @@ -136,6 +150,7 @@ public static double metersPerSecondToEncoderTicksPer100ms(double metersPerSecon /** * Convert encoder ticks/100ms (around the axle) to m/s across the ground + * * @param encoderTicksPer100ms Encoder ticks/100ms (angular momentum around the axle) * @return m/s across the ground */ @@ -184,12 +199,14 @@ public static double encoderTicksPer100msToMetersPerSecond(double encoderTicksPe /** * Configures a passed WPI_TalonSRX with the passed PID values + * * @param talon WPI_TalonSRX to configure * @param P kP value to set * @param I kI value to set * @param D kD value to set * @return The configured WPI_TalonSRX. You will NEED to set whatever variable represents your - * Talon to this returned value, like so: talon = configureTalonPID(talon, kP, kI, kD);. + * Talon to this returned value, like so: talon = configureTalonPID(talon, kP, kI, kD); + * . */ private WPI_TalonSRX configureTalonPID(WPI_TalonSRX talon, double P, double I, double D) { talon.config_kP(0, P); @@ -234,13 +251,13 @@ public void moveArcade(double x, double z) { public void moveTank(double left, double right) { drivetrain.tankDrive(left, right); } - + /** * Sets the velocity setpoint of the left and right sides of the drivetrain. - * - *

These are PID setpoints - they will require the drivetrain to have appropriately-tuned - * PID gains. - * + * + *

These are PID setpoints - they will require the drivetrain to have appropriately-tuned PID + * gains. + * * @param velocityLeft Velocity setpoint for the left side of the drivetrain * @param velocityRight Velocity setpoint for the right side of the drivetrain */ @@ -253,8 +270,8 @@ public void setVelocity(double velocityLeft, double velocityRight) { * Stop. * *

(stops all motors controlled by this subsystem) - * - *

WILL NOT stop motors that currently have a PID setpoint. Use {@link #setVelocity} + * + *

WILL NOT stop motors that currently have a PID setpoint. Use {@link #setVelocity} * and zero their setpoints instead. */ public void stop() { @@ -268,17 +285,19 @@ public void stop() { * @return Position of the encoder on the left side of the drivetrain */ private double getLeftPosition() { - double leftPositionMeters = EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(left.getSelectedSensorPosition()); + double leftPositionMeters = + EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(left.getSelectedSensorPosition()); return leftPositionMeters; } /** * Get the position of the encoder on the right side of the drivetrain. - * + * * @return Position of the encoder on the right side of the drivetrain */ private double getRightPosition() { - double rightPositionMeters = EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(right.getSelectedSensorPosition()); + double rightPositionMeters = + EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(right.getSelectedSensorPosition()); return rightPositionMeters; } @@ -322,11 +341,10 @@ public double getPitch() { public void resetPosition(Pose2d currentPose) { whereTheHeckAreWe.resetPosition( gyroscope.getRotation2d(), - this - .getLeftPosition(), // we miiiiight need to create an offset and zero these, - // which would be horrible, but doable - // I think we can actually just run a method that tells the - // motor controllers to zero them + this.getLeftPosition(), // we miiiiight need to create an offset and zero these, + // which would be horrible, but doable + // I think we can actually just run a method that tells the + // motor controllers to zero them this.getRightPosition(), currentPose); } @@ -349,8 +367,10 @@ public void trackTrajectory(Trajectory.State nextState) { ChassisSpeeds chassisSpeeds = ramseteController.calculate(whereTheHeckAreWe.getEstimatedPosition(), nextState); DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - double leftSetpoint = EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.leftMetersPerSecond); - double rightSetpoint = EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.rightMetersPerSecond); + double leftSetpoint = + EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.leftMetersPerSecond); + double rightSetpoint = + EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.rightMetersPerSecond); left.set(ControlMode.Velocity, leftSetpoint); right.set(ControlMode.Velocity, rightSetpoint); } @@ -394,6 +414,6 @@ public void periodic() { } /* TODO: REMOVE IN PROD */ - //updatePIDValuesFromSmartDash(); + // updatePIDValuesFromSmartDash(); } } diff --git a/src/main/java/frc/robot/subsystems/I2CManager.java b/src/main/java/frc/robot/subsystems/I2CManager.java index 0ce5561..8a9d75b 100644 --- a/src/main/java/frc/robot/subsystems/I2CManager.java +++ b/src/main/java/frc/robot/subsystems/I2CManager.java @@ -10,16 +10,18 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.TCA9548A; + // import edu.wpi.first.wpilibj.Notifier; // import com.revrobotics.ColorSensorV3; public class I2CManager extends SubsystemBase { - private TCA9548A multiplexer; - private Rev2mDistanceSensor dist; /* This is a color sensor, but we only use it for distance */ + private final TCA9548A multiplexer; + private final Rev2mDistanceSensor dist; /* This is a color sensor, but we only use it for distance */ // private ColorSensorV3 color; private boolean currentRangeValid; private double currentRange; private Color currentColor; + // private Notifier thread; /** Creates a new I2CManager. */ diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 72d9b40..8282d0b 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -33,20 +33,25 @@ public class Limelight extends SubsystemBase { /* it would be so cool if we just didn't have to do multiple pipelines at all this year */ /* variable chaingun, I promise we use all of these */ - private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber; - private DoubleArraySubscriber botposSubscriber; + private final DoubleSubscriber tvSubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleSubscriber taSubscriber; + private final DoubleArraySubscriber botposSubscriber; private double tv, tx, ty, ta; /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag * to understand what the heck the different indices in this array mean */ private double[] botpos; - private String name; + private final String name; private String fmtPath(String end) { return "/" + name + "/" + end; } - /** Creates a new Limelight. Should be run once from {@link frc.robot.RobotContainer}. + /** + * Creates a new Limelight. Should be run once from {@link frc.robot.RobotContainer}. + * * @param name The hostname of the limelight */ public Limelight(String name) { diff --git a/src/main/java/frc/robot/subsystems/LimelightManager.java b/src/main/java/frc/robot/subsystems/LimelightManager.java index bfa2549..6d2163c 100644 --- a/src/main/java/frc/robot/subsystems/LimelightManager.java +++ b/src/main/java/frc/robot/subsystems/LimelightManager.java @@ -5,13 +5,13 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -/** - * Manages connected limelights. - */ + +/** Manages connected limelights. */ public class LimelightManager extends SubsystemBase { - private Limelight[] limelights; + private final Limelight[] limelights; private Limelight cachedLimelight; + /** Creates a new LimelightManager. */ public LimelightManager(Limelight... limelight) { assert limelight != null; @@ -21,7 +21,7 @@ public LimelightManager(Limelight... limelight) { @Override public void periodic() { cachedLimelight = null; // dont keep stale limelights - for (Limelight ll: limelights) { + for (Limelight ll : limelights) { if (ll.hasValidTarget()) { cachedLimelight = ll; break; @@ -31,7 +31,9 @@ public void periodic() { /** * Returns a limelight that has a target. - * @return the limelight that has a target. WILL BE NULL if none have valid targets. + * + * @return the limelight that has a target. WILL BE NULL if none have valid + * targets. */ public Limelight getTargetedLimelight() { return cachedLimelight; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmBase.java b/src/main/java/frc/robot/subsystems/arm/ArmBase.java index 68f35c8..08d3ac6 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmBase.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmBase.java @@ -19,8 +19,8 @@ */ public class ArmBase extends SubsystemBase { - private WPI_TalonSRX armBaseMotor; - private SensorCollection sensorCollection; + private final WPI_TalonSRX armBaseMotor; + private final SensorCollection sensorCollection; /** Creates a new ArmBase. Should be called once from {@link frc.robot.RobotContainer}. */ public ArmBase() { @@ -55,6 +55,7 @@ private WPI_TalonSRX configTalon(int id) { return talon; } + /** Checks if either base limit switch is closed. */ public boolean baseLimitSwitch() { return sensorCollection.isFwdLimitSwitchClosed() || sensorCollection.isRevLimitSwitchClosed(); @@ -63,6 +64,7 @@ public boolean baseLimitSwitch() { public void manualMoveArmBase(double x) { armBaseMotor.set(ControlMode.PercentOutput, x); } + /** Returns the angle of the base CCW from forward in degrees */ public double getAngle() { /* TODO: account for difference between limit switch position and front of bot */ diff --git a/src/main/java/frc/robot/subsystems/arm/ArmClaw.java b/src/main/java/frc/robot/subsystems/arm/ArmClaw.java index 9bbdd56..d06e04b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmClaw.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmClaw.java @@ -16,10 +16,10 @@ */ public class ArmClaw extends SubsystemBase { - private Talon clawMotor; + private final Talon clawMotor; private Double setpoint; - private AnalogInput pressure; + private final AnalogInput pressure; private boolean isAtSetpoint; @@ -103,6 +103,7 @@ public void periodic() { } SmartDashboard.putNumber("Pressure Reading", pressure.getVoltage()); - SmartDashboard.putBoolean("At cone pressure", pressure.getVoltage() >= Constants.Arm.CONE_PRESSURE_THRESHOLD); + SmartDashboard.putBoolean( + "At cone pressure", pressure.getVoltage() >= Constants.Arm.CONE_PRESSURE_THRESHOLD); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtender.java b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java index 573d640..f899894 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmExtender.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java @@ -13,10 +13,10 @@ /** This subsystem represents the climber in a box that extends the arm on the robot. */ public class ArmExtender extends SubsystemBase { - private Talon winch; + private final Talon winch; private boolean extenderAtSetpoint; - private I2CManager I2CManager; + private final I2CManager I2CManager; private Double setpoint; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java b/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java index bf00754..1cf44c2 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java @@ -19,8 +19,8 @@ */ public class ArmShoulder extends SubsystemBase { - private WPI_TalonSRX armShoulderMotor; - private SensorCollection sensorCollection; + private final WPI_TalonSRX armShoulderMotor; + private final SensorCollection sensorCollection; /** Creates a new ArmShoulder. Should be called once from {@link frc.robot.RobotContainer}. */ public ArmShoulder() { diff --git a/src/main/java/frc/robot/subsystems/arm/CompilationArm.java b/src/main/java/frc/robot/subsystems/arm/CompilationArm.java index 95dc01c..cca7af0 100644 --- a/src/main/java/frc/robot/subsystems/arm/CompilationArm.java +++ b/src/main/java/frc/robot/subsystems/arm/CompilationArm.java @@ -10,11 +10,11 @@ @SuppressWarnings("unused") public class CompilationArm extends SubsystemBase { - private ArmBase armBase; - private ArmClaw armClaw; - private ArmExtender armExtender; + private final ArmBase armBase; + private final ArmClaw armClaw; + private final ArmExtender armExtender; private Pose armPose; - private ArmShoulder armShoulder; + private final ArmShoulder armShoulder; /** Creates a new CompilationArm. */ public CompilationArm( diff --git a/src/main/java/frc/robot/subsystems/yourMom.java b/src/main/java/frc/robot/subsystems/yourMom.java index ca9fbbd..a941532 100644 --- a/src/main/java/frc/robot/subsystems/yourMom.java +++ b/src/main/java/frc/robot/subsystems/yourMom.java @@ -12,6 +12,7 @@ public class yourMom extends CommandBase { public yourMom() { // Use addRequirements() here to declare subsystem dependencies. } + /** prints "Your Mom" */ public void printYourMom() { System.out.print("Your Mom"); @@ -25,6 +26,7 @@ public void printYourMom() { public PrintCommand commandPrintYourMom() { return new PrintCommand("Your Mom"); } + // Called when the command is initially scheduled. @Override public void initialize() {} From 86ece5a347c8e57f0e73b6338eb59fd42e944b58 Mon Sep 17 00:00:00 2001 From: Quinn Date: Thu, 26 Oct 2023 09:37:49 -0500 Subject: [PATCH 5/9] inline defaultbotpos - quinn --- src/main/java/frc/robot/subsystems/Limelight.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 8282d0b..79b975d 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -60,14 +60,10 @@ public Limelight(String name) { txSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tx")).subscribe(0.0); tySubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ty")).subscribe(0.0); taSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ta")).subscribe(0.0); - /* In theory this won't break. It got mad when I tried to insert the array into the - * method like .subscribe({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) so ¯\_(ツ)_/¯ - */ - double[] defaultBotpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; botposSubscriber = NetworkTableInstance.getDefault() .getDoubleArrayTopic(fmtPath("botpose")) - .subscribe(defaultBotpos); + .subscribe(new double[]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); } /* now its time for getter method chaingun, which I have to write manually because VS Code */ From d82df07014f40a66d259560ec873ee4c048af156 Mon Sep 17 00:00:00 2001 From: Quinn Date: Thu, 26 Oct 2023 09:38:22 -0500 Subject: [PATCH 6/9] remove SuppressWarnings in robot container - quinn --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 148eaef..46f6c38 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,7 +50,6 @@ import frc.robot.subsystems.arm.ArmShoulder; import frc.robot.subsystems.arm.CompilationArm; -// @SuppressWarnings("unused") let me see unused imports >:( /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} From f466d7f2747ddd1f438245435c616006e1cbb1e9 Mon Sep 17 00:00:00 2001 From: Quinn Date: Thu, 26 Oct 2023 09:42:13 -0500 Subject: [PATCH 7/9] remove test thing - quinnnnnnnnnnnnn n --- .../java/frc/robot/commands/TestDrivePID.java | 41 ------------------- src/main/java/frc/robot/subsystems/Drive.java | 3 -- 2 files changed, 44 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/TestDrivePID.java diff --git a/src/main/java/frc/robot/commands/TestDrivePID.java b/src/main/java/frc/robot/commands/TestDrivePID.java deleted file mode 100644 index b06c9a2..0000000 --- a/src/main/java/frc/robot/commands/TestDrivePID.java +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.subsystems.Drive; - -public class TestDrivePID extends CommandBase { - private final Drive drive; - - /** Creates a new TestDrivePID. */ - public TestDrivePID(Drive drive) { - // Use addRequirements() here to declare subsystem dependencies. - this.drive = drive; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - drive.setVelocity(Constants.Drive.TEST_PID_SETPOINT, Constants.Drive.TEST_PID_SETPOINT); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - drive.setVelocity(0, 0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 2ad5ce8..b9c5237 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -412,8 +412,5 @@ public void periodic() { whereTheHeckAreWe.addVisionMeasurement(limelight.getBotpose2d(), Timer.getFPGATimestamp()); } } - - /* TODO: REMOVE IN PROD */ - // updatePIDValuesFromSmartDash(); } } From b843f8fe399606f6677221c8cef93f6779ddde7c Mon Sep 17 00:00:00 2001 From: Quinn Date: Thu, 26 Oct 2023 12:23:07 -0500 Subject: [PATCH 8/9] bye astar ara was able to kill it -- ara & quinn --- .../java/frc/robot/positioning/AStar.java | 377 ------------------ 1 file changed, 377 deletions(-) delete mode 100644 src/main/java/frc/robot/positioning/AStar.java diff --git a/src/main/java/frc/robot/positioning/AStar.java b/src/main/java/frc/robot/positioning/AStar.java deleted file mode 100644 index 47cdaf5..0000000 --- a/src/main/java/frc/robot/positioning/AStar.java +++ /dev/null @@ -1,377 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.positioning; - -import frc.robot.Constants; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.PriorityQueue; -import java.util.concurrent.atomic.AtomicReference; - -/** - * @param cell - the map of nodes - * @param pathList - likely the path - * @param closedList - nodes that no longer need to be aknowledged by the pathfinder - * @param grid - the map of obstacles - * @param startY - the y of the start coord - * @param startX - the x of the start coord - * @param endY - the y of the end coord - * @param endX - the x of the end coord - */ -public class AStar { - private static Node[][] cell; - private final ArrayList pathList = new ArrayList<>(); - private final ArrayList closedList = new ArrayList<>(); - private static final boolean[][] grid = - new boolean[Constants.AStar.FIELD_X * 2][Constants.AStar.FIELD_Y * 2]; - private final AtomicReference currPath = new AtomicReference(new Path()); - private Thread genThread; - public int startY; - public int startX; - public int endY; - public int endX; - - static { - // creates the boolean obstacle matrix - // TODO: THIS IS (I BELIEVE) WHERE WE INPUT THE OBSTACLES, SO, YA KNOW, **IMPORTANT** - // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - for (int i = 0; i < Constants.AStar.FIELD_X * 2; i++) { - for (int j = 0; j < Constants.AStar.FIELD_Y * 2; j++) { - // HERE - // Creating a new Node object for each and every Cell of the Grid (Matrix) - cell[i][j] = new Node(i, j); - } - } - } - - /** Creates a new pathfinding situation. Input should be in centimeters. */ - public AStar(int startX, int startY, int endX, int endY) { - this.startX = startX; - this.startY = startY; - this.endX = endX; - this.endY = endY; - } - - public AtomicReference generateAStarPath() { - currPath.set(new Path()); - if (!genThread.isAlive()) { - this.genThread = - new Thread( - () -> { - int gCost = 0; - /*int fCost = 0;*/ - - // Creation of a Node type 2D array - cell = new Node[Constants.AStar.FIELD_X][Constants.AStar.FIELD_Y]; - - // Loop to find all 3 pathways and their relative Final Cost values - - generateHValue( - grid, - startY, - startX, - endY, - endX, - Constants.AStar.FIELD_X * 2, - Constants.AStar.FIELD_Y * 2, - 10, - 14); - - if (cell[startY][startX].hValue != -1 && pathList.contains(cell[endY][endX])) { - - for (int i = 0; i < pathList.size() - 1; i++) { - /* System.out.println(pathList.get(i).x + " " + pathList.get(i).y);*/ - /*StdDraw.circle(pathList.get(i).y, n - pathList.get(i).x - 1, .4);*/ - gCost += pathList.get(i).gValue; - /*fCost += pathList.get(i).fValue;*/ - } - - System.out.println("Euclidean Path Found"); - System.out.println("Total Cost: " + gCost / 10.0); - /*System.out.println("Total fCost: " + fCost);*/ - gCost = 0; - /*fCost = 0;*/ - - } else { - System.out.println("Euclidean Path Not found"); - } - - ArrayList pathListInMeters = new ArrayList(); - for (Node node : pathList) { - Double[] tempNode = {(double) node.getX() / 100, (double) node.getY() / 100}; - pathListInMeters.add(tempNode); - } - - currPath.set(new Path(pathListInMeters)); - }); - } - genThread.start(); - - return currPath; - } - - /** - * @param matrix The boolean matrix that the framework generates - * @param startY Starting point's x value - * @param startX Starting point's y value - * @param endY Ending point's x value - * @param endX Ending point's y value - * @param width Width of the field - * @param length Length of the field - * @param v Cost between 2 cells located horizontally or vertically next to each other - * @param d Cost between 2 cells located Diagonally next to each other - */ - private ArrayList generateHValue( - boolean[][] matrix, - int startY, - int startX, - int endY, - int endX, - int width, - int length, - int v, - int d) { - - for (int i = 0; i < matrix.length; i++) { - for (int j = 0; j < matrix.length; j++) { - - // Checks whether a cell is Blocked or Not by checking the boolean value - if (matrix[i][j]) { - // Assigning the Euclidean Heuristic value - // TODO: IF THE THING IS RUNNING SLOW, REMOVE THE Math.sqrt() TO MAYBE IMPROVE PROCESSING - // PROWER - cell[i][j].hValue = (int) Math.sqrt(Math.pow(i - endY, 2) + Math.pow(j - endX, 2)); - } - } - } - return generatePath( - cell, startY, startX, endY, endX, Constants.AStar.FIELD_X, Constants.AStar.FIELD_Y, v, d); - } - - /** - * @param hValue Node type 2D Array (Matrix) - * @param startY Starting point's y value - * @param startX Starting point's x value - * @param endY Ending point's y value - * @param endX Ending point's x value - * @param width Width of the field - * @param length Length of the field - * @param v Cost between 2 cells located horizontally or vertically next to each other - * @param d Cost between 2 cells located Diagonally next to each other - * @param additionalPath Boolean to decide whether to calculate the cost of through the diagonal - * path - */ - private ArrayList generatePath( - Node[][] hValue, int startY, int startX, int endY, int endX, int x, int y, int v, int d) { - - // Creation of a PriorityQueue and the declaration of the Comparator - PriorityQueue openList = - new PriorityQueue( - 11, - new Comparator() { - @Override - // Compares 2 Node objects stored in the PriorityQueue and Reorders the Queue - // according to the object which has the lowest fValue - public int compare(Node cell1, Node cell2) { - return cell1.fValue < cell2.fValue ? -1 : cell1.fValue > cell2.fValue ? 1 : 0; - } - }); - - // Adds the Starting cell inside the openList - openList.add(cell[startY][startX]); - - // Executes the rest if there are objects left inside the PriorityQueue - while (true) { - - // Gets and removes the objects that's stored on the top of the openList and saves it inside - // node - Node node = openList.poll(); - - // Checks if whether node is empty and f it is then breaks the while loop - if (node == null) { - break; - } - - // Checks if whether the node returned is having the same node object values of the ending - // point - // If it des then stores that inside the closedList and breaks the while loop - if (node == cell[endY][endX]) { - closedList.add(node); - break; - } - - closedList.add(node); - - // Left Cell - try { - if (cell[node.getX()][node.getY() - 1].hValue != -1 - && !openList.contains(cell[node.getX()][node.getY() - 1]) - && !closedList.contains(cell[node.getX()][node.getY() - 1])) { - int tCost = node.fValue + v; - cell[node.getX()][node.getY() - 1].gValue = v; - int cost = cell[node.getX()][node.getY() - 1].hValue + tCost; - if (cell[node.getX()][node.getY() - 1].fValue > cost - || !openList.contains(cell[node.getX()][node.getY() - 1])) - cell[node.getX()][node.getY() - 1].fValue = cost; - - openList.add(cell[node.getX()][node.getY() - 1]); - cell[node.getX()][node.getY() - 1].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // Right Cell - try { - if (cell[node.getX()][node.getY() + 1].hValue != -1 - && !openList.contains(cell[node.getX()][node.getY() + 1]) - && !closedList.contains(cell[node.getX()][node.getY() + 1])) { - int tCost = node.fValue + v; - cell[node.getX()][node.getY() + 1].gValue = v; - int cost = cell[node.getX()][node.getY() + 1].hValue + tCost; - if (cell[node.getX()][node.getY() + 1].fValue > cost - || !openList.contains(cell[node.getX()][node.getY() + 1])) - cell[node.getX()][node.getY() + 1].fValue = cost; - - openList.add(cell[node.getX()][node.getY() + 1]); - cell[node.getX()][node.getY() + 1].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // Bottom Cell - try { - if (cell[node.getX() + 1][node.getY()].hValue != -1 - && !openList.contains(cell[node.getX() + 1][node.getY()]) - && !closedList.contains(cell[node.getX() + 1][node.getY()])) { - int tCost = node.fValue + v; - cell[node.getX() + 1][node.getY()].gValue = v; - int cost = cell[node.getX() + 1][node.getY()].hValue + tCost; - if (cell[node.getX() + 1][node.getY()].fValue > cost - || !openList.contains(cell[node.getX() + 1][node.getY()])) - cell[node.getX() + 1][node.getY()].fValue = cost; - - openList.add(cell[node.getX() + 1][node.getY()]); - cell[node.getX() + 1][node.getY()].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // Top Cell - try { - if (cell[node.getX() - 1][node.getY()].hValue != -1 - && !openList.contains(cell[node.getX() - 1][node.getY()]) - && !closedList.contains(cell[node.getX() - 1][node.getY()])) { - int tCost = node.fValue + v; - cell[node.getX() - 1][node.getY()].gValue = v; - int cost = cell[node.getX() - 1][node.getY()].hValue + tCost; - if (cell[node.getX() - 1][node.getY()].fValue > cost - || !openList.contains(cell[node.getX() - 1][node.getY()])) - cell[node.getX() - 1][node.getY()].fValue = cost; - - openList.add(cell[node.getX() - 1][node.getY()]); - cell[node.getX() - 1][node.getY()].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // TopLeft Cell - try { - if (cell[node.getX() - 1][node.getY() - 1].hValue != -1 - && !openList.contains(cell[node.getX() - 1][node.getY() - 1]) - && !closedList.contains(cell[node.getX() - 1][node.getY() - 1])) { - int tCost = node.fValue + d; - cell[node.getX() - 1][node.getY() - 1].gValue = d; - int cost = cell[node.getX() - 1][node.getY() - 1].hValue + tCost; - if (cell[node.getX() - 1][node.getY() - 1].fValue > cost - || !openList.contains(cell[node.getX() - 1][node.getY() - 1])) - cell[node.getX() - 1][node.getY() - 1].fValue = cost; - - openList.add(cell[node.getX() - 1][node.getY() - 1]); - cell[node.getX() - 1][node.getY() - 1].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // TopRight Cell - try { - if (cell[node.getX() - 1][node.getY() + 1].hValue != -1 - && !openList.contains(cell[node.getX() - 1][node.getY() + 1]) - && !closedList.contains(cell[node.getX() - 1][node.getY() + 1])) { - int tCost = node.fValue + d; - cell[node.getX() - 1][node.getY() + 1].gValue = d; - int cost = cell[node.getX() - 1][node.getY() + 1].hValue + tCost; - if (cell[node.getX() - 1][node.getY() + 1].fValue > cost - || !openList.contains(cell[node.getX() - 1][node.getY() + 1])) - cell[node.getX() - 1][node.getY() + 1].fValue = cost; - - openList.add(cell[node.getX() - 1][node.getY() + 1]); - cell[node.getX() - 1][node.getY() + 1].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // BottomLeft Cell - try { - if (cell[node.getX() + 1][node.getY() - 1].hValue != -1 - && !openList.contains(cell[node.getX() + 1][node.getY() - 1]) - && !closedList.contains(cell[node.getX() + 1][node.getY() - 1])) { - int tCost = node.fValue + d; - cell[node.getX() + 1][node.getY() - 1].gValue = d; - int cost = cell[node.getX() + 1][node.getY() - 1].hValue + tCost; - if (cell[node.getX() + 1][node.getY() - 1].fValue > cost - || !openList.contains(cell[node.getX() + 1][node.getY() - 1])) - cell[node.getX() + 1][node.getY() - 1].fValue = cost; - - openList.add(cell[node.getX() + 1][node.getY() - 1]); - cell[node.getX() + 1][node.getY() - 1].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - - // BottomRight Cell - try { - if (cell[node.getX() + 1][node.getY() + 1].hValue != -1 - && !openList.contains(cell[node.getX() + 1][node.getY() + 1]) - && !closedList.contains(cell[node.getX() + 1][node.getY() + 1])) { - int tCost = node.fValue + d; - cell[node.getX() + 1][node.getY() + 1].gValue = d; - int cost = cell[node.getX() + 1][node.getY() + 1].hValue + tCost; - if (cell[node.getX() + 1][node.getY() + 1].fValue > cost - || !openList.contains(cell[node.getX() + 1][node.getY() + 1])) - cell[node.getX() + 1][node.getY() + 1].fValue = cost; - - openList.add(cell[node.getX() + 1][node.getY() + 1]); - cell[node.getX() + 1][node.getY() + 1].setParent(node); - } - } catch (IndexOutOfBoundsException e) { - } - } - - /*for (int i = 0; i < n; ++i) { - for (int j = 0; j < n; ++j) { - System.out.print(cell[i][j].fValue + " "); - } - System.out.println(); - }*/ - - // Assigns the last Object in the closedList to the endNode variable - Node endNode = closedList.get(closedList.size() - 1); - - // Checks if whether the endNode variable currently has a parent Node. if it doesn't then stops - // moving forward. - // Stores each parent Node to the PathList so it is easier to trace back the final path - while (endNode.getParent() != null) { - Node currentNode = endNode; - pathList.add(currentNode); - endNode = endNode.getParent(); - } - - pathList.add(cell[startY][startX]); - // Clears the openList - openList.clear(); - - return pathList; - } -} From 8355ede7ee2b624f31fc816d04a150854e068964 Mon Sep 17 00:00:00 2001 From: Quinn Date: Thu, 26 Oct 2023 16:00:19 -0500 Subject: [PATCH 9/9] undo formatting theoretically - quinn --- src/main/java/frc/robot/Constants.java | 39 ++------ src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 51 ++++------- src/main/java/frc/robot/commands/Autos.java | 20 ++--- src/main/java/frc/robot/commands/Balance.java | 14 +-- .../frc/robot/commands/CloseClawCone.java | 2 +- .../frc/robot/commands/CloseClawCube.java | 2 +- .../java/frc/robot/commands/ExtendArm.java | 3 +- .../frc/robot/commands/MoveBaseDegrees.java | 5 +- .../robot/commands/MoveShoulderDegrees.java | 5 +- .../java/frc/robot/commands/RetractArm.java | 3 +- .../commands/homingRoutine/HomeBase.java | 7 +- .../commands/homingRoutine/HomeShoulder.java | 7 +- .../commands/moveToPose/MoveBaseToPose.java | 4 +- .../moveToPose/MoveExtenderToPose.java | 4 +- .../moveToPose/MoveShoulderToPose.java | 4 +- src/main/java/frc/robot/positioning/Path.java | 2 +- .../java/frc/robot/subsystems/Camera.java | 3 +- src/main/java/frc/robot/subsystems/Drive.java | 88 +++++++------------ .../java/frc/robot/subsystems/I2CManager.java | 6 +- .../java/frc/robot/subsystems/Limelight.java | 13 +-- .../robot/subsystems/LimelightManager.java | 14 ++- .../frc/robot/subsystems/arm/ArmBase.java | 6 +- .../frc/robot/subsystems/arm/ArmClaw.java | 7 +- .../frc/robot/subsystems/arm/ArmExtender.java | 4 +- .../frc/robot/subsystems/arm/ArmShoulder.java | 4 +- .../robot/subsystems/arm/CompilationArm.java | 8 +- .../java/frc/robot/subsystems/yourMom.java | 2 - 28 files changed, 126 insertions(+), 202 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 89f9271..fa3efa2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,11 @@ package frc.robot; +import frc.robot.positioning.Pose; + +import java.util.ArrayList; +import java.util.List; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -11,9 +16,6 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.util.Units; -import frc.robot.positioning.Pose; -import java.util.ArrayList; -import java.util.List; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -33,10 +35,8 @@ public static class OperatorConstants { public static class RobotDimensions { /** The pose of the robot's center */ public static final Pose CENTER_POSE = new Pose(0, 0, 8.5); - /** The front/back length of the robot's frame in inches */ public static final double FRAME_LENGTH = 29.0; - /** The left/right width of the robot's frame in inches */ public static final double FRAME_WIDTH = 29.0; } @@ -74,8 +74,7 @@ public static class Drive { public static final double WHEEL_DIAMETER_INCHES = 8; // I THINK they're 8" public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(WHEEL_DIAMETER_INCHES); - public static final double TEST_PID_SETPOINT = - 4096; // This will no longer work after I fix the set method + public static final double TEST_PID_SETPOINT = 4096; // This will no longer work after I fix the set method public static class DrivePIDYAY { public static final double P = 0.13939; @@ -112,12 +111,9 @@ public static class Constraints { public static class Trajectories { private static final Pose2d startPose = new Pose2d(5.9, -2.8, new Rotation2d()); - private static final ArrayList interiorWaypoints = - new ArrayList<>(List.of(new Translation2d(5.5, -2.9))); + private static final ArrayList interiorWaypoints = new ArrayList<>(List.of(new Translation2d(5.5, -2.9))); private static final Pose2d endPose = new Pose2d(4.7, -2.3, new Rotation2d()); - public static final Trajectory DEMO_TRAJECTORY = - TrajectoryGenerator.generateTrajectory( - startPose, interiorWaypoints, endPose, new TrajectoryConfig(5.0, 3.0)); + public static final Trajectory DEMO_TRAJECTORY = TrajectoryGenerator.generateTrajectory(startPose, interiorWaypoints, endPose, new TrajectoryConfig(5.0, 3.0)); } } @@ -150,13 +146,10 @@ public static class DriveConstants { public static class Arm { /** The pose of the arm's base in robot oriented coordinates */ public static final Pose BASE_POSE = new Pose(0, 0, 8.5); - /** The distance between the floor and the arm base in inches */ public static final double BASE_DISTANCE_TO_FLOOR = 8.5; - /** Length in inches from the base of the arm to the shoulder joint */ public static final double BASE_TO_SHOULDER_LENGTH = 19; - /** * How high above the arm base should we keep the end effector to stop the arm from stabbing the * robot? This only matters inside the frame perimeter. Distance in inches. @@ -238,10 +231,8 @@ public static class I2C { public static class Limits { /** Maximum horizontal extension over the frame in inches */ public static final double MAX_FRAME_EXTENSION = 48.0; - /** Maximum extended robot height in inches */ public static final double MAX_EXTENDED_HEIGHT = 78.0; - /** Minimum distance from overextending we want to keep in inches */ public static final double OVEREXTENSION_DANGER_DISTANCE = 4.0; } @@ -254,13 +245,10 @@ public static class ConeNode { public static class Top { /** Height of the node in feet */ public static final double HEIGHT = 3.8333; - /** Horizontal distance from the bumper to the node in feet */ public static final double DISTANCE = 3.3125; - /** Height of the reflective tape off the ground in feet */ public static final double TAPE_HEIGHT = 3.4896; - /** The distance from the top of the tape to the top of the node */ public static final double ABOVE_TAPE = 0.0156; } @@ -268,13 +256,10 @@ public static class Top { public static class Middle { /** Height of the node in feet */ public static final double HEIGHT = 2.8333; - /** Horizontal distance from the bumper to the node in feet */ public static final double DISTANCE = 1.8958; - /** Height of the reflective tape off the ground in feet */ public static final double TAPE_HEIGHT = 1.8438; - /** The distance from the top of the tape to the top of the node */ public static final double ABOVE_TAPE = 0.6667; } @@ -283,7 +268,6 @@ public static class CubeNode { public static class Top { /** Height of the node in feet */ public static final double HEIGHT = 2.9583; - /** Horizontal distance from the bumper to the middle of the node in feet */ public static final double DISTANCE = 3.3125; } @@ -291,7 +275,6 @@ public static class Top { public static class Middle { /** Height of the node in feet */ public static final double HEIGHT = 1.9583; - /** Horizontal distance from the bumper to the middle of the node in feet */ public static final double DISTANCE = 1.8958; } @@ -303,19 +286,14 @@ public static class Objects { public static class Cone { /** Height in inches */ public static final double HEIGHT = 12.8125; - /** Width of the square base in inches */ public static final double WIDTH = 8.375; - /** Diameter of the bottom of the cone */ public static final double BOTTOM_DIAMETER = 6.625; - /** Diameter of top bottom of the cone */ public static final double TOP_DIAMETER = 1.75; - /** Average diameter of the cone's grabbable area */ public static final double AVG_DIAMETER = 4.1875; - /** The pressure reading required to securely hold the cone */ public static final double PRESSURE_TO_HOLD = -1; } @@ -323,7 +301,6 @@ public static class Cone { public static class Cube { /** Side length of a properly inflated cube in inches */ public static final double LENGTH = 9.5; - /** The pressure reading required to securely hold the cube */ public static final double PRESSURE_TO_HOLD = -1; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f070516..d8b3aba 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,3 +1,4 @@ + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 46f6c38..b1e2ef6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -66,8 +66,7 @@ public class RobotContainer { /* LIMELIGHT */ private final Limelight limelightBack = new Limelight("limelight-back"); private final Limelight limelightFront = new Limelight("limelight-front"); - private final LimelightManager limelightManager = - new LimelightManager(limelightBack, limelightFront); + private final LimelightManager limelightManager = new LimelightManager(limelightBack, limelightFront); /* CAMERA */ private final Camera aimCamera = new Camera(Constants.Camera.PORT_CAMERA_AIM); /* DRIVE */ @@ -81,7 +80,7 @@ public class RobotContainer { private final CompilationArm compilationArm = new CompilationArm(armBase, armClaw, armExtender, armShoulder); private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); - private final RetractArm retractArmCommand = new RetractArm(armExtender); + private RetractArm retractArmCommand = new RetractArm(armExtender); /* =============================================================== * COMMANDS @@ -89,13 +88,10 @@ public class RobotContainer { */ /* AUTO COMMANDS */ - private final StartLeavingCommunity startLeavingCommunity = - new StartLeavingCommunity(driveSubsystem); - private final StopLeavingCommunity stopLeavingCommunity = - new StopLeavingCommunity(driveSubsystem); + private final StartLeavingCommunity startLeavingCommunity = new StartLeavingCommunity(driveSubsystem); + private final StopLeavingCommunity stopLeavingCommunity = new StopLeavingCommunity(driveSubsystem); /* CAMERA COMMAND */ - private final InstantCommand displayAimVideo = - new InstantCommand(aimCamera::startCamera, aimCamera); + private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera); /* BALANCE COMMAND */ private final Balance balance = new Balance(driveSubsystem); /* DRIVE COMMANDS*/ @@ -104,16 +100,15 @@ public class RobotContainer { private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); /* ARM COMMANDS */ - private final Command openClaw = armClaw.manualOpenClawCommand(); + private final Command openClaw = armClaw.manualOpenClawCommand(); private final Command closeClaw = armClaw.manualCloseClawCommand(); private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); private final ExtendArm extend = new ExtendArm(armExtender); private final RetractArm retract = new RetractArm(armExtender); /* TRAJECTORY COMMAND */ - private final TrackTrajectory demoTrajectoryTrackingCommand = - new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); - + private final TrackTrajectory demoTrajectoryTrackingCommand = new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); + // Replace with CommandPS4Controller or CommandJoystick if needed public static CommandXboxController controller = new CommandXboxController(OperatorConstants.CONTROLLER_NUMBER); @@ -121,27 +116,19 @@ public class RobotContainer { public static CommandJoystick joystick = new CommandJoystick(OperatorConstants.JOYSTICK_NUMBER); private final SendableChooser autoChooser; - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureBindings(); /* TODO: add ResetDrivePose into auto*/ autoChooser = new SendableChooser<>(); - autoChooser.setDefaultOption( - "Fling cube + mobility bonus", - Commands.sequence( - Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), - new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), - new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), - new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); - autoChooser.addOption( - "Home arm (DOES NOT MOVE)", - Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); + autoChooser.setDefaultOption("Fling cube + mobility bonus", + Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); + autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); SmartDashboard.putData("Choose auto", autoChooser); - // compilationArm.setDefaultCommand(manualArm); + //compilationArm.setDefaultCommand(manualArm); driveSubsystem.setDefaultCommand(driveRO); } @@ -166,14 +153,9 @@ private void configureBindings() { controller.x().whileTrue(demoTrajectoryTrackingCommand); - joystick - .button(9) - .whileTrue( - MoveToPose.extendingMoveToPose( - Constants.Arm.Poses.PICKUP, armBase, armShoulder, armExtender, compilationArm)); - joystick - .button(4) - .whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); + joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase, + armShoulder, armExtender, compilationArm)); + joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); joystick.povUp().whileTrue(extend); joystick.povDown().whileTrue(retract); @@ -251,6 +233,7 @@ private void configureBindings() { .onTrue(armClaw.manualCloseClawCommand()); */ } + /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -313,7 +296,7 @@ public static double getScaledControllerRightYAxis() { } public static double getJoystickXAxis() { - return joystick.getX() * -1.0; + return joystick.getX()* -1.0; } public static double getScaledJoystickXAxis() { diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 16425b7..5b2cbdd 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -23,22 +23,14 @@ public static CommandBase homingRoutine( ArmClaw armClawSubsystem, CompilationArm compilationArmSubsystem) { return Commands.sequence( - armExtenderSubsystem.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), - new HomeShoulder(armShoulderSubsystem, compilationArmSubsystem), - new HomeBase(armBaseSubsystem, compilationArmSubsystem), - armClawSubsystem.openClawCommand()) - .withTimeout(5.5); + armExtenderSubsystem.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), + new HomeShoulder(armShoulderSubsystem, compilationArmSubsystem), + new HomeBase(armBaseSubsystem, compilationArmSubsystem), + armClawSubsystem.openClawCommand()).withTimeout(5.5); } - public static CommandBase homingNoOpenClaw( - ArmShoulder armShoulder, - ArmBase armBase, - ArmExtender armExtender, - CompilationArm compilationArm) { - return Commands.sequence( - armExtender.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), - new HomeShoulder(armShoulder, compilationArm), - new HomeBase(armBase, compilationArm)); + public static CommandBase homingNoOpenClaw(ArmShoulder armShoulder, ArmBase armBase, ArmExtender armExtender, CompilationArm compilationArm) { + return Commands.sequence(armExtender.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), new HomeShoulder(armShoulder, compilationArm), new HomeBase(armBase, compilationArm)); } private Autos() { diff --git a/src/main/java/frc/robot/commands/Balance.java b/src/main/java/frc/robot/commands/Balance.java index abf111f..70c619b 100644 --- a/src/main/java/frc/robot/commands/Balance.java +++ b/src/main/java/frc/robot/commands/Balance.java @@ -30,15 +30,19 @@ public void initialize() {} @Override public void execute() { double pitchAngleDegrees = drive.getPitch(); // forward back - + if (pitchAngleDegrees > BalanceConstants.OFF_ANGLE_THRESHOLD) { - this.forwards.execute(); + this.forwards.execute(); } else if (pitchAngleDegrees < -BalanceConstants.OFF_ANGLE_THRESHOLD) { - this.reverse.execute(); + this.reverse.execute(); } else { - this.reverse.end(true); - this.forwards.end(true); + this.reverse.end(true); + this.forwards.end(true); } + + + + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/CloseClawCone.java b/src/main/java/frc/robot/commands/CloseClawCone.java index 3a3a36d..303d1af 100644 --- a/src/main/java/frc/robot/commands/CloseClawCone.java +++ b/src/main/java/frc/robot/commands/CloseClawCone.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.arm.ArmClaw; public class CloseClawCone extends CommandBase { - private final ArmClaw armClaw; + private ArmClaw armClaw; /** Creates a new CloseClawCone. */ public CloseClawCone(ArmClaw armClaw) { diff --git a/src/main/java/frc/robot/commands/CloseClawCube.java b/src/main/java/frc/robot/commands/CloseClawCube.java index 8d08bf4..479697b 100644 --- a/src/main/java/frc/robot/commands/CloseClawCube.java +++ b/src/main/java/frc/robot/commands/CloseClawCube.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.arm.ArmClaw; public class CloseClawCube extends CommandBase { - private final ArmClaw armClaw; + private ArmClaw armClaw; /** Creates a new CloseClawCube. */ public CloseClawCube(ArmClaw armClaw) { diff --git a/src/main/java/frc/robot/commands/ExtendArm.java b/src/main/java/frc/robot/commands/ExtendArm.java index f3fb5f8..53b795d 100644 --- a/src/main/java/frc/robot/commands/ExtendArm.java +++ b/src/main/java/frc/robot/commands/ExtendArm.java @@ -8,8 +8,7 @@ import frc.robot.subsystems.arm.ArmExtender; public class ExtendArm extends CommandBase { - private final ArmExtender armExtender; - + private ArmExtender armExtender; /** Creates a new RetractArm. */ public ExtendArm(ArmExtender armExtender) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/MoveBaseDegrees.java b/src/main/java/frc/robot/commands/MoveBaseDegrees.java index ee0b509..2c21078 100644 --- a/src/main/java/frc/robot/commands/MoveBaseDegrees.java +++ b/src/main/java/frc/robot/commands/MoveBaseDegrees.java @@ -9,9 +9,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveBaseDegrees extends CommandBase { - private final int degrees; - private final ArmBase armBase; - + private int degrees; + private ArmBase armBase; /** Creates a new MoveBaseDegrees. */ public MoveBaseDegrees(int degrees, ArmBase armBase, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/MoveShoulderDegrees.java b/src/main/java/frc/robot/commands/MoveShoulderDegrees.java index 7adfb0a..25fa760 100644 --- a/src/main/java/frc/robot/commands/MoveShoulderDegrees.java +++ b/src/main/java/frc/robot/commands/MoveShoulderDegrees.java @@ -9,9 +9,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveShoulderDegrees extends CommandBase { - private final int degrees; - private final ArmShoulder armShoulder; - + private int degrees; + private ArmShoulder armShoulder; /** Creates a new MoveBaseDegrees. */ public MoveShoulderDegrees(int degrees, ArmShoulder armShoulder, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/RetractArm.java b/src/main/java/frc/robot/commands/RetractArm.java index 1fecd16..093b883 100644 --- a/src/main/java/frc/robot/commands/RetractArm.java +++ b/src/main/java/frc/robot/commands/RetractArm.java @@ -8,8 +8,7 @@ import frc.robot.subsystems.arm.ArmExtender; public class RetractArm extends CommandBase { - private final ArmExtender armExtender; - + private ArmExtender armExtender; /** Creates a new RetractArm. */ public RetractArm(ArmExtender armExtender) { // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java b/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java index 09ffe49..c6139c9 100644 --- a/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java +++ b/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java @@ -11,7 +11,6 @@ public class HomeBase extends CommandBase { private final ArmBase armBase; - /** Creates a new HomeShoulder. */ public HomeBase(ArmBase armShoulder, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. @@ -39,6 +38,10 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return armBase.baseLimitSwitch(); + if (armBase.baseLimitSwitch()) { + return true; + } else { + return false; + } } } diff --git a/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java b/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java index cfc7aee..1dd48a7 100644 --- a/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java +++ b/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java @@ -11,7 +11,6 @@ public class HomeShoulder extends CommandBase { private final ArmShoulder armShoulder; - /** Creates a new HomeShoulder. */ public HomeShoulder(ArmShoulder armShoulder, CompilationArm compilationArm) { // Use addRequirements() here to declare subsystem dependencies. @@ -39,6 +38,10 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return armShoulder.shoulderLimitSwitch(); + if (armShoulder.shoulderLimitSwitch()) { + return true; + } else { + return false; + } } } diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java index cc08aa5..adfd1b1 100644 --- a/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java +++ b/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveBaseToPose extends CommandBase { - private final ArmBase armBase; - private final Pose pose; + private ArmBase armBase; + private Pose pose; /** Creates a new moveBaseToPose. */ public MoveBaseToPose(Pose pose, ArmBase armBase, CompilationArm compilationArm) { diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java index 36fe799..8ab0cea 100644 --- a/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java +++ b/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveExtenderToPose extends CommandBase { - private final ArmExtender armExtender; - private final Pose pose; + private ArmExtender armExtender; + private Pose pose; /** Creates a new moveBaseToPose. */ public MoveExtenderToPose(Pose pose, ArmExtender armExtender, CompilationArm compilationArm) { diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java index a16f2e1..288a495 100644 --- a/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java +++ b/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java @@ -11,8 +11,8 @@ import frc.robot.subsystems.arm.CompilationArm; public class MoveShoulderToPose extends CommandBase { - private final ArmShoulder armShoulder; - private final Pose pose; + private ArmShoulder armShoulder; + private Pose pose; /** Creates a new moveBaseToPose. */ public MoveShoulderToPose(Pose pose, ArmShoulder armShoulder, CompilationArm compilationArm) { diff --git a/src/main/java/frc/robot/positioning/Path.java b/src/main/java/frc/robot/positioning/Path.java index 6110503..d67ccbb 100644 --- a/src/main/java/frc/robot/positioning/Path.java +++ b/src/main/java/frc/robot/positioning/Path.java @@ -5,7 +5,7 @@ /** Return object for {@link positioning.Path.generateAStarPath} */ public class Path { private ArrayList pathList; - private final boolean generatedPath; + private boolean generatedPath; public ArrayList getPathList() { return this.pathList; diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java index ff73b38..413a017 100644 --- a/src/main/java/frc/robot/subsystems/Camera.java +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -10,9 +10,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Camera extends SubsystemBase { - private final int port; + private int port; private UsbCamera camera; - /** Creates a new Camera. */ public Camera(int port) { this.port = port; diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index b9c5237..a7143f0 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -26,22 +26,19 @@ public class Drive extends SubsystemBase { /* Motor controllers */ - private WPI_TalonSRX left; - private WPI_TalonSRX right; - private final WPI_TalonSRX frontLeft; - private final WPI_TalonSRX frontRight; + private WPI_TalonSRX left, right, frontLeft, frontRight; - private final DifferentialDriveKinematics kinematics; + private DifferentialDriveKinematics kinematics; - private final DifferentialDrivePoseEstimator whereTheHeckAreWe; + private DifferentialDrivePoseEstimator whereTheHeckAreWe; - private final RamseteController ramseteController; + private RamseteController ramseteController; - private final DifferentialDrive drivetrain; + private DifferentialDrive drivetrain; - private final AHRS gyroscope; + private AHRS gyroscope; - private final LimelightManager limelightManager; + private LimelightManager limelightManager; /** Creates a new drivetrain using IDs from {@link Constants.Drive}. */ public Drive(LimelightManager limelightManager) { @@ -87,7 +84,7 @@ public Drive(LimelightManager limelightManager) { /* Instantiates the gyroscope. */ gyroscope = new AHRS(SPI.Port.kMXP); - /* Gives us an instance of the Limelight Manager, which chooses which Limelight we pull data + /* Gives us an instance of the Limelight Manager, which chooses which Limelight we pull data * from, since there are two on the bot */ this.limelightManager = limelightManager; @@ -109,34 +106,23 @@ public Drive(LimelightManager limelightManager) { ramseteController = new RamseteController(); /* Configure PID values for both sides */ - left = - configureTalonPID( - left, - Constants.Drive.DrivePIDYAY.P, - Constants.Drive.DrivePIDYAY.I, - Constants.Drive.DrivePIDYAY.D); - right = - configureTalonPID( - right, - Constants.Drive.DrivePIDYAY.P, - Constants.Drive.DrivePIDYAY.I, - Constants.Drive.DrivePIDYAY.D); - // frontLeft = configureTalonPID(frontLeft, Constants.Drive.DrivePIDYAY.P, - // Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); - // frontRight = configureTalonPID(frontRight, Constants.Drive.DrivePIDYAY.P, - // Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); - + left = configureTalonPID(left, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); + right = configureTalonPID(right, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); + //frontLeft = configureTalonPID(frontLeft, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); + //frontRight = configureTalonPID(frontRight, Constants.Drive.DrivePIDYAY.P, Constants.Drive.DrivePIDYAY.I, Constants.Drive.DrivePIDYAY.D); + /*SmartDashboard.putString("REMEMBER TO WRITE DOWN YOUR PID VALUES", "I MEAN IT, WRITE THEM DOWN"); SmartDashboard.putNumber("Drive P", 0); SmartDashboard.putNumber("Drive I", 0); SmartDashboard.putNumber("Drive D", 0);*/ } - /** Class to handle converting m/s across the ground to encoder ticks/100ms */ + /** + * Class to handle converting m/s across the ground to encoder ticks/100ms + */ private static class EvilUnitConverter { /** * Convert m/s across the ground to encoder ticks/100ms (around the axle) - * * @param metersPerSecond m/s across the ground * @return Encoder ticks/100ms (angular momentum around the axle) */ @@ -150,7 +136,6 @@ public static double metersPerSecondToEncoderTicksPer100ms(double metersPerSecon /** * Convert encoder ticks/100ms (around the axle) to m/s across the ground - * * @param encoderTicksPer100ms Encoder ticks/100ms (angular momentum around the axle) * @return m/s across the ground */ @@ -199,14 +184,12 @@ public static double encoderTicksPer100msToMetersPerSecond(double encoderTicksPe /** * Configures a passed WPI_TalonSRX with the passed PID values - * * @param talon WPI_TalonSRX to configure * @param P kP value to set * @param I kI value to set * @param D kD value to set * @return The configured WPI_TalonSRX. You will NEED to set whatever variable represents your - * Talon to this returned value, like so: talon = configureTalonPID(talon, kP, kI, kD); - * . + * Talon to this returned value, like so: talon = configureTalonPID(talon, kP, kI, kD);. */ private WPI_TalonSRX configureTalonPID(WPI_TalonSRX talon, double P, double I, double D) { talon.config_kP(0, P); @@ -251,13 +234,13 @@ public void moveArcade(double x, double z) { public void moveTank(double left, double right) { drivetrain.tankDrive(left, right); } - + /** * Sets the velocity setpoint of the left and right sides of the drivetrain. - * - *

These are PID setpoints - they will require the drivetrain to have appropriately-tuned PID - * gains. - * + * + *

These are PID setpoints - they will require the drivetrain to have appropriately-tuned + * PID gains. + * * @param velocityLeft Velocity setpoint for the left side of the drivetrain * @param velocityRight Velocity setpoint for the right side of the drivetrain */ @@ -270,8 +253,8 @@ public void setVelocity(double velocityLeft, double velocityRight) { * Stop. * *

(stops all motors controlled by this subsystem) - * - *

WILL NOT stop motors that currently have a PID setpoint. Use {@link #setVelocity} + * + *

WILL NOT stop motors that currently have a PID setpoint. Use {@link #setVelocity} * and zero their setpoints instead. */ public void stop() { @@ -285,19 +268,17 @@ public void stop() { * @return Position of the encoder on the left side of the drivetrain */ private double getLeftPosition() { - double leftPositionMeters = - EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(left.getSelectedSensorPosition()); + double leftPositionMeters = EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(left.getSelectedSensorPosition()); return leftPositionMeters; } /** * Get the position of the encoder on the right side of the drivetrain. - * + * * @return Position of the encoder on the right side of the drivetrain */ private double getRightPosition() { - double rightPositionMeters = - EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(right.getSelectedSensorPosition()); + double rightPositionMeters = EvilUnitConverter.encoderTicksPer100msToMetersPerSecond(right.getSelectedSensorPosition()); return rightPositionMeters; } @@ -341,10 +322,11 @@ public double getPitch() { public void resetPosition(Pose2d currentPose) { whereTheHeckAreWe.resetPosition( gyroscope.getRotation2d(), - this.getLeftPosition(), // we miiiiight need to create an offset and zero these, - // which would be horrible, but doable - // I think we can actually just run a method that tells the - // motor controllers to zero them + this + .getLeftPosition(), // we miiiiight need to create an offset and zero these, + // which would be horrible, but doable + // I think we can actually just run a method that tells the + // motor controllers to zero them this.getRightPosition(), currentPose); } @@ -367,10 +349,8 @@ public void trackTrajectory(Trajectory.State nextState) { ChassisSpeeds chassisSpeeds = ramseteController.calculate(whereTheHeckAreWe.getEstimatedPosition(), nextState); DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - double leftSetpoint = - EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.leftMetersPerSecond); - double rightSetpoint = - EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.rightMetersPerSecond); + double leftSetpoint = EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.leftMetersPerSecond); + double rightSetpoint = EvilUnitConverter.metersPerSecondToEncoderTicksPer100ms(wheelSpeeds.rightMetersPerSecond); left.set(ControlMode.Velocity, leftSetpoint); right.set(ControlMode.Velocity, rightSetpoint); } diff --git a/src/main/java/frc/robot/subsystems/I2CManager.java b/src/main/java/frc/robot/subsystems/I2CManager.java index 8a9d75b..0ce5561 100644 --- a/src/main/java/frc/robot/subsystems/I2CManager.java +++ b/src/main/java/frc/robot/subsystems/I2CManager.java @@ -10,18 +10,16 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.TCA9548A; - // import edu.wpi.first.wpilibj.Notifier; // import com.revrobotics.ColorSensorV3; public class I2CManager extends SubsystemBase { - private final TCA9548A multiplexer; - private final Rev2mDistanceSensor dist; /* This is a color sensor, but we only use it for distance */ + private TCA9548A multiplexer; + private Rev2mDistanceSensor dist; /* This is a color sensor, but we only use it for distance */ // private ColorSensorV3 color; private boolean currentRangeValid; private double currentRange; private Color currentColor; - // private Notifier thread; /** Creates a new I2CManager. */ diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 79b975d..f9f52ed 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -33,25 +33,20 @@ public class Limelight extends SubsystemBase { /* it would be so cool if we just didn't have to do multiple pipelines at all this year */ /* variable chaingun, I promise we use all of these */ - private final DoubleSubscriber tvSubscriber; - private final DoubleSubscriber txSubscriber; - private final DoubleSubscriber tySubscriber; - private final DoubleSubscriber taSubscriber; - private final DoubleArraySubscriber botposSubscriber; + private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber; + private DoubleArraySubscriber botposSubscriber; private double tv, tx, ty, ta; /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag * to understand what the heck the different indices in this array mean */ private double[] botpos; - private final String name; + private String name; private String fmtPath(String end) { return "/" + name + "/" + end; } - /** - * Creates a new Limelight. Should be run once from {@link frc.robot.RobotContainer}. - * + /** Creates a new Limelight. Should be run once from {@link frc.robot.RobotContainer}. * @param name The hostname of the limelight */ public Limelight(String name) { diff --git a/src/main/java/frc/robot/subsystems/LimelightManager.java b/src/main/java/frc/robot/subsystems/LimelightManager.java index 6d2163c..bfa2549 100644 --- a/src/main/java/frc/robot/subsystems/LimelightManager.java +++ b/src/main/java/frc/robot/subsystems/LimelightManager.java @@ -5,13 +5,13 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** Manages connected limelights. */ +/** + * Manages connected limelights. + */ public class LimelightManager extends SubsystemBase { - private final Limelight[] limelights; + private Limelight[] limelights; private Limelight cachedLimelight; - /** Creates a new LimelightManager. */ public LimelightManager(Limelight... limelight) { assert limelight != null; @@ -21,7 +21,7 @@ public LimelightManager(Limelight... limelight) { @Override public void periodic() { cachedLimelight = null; // dont keep stale limelights - for (Limelight ll : limelights) { + for (Limelight ll: limelights) { if (ll.hasValidTarget()) { cachedLimelight = ll; break; @@ -31,9 +31,7 @@ public void periodic() { /** * Returns a limelight that has a target. - * - * @return the limelight that has a target. WILL BE NULL if none have valid - * targets. + * @return the limelight that has a target. WILL BE NULL if none have valid targets. */ public Limelight getTargetedLimelight() { return cachedLimelight; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmBase.java b/src/main/java/frc/robot/subsystems/arm/ArmBase.java index 08d3ac6..68f35c8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmBase.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmBase.java @@ -19,8 +19,8 @@ */ public class ArmBase extends SubsystemBase { - private final WPI_TalonSRX armBaseMotor; - private final SensorCollection sensorCollection; + private WPI_TalonSRX armBaseMotor; + private SensorCollection sensorCollection; /** Creates a new ArmBase. Should be called once from {@link frc.robot.RobotContainer}. */ public ArmBase() { @@ -55,7 +55,6 @@ private WPI_TalonSRX configTalon(int id) { return talon; } - /** Checks if either base limit switch is closed. */ public boolean baseLimitSwitch() { return sensorCollection.isFwdLimitSwitchClosed() || sensorCollection.isRevLimitSwitchClosed(); @@ -64,7 +63,6 @@ public boolean baseLimitSwitch() { public void manualMoveArmBase(double x) { armBaseMotor.set(ControlMode.PercentOutput, x); } - /** Returns the angle of the base CCW from forward in degrees */ public double getAngle() { /* TODO: account for difference between limit switch position and front of bot */ diff --git a/src/main/java/frc/robot/subsystems/arm/ArmClaw.java b/src/main/java/frc/robot/subsystems/arm/ArmClaw.java index d06e04b..9bbdd56 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmClaw.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmClaw.java @@ -16,10 +16,10 @@ */ public class ArmClaw extends SubsystemBase { - private final Talon clawMotor; + private Talon clawMotor; private Double setpoint; - private final AnalogInput pressure; + private AnalogInput pressure; private boolean isAtSetpoint; @@ -103,7 +103,6 @@ public void periodic() { } SmartDashboard.putNumber("Pressure Reading", pressure.getVoltage()); - SmartDashboard.putBoolean( - "At cone pressure", pressure.getVoltage() >= Constants.Arm.CONE_PRESSURE_THRESHOLD); + SmartDashboard.putBoolean("At cone pressure", pressure.getVoltage() >= Constants.Arm.CONE_PRESSURE_THRESHOLD); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtender.java b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java index f899894..573d640 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmExtender.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java @@ -13,10 +13,10 @@ /** This subsystem represents the climber in a box that extends the arm on the robot. */ public class ArmExtender extends SubsystemBase { - private final Talon winch; + private Talon winch; private boolean extenderAtSetpoint; - private final I2CManager I2CManager; + private I2CManager I2CManager; private Double setpoint; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java b/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java index 1cf44c2..bf00754 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java @@ -19,8 +19,8 @@ */ public class ArmShoulder extends SubsystemBase { - private final WPI_TalonSRX armShoulderMotor; - private final SensorCollection sensorCollection; + private WPI_TalonSRX armShoulderMotor; + private SensorCollection sensorCollection; /** Creates a new ArmShoulder. Should be called once from {@link frc.robot.RobotContainer}. */ public ArmShoulder() { diff --git a/src/main/java/frc/robot/subsystems/arm/CompilationArm.java b/src/main/java/frc/robot/subsystems/arm/CompilationArm.java index cca7af0..95dc01c 100644 --- a/src/main/java/frc/robot/subsystems/arm/CompilationArm.java +++ b/src/main/java/frc/robot/subsystems/arm/CompilationArm.java @@ -10,11 +10,11 @@ @SuppressWarnings("unused") public class CompilationArm extends SubsystemBase { - private final ArmBase armBase; - private final ArmClaw armClaw; - private final ArmExtender armExtender; + private ArmBase armBase; + private ArmClaw armClaw; + private ArmExtender armExtender; private Pose armPose; - private final ArmShoulder armShoulder; + private ArmShoulder armShoulder; /** Creates a new CompilationArm. */ public CompilationArm( diff --git a/src/main/java/frc/robot/subsystems/yourMom.java b/src/main/java/frc/robot/subsystems/yourMom.java index a941532..ca9fbbd 100644 --- a/src/main/java/frc/robot/subsystems/yourMom.java +++ b/src/main/java/frc/robot/subsystems/yourMom.java @@ -12,7 +12,6 @@ public class yourMom extends CommandBase { public yourMom() { // Use addRequirements() here to declare subsystem dependencies. } - /** prints "Your Mom" */ public void printYourMom() { System.out.print("Your Mom"); @@ -26,7 +25,6 @@ public void printYourMom() { public PrintCommand commandPrintYourMom() { return new PrintCommand("Your Mom"); } - // Called when the command is initially scheduled. @Override public void initialize() {}