From eec4b4a91f5cf12b572ff165751c6ac77a220506 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Sat, 30 Sep 2023 17:40:04 -0700 Subject: [PATCH] Shift RollerMode to Roller was changeable but in constants, now moved to Roller subsystem =Also remove get/set of RollerMode.speed in favor of just making it public (if you both get and set a variable publicly without doing anything special, don't make it private) --- .../robotcode2023/Constants.java | 36 -------------- .../robotcode2023/RobotContainer.java | 25 +++++----- .../robotcode2023/commands/RunRoller.java | 17 +++---- .../robotcode2023/subsystems/Roller.java | 47 +++++++++++++++---- 4 files changed, 58 insertions(+), 67 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java index 3fd73ac1..088d2e5f 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java @@ -380,42 +380,6 @@ public static final class Roller { //#endregion //#region Command Constants - // TODO: Determine actual speeds/timings for roller - public static class RollerMode { - public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor); - public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor); - // The obj indicates which game object the roller is trying to intake - // if obj == NONE, that means it is trying to outtake rather than intake - public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor); - public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor); - public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor); - private double speed; - public double time; - public GameObject obj; - public Color ledColor; - - /** - * @param speed A number between -1 and 1 - * @param time Amount of time in seconds to keep the motor running after - * distance sensor has detected an object - * @param intake Whether the roller is outtaking or intaking - */ - public RollerMode(double speed, double time, GameObject obj, Color ledColor) { - this.speed = speed; - this.time = time; - this.obj = obj; - this.ledColor = ledColor; - } - - public double getSpeed() { - return speed; - } - - public void setSpeed(double speed) { - this.speed = speed; - } - } - //#endregion public static enum GameObject { diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java index 40110069..e414cb0e 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java @@ -14,7 +14,6 @@ import org.carlmontrobotics.robotcode2023.Constants.GoalPos; import org.carlmontrobotics.robotcode2023.Constants.OI.Driver; import org.carlmontrobotics.robotcode2023.Constants.OI.Manipulator; -import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode; import org.carlmontrobotics.robotcode2023.commands.AlignChargingStation; import org.carlmontrobotics.robotcode2023.commands.ArmTeleop; import org.carlmontrobotics.robotcode2023.commands.DriveOverChargeStation; @@ -76,7 +75,7 @@ public RobotContainer() { eventMap.put("Cone High Pos.", new SetArmWristGoalPreset(GoalPos.HIGH, () -> false, () -> false, arm)); // Command fakeArmCommand = new InstantCommand(() -> System.err.println("==============Store================="), arm); // eventMap.put("Stored Pos.", new SequentialCommandGroup(fakeArmCommand, new WaitCommand(2))); - eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CUBE))); + eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE))); eventMap.put("DriveOverChargeStation", new DriveOverChargeStation(drivetrain)); eventMap.put("Extend Arm High Cube", new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm)); eventMap.put("Extend Arm Mid Cube", new SetArmWristGoalPreset(GoalPos.MID, () -> true, () -> false, arm)); @@ -87,9 +86,9 @@ public RobotContainer() { new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm), new PrintCommand("================================Cube High Pos. Ended==================================") )); - eventMap.put("Run Cube Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CUBE)); - eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CONE))); - eventMap.put("Run Cone Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CONE)); + eventMap.put("Run Cube Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE)); + eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); + eventMap.put("Run Cone Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE)); //eventMap.put("Move Arm Back", new SetArmWristPositionV3((-5*Math.PI)/8, Constants.Arm.WRIST_STOW_POS_RAD, arm)); eventMap.put("Cone Intake Pos.", new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm)); eventMap.put("Cube Intake Pos.", new SequentialCommandGroup( @@ -171,23 +170,23 @@ private void configureButtonBindingsManipulator() { new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm)); new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm)); - new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE))); - new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE))); - new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CUBE))); - new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, RollerMode.STOP)); + new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); + new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); + new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE))); + new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, Roller.RollerMode.STOP)); // axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton) // .onTrue(new RunRoller(roller, RollerMode.INTAKE_CONE, Constants.Roller.conePickupColor)); axisTrigger(manipulatorController, Manipulator.rollerIntakeCubeButton) .onTrue(new ConditionalCommand( - new RunRoller(roller, RollerMode.INTAKE_CUBE), - new RunRoller(roller, RollerMode.OUTTAKE_CUBE), + new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE), + new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE), isIntake )); axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton) .onTrue(new ConditionalCommand( - new RunRoller(roller, RollerMode.INTAKE_CONE), - new RunRoller(roller, RollerMode.OUTTAKE_CONE), + new RunRoller(roller, Roller.RollerMode.INTAKE_CONE), + new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE), isIntake )); diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java b/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java index a5258df7..88bdac88 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java @@ -1,8 +1,7 @@ package org.carlmontrobotics.robotcode2023.commands; -import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject; -import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode; import org.carlmontrobotics.robotcode2023.subsystems.Roller; +import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -12,9 +11,9 @@ public class RunRoller extends CommandBase { private final Roller roller; private final Timer timer = new Timer(); - private final RollerMode mode; + private final Roller.RollerMode mode; - public RunRoller(Roller roller, RollerMode mode) { + public RunRoller(Roller roller, Roller.RollerMode mode) { addRequirements(this.roller = roller); this.mode = mode; } @@ -23,7 +22,7 @@ public RunRoller(Roller roller, RollerMode mode) { public void initialize() { System.err.println("=============================RunRoller is Started============================="); - if (mode.getSpeed() > 0) { // should not interrupt command to stop rollers + if (mode.speed > 0) { // should not interrupt command to stop rollers if(roller.hasGamePiece() && isIntake()) cancel(); if(!roller.hasGamePiece() && !isIntake()) cancel(); } @@ -38,8 +37,10 @@ public void execute() {} @Override public void end(boolean interrupted) { // keep it running if its a cone - if (mode.obj != GameObject.CONE && !interrupted) - roller.setSpeed(0); + if (mode.obj != GameObject.CONE && !interrupted) { + mode.speed = 0; + roller.setRollerMode(mode); + } timer.stop(); System.err.println("=============================RunRoller has ended=========================================================="); @@ -48,7 +49,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - if(mode.getSpeed() == 0) return true; + if(mode.speed == 0) return true; double time = timer.get(); diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java index d728ea3d..69a1feb3 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java @@ -89,15 +89,15 @@ public double getGamePieceDistanceIn() { } public void setRollerMode(RollerMode mode) { - setSpeed(mode.getSpeed()); + setSpeed(mode.speed); setLedColor(mode.ledColor); } public void putRollerConstsOnSmartDashboard() { - SmartDashboard.putNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.getSpeed()); - SmartDashboard.putNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.getSpeed()); - SmartDashboard.putNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.getSpeed()); - SmartDashboard.putNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.getSpeed()); + SmartDashboard.putNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.speed); + SmartDashboard.putNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.speed); + SmartDashboard.putNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.speed); + SmartDashboard.putNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.speed); SmartDashboard.putNumber("Intake Cone Time", RollerMode.INTAKE_CONE.time); SmartDashboard.putNumber("Outtake Cone Time", RollerMode.OUTTAKE_CONE.time); SmartDashboard.putNumber("Intake Cube Time", RollerMode.INTAKE_CUBE.time); @@ -105,10 +105,10 @@ public void putRollerConstsOnSmartDashboard() { } public void getRollerConstsOnSmartDashboard() { - RollerMode.INTAKE_CONE.setSpeed(SmartDashboard.getNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.getSpeed())); - RollerMode.OUTTAKE_CONE.setSpeed(SmartDashboard.getNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.getSpeed())); - RollerMode.INTAKE_CUBE.setSpeed(SmartDashboard.getNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.getSpeed())); - RollerMode.OUTTAKE_CUBE.setSpeed(SmartDashboard.getNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.getSpeed())); + RollerMode.INTAKE_CONE.speed = SmartDashboard.getNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.speed); + RollerMode.OUTTAKE_CONE.speed = SmartDashboard.getNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.speed); + RollerMode.INTAKE_CUBE.speed = SmartDashboard.getNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.speed); + RollerMode.OUTTAKE_CUBE.speed = SmartDashboard.getNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.speed); RollerMode.INTAKE_CONE.time = SmartDashboard.getNumber("Intake Cone Time", RollerMode.INTAKE_CONE.time); RollerMode.OUTTAKE_CONE.time = SmartDashboard.getNumber("Outtake Cone Time", RollerMode.OUTTAKE_CONE.time); RollerMode.INTAKE_CUBE.time = SmartDashboard.getNumber("Intake Cube Time", RollerMode.INTAKE_CUBE.time); @@ -118,5 +118,32 @@ public void getRollerConstsOnSmartDashboard() { public double getPosition() { return motor.getEncoder().getPosition(); } - + + // TODO: Determine actual speeds/timings for roller + public static class RollerMode { + public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor); + public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor); + // The obj indicates which game object the roller is trying to intake + // if obj == NONE, that means it is trying to outtake rather than intake + public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor); + public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor); + public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor); + public double speed; + public double time; + public GameObject obj; + public Color ledColor; + + /** + * @param speed A number between -1 and 1 + * @param time Amount of time in seconds to keep the motor running after + * distance sensor has detected an object + * @param intake Whether the roller is outtaking or intaking + */ + public RollerMode(double speed, double time, GameObject obj, Color ledColor) { + this.speed = speed; + this.time = time; + this.obj = obj; + this.ledColor = ledColor; + } + } }