From 818588b172876d31eaf604131d2421546cd8dd6b Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 10 Jul 2023 08:10:12 -0600 Subject: [PATCH 01/24] switching to logged chooser --- simgui-ds.json | 5 ++- simgui-window.json | 2 +- simgui.json | 1 + src/main/java/team3176/robot/Robot.java | 2 +- .../java/team3176/robot/RobotContainer.java | 32 ++++++++++++++----- .../subsystems/drivetrain/Drivetrain.java | 4 +-- 6 files changed, 31 insertions(+), 15 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 17e0f72..4726e2d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,12 +90,11 @@ } ], "robotJoysticks": [ - {}, { - "guid": "Keyboard1" + "guid": "Keyboard0" }, { - "guid": "Keyboard0" + "guid": "Keyboard1" } ] } diff --git a/simgui-window.json b/simgui-window.json index 81ae590..55f1055 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -27,7 +27,7 @@ }, "###NetworkTables": { "Collapsed": "0", - "Pos": "250,277", + "Pos": "244,55", "Size": "750,185" }, "###Other Devices": { diff --git a/simgui.json b/simgui.json index 5543b54..08ac180 100644 --- a/simgui.json +++ b/simgui.json @@ -29,6 +29,7 @@ "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", + "/SmartDashboard/AutoSelector": "String Chooser", "/SmartDashboard/Auton Choice": "String Chooser", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/Scheduler": "Scheduler" diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 9ecc806..8c2c229 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/src/main/java/team3176/robot/Robot.java @@ -131,7 +131,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { - //nan + robotContainer.checkAutonomousSelection(); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 069839f..73b0ae0 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,6 +6,8 @@ import java.io.File; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedDashboardInput; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Joystick; @@ -15,6 +17,7 @@ import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import team3176.robot.commands.*; import team3176.robot.commands.drivetrain.*; @@ -57,7 +60,9 @@ public class RobotContainer { private final Drivetrain drivetrain; private final VisionCubeChase vision; private final Superstructure superstructure; - private SendableChooser autonChooser; + private LoggedDashboardChooser autonChooser = new LoggedDashboardChooser<>("AutoSelector"); + private String choosenAutonomousString = ""; + private Command choosenAutonomousCommand; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -79,7 +84,7 @@ public RobotContainer() { controller::getStrafe, controller::getSpin)); arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); - autonChooser = new SendableChooser<>(); + autonChooser.addDefaultOption("wall_3_cube_poop_4_steal", "wall_3_cube_poop_4_steal"); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); for (File f : paths.listFiles()) { if (!f.isDirectory()) { @@ -89,7 +94,7 @@ public RobotContainer() { } - SmartDashboard.putData("Auton Choice", autonChooser); + SmartDashboard.putData("Auton Choice", autonChooser.getSendableChooser()); configureBindings(); } @@ -219,7 +224,13 @@ public void clearCanFaults(){ public void printCanFaults(){ pdh.getStickyFaults(); } - + public void checkAutonomousSelection() { + if(autonChooser.get() != null && !choosenAutonomousString.equals(autonChooser.get())) { + choosenAutonomousString = autonChooser.get(); + choosenAutonomousCommand = new PathPlannerAuto(choosenAutonomousString).getauto(); + + } + } /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -227,10 +238,15 @@ public void printCanFaults(){ */ public Command getAutonomousCommand() { // An example command will be run in autonomous - String chosen = autonChooser.getSelected(); - //String chosen = "wall_cone_exit_balance"; + + if(choosenAutonomousCommand == null) { + //String chosen = autonChooser.getSelected(); + String chosen = "wall_3nSteal_4"; - PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); - return ppSwerveAuto.getauto(); + PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); + return ppSwerveAuto.getauto(); + } + return choosenAutonomousCommand; + } } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 656ad51..c0f5efc 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -267,8 +267,8 @@ private void calculateNSetPodPositions() { realStates[idx] = new SwerveModuleState(pods.get(idx).getVelocity(),Rotation2d.fromDegrees(pods.get(idx).getAzimuth())); } //Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates); - //Logger.getInstance().recordOutput("SwerveStates/real", realStates); - //Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); + Logger.getInstance().recordOutput("SwerveStates/real", realStates); + Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); //Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); SmartDashboard.putNumber("spinCommand", spinCommand); SmartDashboard.putNumber("pod0 m/s", podStates[0].speedMetersPerSecond); From 868afa27a8392f6d76175619fd3c12430b748441 Mon Sep 17 00:00:00 2001 From: Jonathan Heidegger Date: Mon, 10 Jul 2023 11:03:27 -0400 Subject: [PATCH 02/24] verifying in simulation --- .../java/team3176/robot/RobotContainer.java | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 73b0ae0..befb853 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,6 +6,7 @@ import java.io.File; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardInput; @@ -15,6 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -84,7 +86,7 @@ public RobotContainer() { controller::getStrafe, controller::getSpin)); arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); - autonChooser.addDefaultOption("wall_3_cube_poop_4_steal", "wall_3_cube_poop_4_steal"); + //autonChooser.addDefaultOption("wall_3_cube_poop_4_steal", "wall_3_cube_poop_4_steal"); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); for (File f : paths.listFiles()) { if (!f.isDirectory()) { @@ -226,9 +228,18 @@ public void printCanFaults(){ } public void checkAutonomousSelection() { if(autonChooser.get() != null && !choosenAutonomousString.equals(autonChooser.get())) { + Long start = System.nanoTime(); choosenAutonomousString = autonChooser.get(); - choosenAutonomousCommand = new PathPlannerAuto(choosenAutonomousString).getauto(); - + try { + choosenAutonomousCommand = new PathPlannerAuto(choosenAutonomousString).getauto(); + } + catch(Exception e){ + System.out.println("[ERROR] could not find" + choosenAutonomousString); + System.out.println(e.toString()); + } + + Long totalTime = System.nanoTime() - start; + System.out.println("Autonomous Selected: [" + choosenAutonomousString + "] generated in " + (totalTime / 1000000.0) + "ms"); } } /** @@ -240,8 +251,8 @@ public Command getAutonomousCommand() { // An example command will be run in autonomous if(choosenAutonomousCommand == null) { - //String chosen = autonChooser.getSelected(); - String chosen = "wall_3nSteal_4"; + //this is if for some reason checkAutonomousSelection is never called + String chosen = autonChooser.get(); PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); From 4e44b33bcdedba965b060ec8ebdd131db3e2b4ca Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 10 Jul 2023 11:04:35 -0600 Subject: [PATCH 03/24] logging commands and using withName decoration for clarity of inline commands --- src/main/java/team3176/robot/Constants.java | 2 +- src/main/java/team3176/robot/Robot.java | 32 +++++++++++++++++++ .../java/team3176/robot/RobotContainer.java | 14 ++++---- .../robot/subsystems/superstructure/Arm.java | 8 ++--- .../robot/subsystems/superstructure/Claw.java | 10 +++--- .../subsystems/superstructure/IntakeCone.java | 2 +- .../subsystems/superstructure/IntakeCube.java | 9 +++++- .../superstructure/Superstructure.java | 20 ++++++------ 8 files changed, 67 insertions(+), 30 deletions(-) diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index ffc104e..67e298f 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -11,7 +11,7 @@ import java.util.Map; public final class Constants { - private static final RobotType robot = RobotType.ROBOT_SIMBOT; + private static final RobotType robot = RobotType.ROBOT_2023C; public static final double LOOP_PERIODIC_SECS = 0.02; public static final boolean TUNING_MODE = true; diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 9ecc806..75ae9c3 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/src/main/java/team3176/robot/Robot.java @@ -4,6 +4,10 @@ package team3176.robot; +import java.util.HashMap; +import java.util.Map; +import java.util.function.BiConsumer; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -17,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import team3176.robot.Constants.RobotType; +import team3176.robot.subsystems.drivetrain.Drivetrain; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; @@ -82,6 +87,7 @@ public void robotInit() { case REPLAY: String path = LogFileUtil.findReplayLog(); logger.setReplaySource(new WPILOGReader(path)); + logger.addDataReceiver(new NT4Publisher()); logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim"))); break; } @@ -104,6 +110,32 @@ public void robotInit() { fisheyeThread.start(); } + Map commandCounts = new HashMap<>(); + BiConsumer logCommandFunction = + (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.getInstance() + .recordOutput( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + Logger.getInstance().recordOutput("CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize( + (Command command) -> { + logCommandFunction.accept(command, true); + }); + CommandScheduler.getInstance() + .onCommandFinish( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + CommandScheduler.getInstance() + .onCommandInterrupt( + (Command command) -> { + logCommandFunction.accept(command, false); + }); } diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 069839f..fc44ee5 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -78,7 +78,7 @@ public RobotContainer() { controller::getForward, controller::getStrafe, controller::getSpin)); - arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); + //arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); autonChooser = new SendableChooser<>(); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); for (File f : paths.listFiles()) { @@ -87,7 +87,6 @@ public RobotContainer() { autonChooser.addOption(s, s); } } - SmartDashboard.putData("Auton Choice", autonChooser); @@ -133,7 +132,7 @@ private void configureBindings() { ); - controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); + controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense()).withName("setBrakeMode")); controller.rotStick.button(4).whileTrue(superstructure.intakeCubeHumanPlayer()); controller.rotStick.button(4).onFalse(superstructure.prepareCarry()); @@ -142,13 +141,13 @@ private void configureBindings() { double conveyorBumpTime = .1; //In units of seconds controller.operator.povUp().whileTrue(superstructure.prepareScoreHigh()); - controller.operator.povUp().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povUp().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); controller.operator.povRight().whileTrue(superstructure.prepareCarry()); - controller.operator.povRight().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povRight().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); controller.operator.povDown().whileTrue(superstructure.prepareCatch()); - controller.operator.povDown().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povDown().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); controller.operator.povLeft().whileTrue(superstructure.prepareScoreMid()); - controller.operator.povLeft().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povLeft().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); // m_Controller.operator.start().onTrue(new ToggleVisionLEDs()); // m_Controller.operator.back().onTrue(new SwitchToNextVisionPipeline()); @@ -228,7 +227,6 @@ public void printCanFaults(){ public Command getAutonomousCommand() { // An example command will be run in autonomous String chosen = autonChooser.getSelected(); - //String chosen = "wall_cone_exit_balance"; PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java index 5346969..d642bcb 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java @@ -136,7 +136,7 @@ public void setAngleSetpoint(double setpointAngle) { */ public Command armSetPosition(double angleInDegrees) { SmartDashboard.putNumber("armSetPosition",angleInDegrees); - return this.run(() -> setPIDPosition(angleInDegrees)); + return this.run(() -> setPIDPosition(angleInDegrees)).withName("armSetPosition"+angleInDegrees); } public Command armSetPositionBlocking(double angleInDegrees) { return new FunctionalCommand(() -> { @@ -145,15 +145,15 @@ public Command armSetPositionBlocking(double angleInDegrees) { ()-> {}, b -> {}, this::isArmAtPosition, - this); + this).withName("armsetPositionBlocking"); } public Command armSetPositionOnce(double angleInDegrees) { return this.runOnce(() -> { this.currentState = States.CLOSED_LOOP; - this.armSetpointAngleRaw = angleInDegrees;}); + this.armSetpointAngleRaw = angleInDegrees;}).withName("armSetPosition"+angleInDegrees); } public Command armFineTune(DoubleSupplier angleDeltaCommand) { - return this.run(() -> fineTune(angleDeltaCommand.getAsDouble())); + return this.run(() -> fineTune(angleDeltaCommand.getAsDouble())).withName("armFineTune"); } public Command armAnalogUpCommand() { return this.runEnd(this::armAnalogUp, this::idle); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java index 3ab63e4..1f55413 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java @@ -110,7 +110,7 @@ public static Claw getInstance() /** * to be called with a whileTrue trigger binding */ - public CommandBase intakeGamePiece(GamePiece piece) { + public Command intakeGamePiece(GamePiece piece) { return this.startEnd(() -> {this.currentGamePiece = piece; intake();},() -> hold()); } /** @@ -121,17 +121,17 @@ public Command scoreGamePiece() { return this.run(() -> {score(); this.currentGamePiece = GamePiece.NONE;}) .until(() -> this.isEmpty()) .andThen(new WaitCommand(0.7)) - .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()); + .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()).withName("scoreGamepiece"); } public Command scoreGamePieceTeleop() { return this.runEnd(() -> {score(); this.currentGamePiece = GamePiece.NONE;},() -> idle()); } //more examples of command composition and why its awesome!! public Command intakeCone() { - return this.intakeGamePiece(GamePiece.CONE).until(this::getLinebreakTwo); + return this.intakeGamePiece(GamePiece.CONE).until(this::getLinebreakTwo).withName("intakeCone"); } public Command intakeCube() { - return this.intakeGamePiece(GamePiece.CUBE).until(this::getLinebreakOne); + return this.intakeGamePiece(GamePiece.CUBE).until(this::getLinebreakOne).withName("intakeCube"); } public Command determineGamePiece() { return this.runOnce( () -> { @@ -142,7 +142,7 @@ public Command determineGamePiece() { this.currentGamePiece = GamePiece.CUBE; hold(); } - }); + }).withName("determineGamePiece"); } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java index 121b3de..b4e6d1e 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java @@ -105,7 +105,7 @@ public Command coneToClaw() { return this.run(() -> {spit();}) .until(() -> this.claw.getLinebreakTwo() == false) .andThen(new WaitCommand(0.5)) - .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()); + .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()).withName("coneToClaw"); } public double getVelocity() diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java index f4ae54c..9c4d611 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java @@ -148,7 +148,14 @@ public Command bumpConveyor() { this.spinConveyor(-.8); }, () -> { this.spinConveyor(0); - }); + }).withName("bumpConveyor"); + } + public Command bumpConveyorTimeout(double timeout) { + return this.startEnd(() ->{ + this.spinConveyor(-.8); + }, () -> { + this.spinConveyor(0); + }).withTimeout(timeout).withName("bumpConveyor"); } } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java index 064b3bd..f2469fe 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java @@ -34,7 +34,7 @@ public enum GamePiece {CUBE, CONE, NONE} public Command groundCube() { - return new IntakeGroundCube().andThen(this.prepareCarry()); + return new IntakeGroundCube().andThen(this.prepareCarry()).withName("groundCube"); } public Command groundCone() @@ -45,12 +45,12 @@ public Command groundCone() .until(() -> this.claw.getLinebreakThree() == false) .andThen(intakeCone.coneToClaw()) .andThen(new IntakeConeRetractSpinot()) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("groundCone"); } public Command clawIntakeCube() { - return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)); + return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)).withName("clawIntakeCube"); } /* @@ -67,7 +67,7 @@ public Command scoreGamePieceAuto() { return claw.determineGamePiece() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(1.5) .andThen(claw.scoreGamePiece()) - .andThen(this.prepareCarry())); + .andThen(this.prepareCarry())).withName("scoreGamePieceAuto"); } public Command scoreFirstGamePieceAuto() { return claw.determineGamePiece() @@ -75,7 +75,7 @@ public Command scoreFirstGamePieceAuto() { .alongWith(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece().withTimeout(1.0)) - .andThen(this.prepareCarry()))); + .andThen(this.prepareCarry()))).withName("scoreFirstGamePieceAuto"); } public Command scoreGamePieceHigh() { @@ -83,13 +83,13 @@ public Command scoreGamePieceHigh() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0)) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece().withTimeout(1.0)) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("scoreGamePieceHigh"); } public Command scoreCubeLow() { return arm.armSetPosition(SuperStructureConstants.ARM_ZERO_POS) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece()) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("scoreCubeLow"); } public Command scoreGamePieceLowAuto() { @@ -97,16 +97,16 @@ public Command scoreGamePieceLowAuto() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_CATCH_POS).withTimeout(3.0)) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece().withTimeout(1.0)) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("scoreGamePieceLowAuto"); } public Command intakeCubeHumanPlayer() { return new ParallelCommandGroup(new ClawInhaleCube(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) - .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeCubeHumanPlayer"); } public Command intakeConeHumanPlayer() { return new ParallelCommandGroup(new ClawInhaleCone(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) - .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeConeHumanPlayer"); } public Command preparePoop() { From c203578bdc04154e5533674b344057f53c13d509 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 20 Jul 2023 09:42:35 -0600 Subject: [PATCH 04/24] initial removal of commands --- .../java/team3176/robot/RobotContainer.java | 42 ++++++----- .../robot/commands/SetColorWantState.java | 44 ----------- .../commands/drivetrain/FeederPID3D.java | 74 ------------------- .../intakecube/IntakeRetractSpinot.java | 39 ---------- .../team3176/robot/subsystems/RobotState.java | 6 +- .../subsystems/superstructure/IntakeCube.java | 4 +- 6 files changed, 30 insertions(+), 179 deletions(-) delete mode 100644 src/main/java/team3176/robot/commands/SetColorWantState.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeRetractSpinot.java diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index fc44ee5..f387836 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -21,6 +21,7 @@ import team3176.robot.commands.superstructure.claw.ClawIdle; import team3176.robot.commands.superstructure.intakecube.*; import team3176.robot.constants.Hardwaremap; +import team3176.robot.subsystems.RobotState; import team3176.robot.subsystems.controller.Controller; import team3176.robot.subsystems.drivetrain.Drivetrain; import team3176.robot.subsystems.drivetrain.Drivetrain.coordType; @@ -57,6 +58,7 @@ public class RobotContainer { private final Drivetrain drivetrain; private final VisionCubeChase vision; private final Superstructure superstructure; + private final RobotState robotState; private SendableChooser autonChooser; /** @@ -70,6 +72,7 @@ public RobotContainer() { drivetrain = Drivetrain.getInstance(); intakeCube = IntakeCube.getInstance(); intakeCone = IntakeCone.getInstance(); + robotState = RobotState.getInstance(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); vision = VisionCubeChase.getInstance(); @@ -98,20 +101,19 @@ private void configureBindings() { controller.transStick.button(1).whileTrue(claw.scoreGamePiece()); //m_Controller.getTransStick_Button1().onFalse(new InstantCommand(() -> m_Drivetrain.setTurbo(false), m_Drivetrain)); - controller.transStick.button(2).whileTrue(new IntakeGroundCubeGuided()); - controller.transStick.button(2).onFalse(new IntakeRetractSpinot().andThen(superstructure.prepareCarry())); - controller.transStick.button(2).onFalse(superstructure.prepareCarry()); - controller.transStick.button(3).whileTrue(new SetColorWantState(3)); - controller.transStick.button(3).whileTrue(superstructure.groundCube()); - controller.transStick.button(3).onFalse(new IntakeRetractSpinot()); - controller.transStick.button(3).onFalse(superstructure.prepareCarry()); - - controller.transStick.button(4).whileTrue(superstructure.prepareScoreHigh()); - controller.transStick.button(4).onFalse((superstructure.prepareCarry())); -// controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); + controller.transStick.button(2).whileTrue(new IntakeGroundCubeGuided()) + .onFalse(intakeCube.retractSpinNot().andThen(superstructure.prepareCarry())) + .onFalse(superstructure.prepareCarry()); + controller.transStick.button(3).whileTrue(robotState.setColorWantedState(3)) + .whileTrue(superstructure.groundCube()) + .onFalse(intakeCube.retractSpinNot()) + .onFalse(superstructure.prepareCarry()); + controller.transStick.button(4).whileTrue(superstructure.prepareScoreHigh()) + .onFalse((superstructure.prepareCarry())); + //controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); controller.transStick.button(10).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); - //m_Controller.getTransStick_Button10() - // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); + //m_Controller.getTransStick_Button10() + // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); //m_Controller.getRotStick_Button2().whileTrue(new FlipField); controller.transStick.button(14).and(controller.transStick.button(15).onTrue(new CoordTypeFieldCentricOn())); @@ -152,27 +154,27 @@ private void configureBindings() { // m_Controller.operator.start().onTrue(new ToggleVisionLEDs()); // m_Controller.operator.back().onTrue(new SwitchToNextVisionPipeline()); - controller.operator.b().onTrue(new SetColorWantState(1)); + controller.operator.b().onTrue(robotState.setColorWantedState(1)); controller.operator.b().whileTrue(superstructure.intakeConeHumanPlayer()); controller.operator.b().onFalse(superstructure.prepareCarry()); - controller.operator.x().onTrue(new SetColorWantState(2)); + controller.operator.x().onTrue(robotState.setColorWantedState(2)); controller.operator.x().whileTrue(superstructure.intakeCubeHumanPlayer()); controller.operator.x().onFalse(superstructure.prepareCarry()); - controller.operator.a().onTrue(new SetColorWantState(3)); + controller.operator.a().onTrue(robotState.setColorWantedState(3)); controller.operator.a().whileTrue(superstructure.groundCube()); - controller.operator.a().onFalse(new IntakeRetractSpinot()); + controller.operator.a().onFalse(intakeCube.retractSpinNot()); controller.operator.a().onFalse(superstructure.prepareCarry()); - controller.operator.y().onTrue(new SetColorWantState(0)); + controller.operator.y().onTrue(robotState.setColorWantedState(0)); controller.operator.y().whileTrue(claw.scoreGamePiece()); controller.operator.y().onFalse(new ClawIdle()); - controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onTrue(new SetColorWantState(3)); + controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onTrue(robotState.setColorWantedState(3)); controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).whileTrue(new IntakeGroundCube()); - controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onFalse(new IntakeRetractSpinot()); + controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onFalse(intakeCube.retractSpinNot()); //m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).onFalse(m_Superstructure.prepareCarry()); controller.operator.leftBumper().and(controller.operator.rightBumper()).whileTrue((new PoopCube())); diff --git a/src/main/java/team3176/robot/commands/SetColorWantState.java b/src/main/java/team3176/robot/commands/SetColorWantState.java deleted file mode 100644 index f25d581..0000000 --- a/src/main/java/team3176/robot/commands/SetColorWantState.java +++ /dev/null @@ -1,44 +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 team3176.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.RobotConstants.Status; -import team3176.robot.subsystems.RobotState; - -public class SetColorWantState extends CommandBase { - /** Creates a new SetColorWantState. */ - RobotState m_RobotState; - int LEDState; - public SetColorWantState(int LEDState) { - // Use addRequirements() here to declare subsystem dependencies. - m_RobotState = RobotState.getInstance(); - addRequirements(m_RobotState); - this.LEDState = LEDState; - - } - - // 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() - { - //System.out.println("SetColorWantStateCommand()"); - m_RobotState.setColorWantState(LEDState); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java b/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java deleted file mode 100644 index eee98be..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/FeederPID3D.java +++ /dev/null @@ -1,74 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.CommandBase; - - -import team3176.robot.subsystems.drivetrain.Drivetrain; - -public class FeederPID3D extends CommandBase{ - Drivetrain drivetrain; - PIDController xController = new PIDController(2.0,0.0,0.0); - PIDController yController = new PIDController(2.0,0.0,0.0); - Pose2d redRight = new Pose2d(1.17, 7.44, Rotation2d.fromDegrees(180)); - Pose2d redLeft = new Pose2d(1.17, 6.17, Rotation2d.fromDegrees(180)); - Pose2d blueRight = new Pose2d(15.44, 6.17, Rotation2d.fromDegrees(0.0)); - Pose2d blueLeft = new Pose2d(15.44, 7.44, Rotation2d.fromDegrees(0.0)); - Pose2d targetPose; - NetworkTable vision; - Alliance alliance; - String side; - public FeederPID3D(String side) { - this.side = side; - alliance = DriverStation.getAlliance(); - drivetrain = Drivetrain.getInstance(); - addRequirements(drivetrain); - vision = NetworkTableInstance.getDefault().getTable("limelight"); - if(side.equals("right")) { - if(DriverStation.getAlliance() == Alliance.Red) { - targetPose = redRight; - } else { - targetPose = blueRight; - } - } else { - if(DriverStation.getAlliance() == Alliance.Red) { - targetPose = redLeft; - } else { - targetPose = blueLeft; - } - } - } - @Override - public void initialize(){ - drivetrain.setSpinLock(true); - drivetrain.setSpinLockAngle(targetPose.getRotation().getDegrees()); - } - @Override - public void execute() { - double[] defaultPose = {0.0,0.0,0.0,0.0,0.0,0.0}; - double[] visionPoseArray = vision.getEntry("botpose_wpiblue").getDoubleArray(defaultPose); - Pose2d camPose = new Pose2d(visionPoseArray[0],visionPoseArray[1],Rotation2d.fromDegrees(visionPoseArray[5])); - double tv = vision.getEntry("tv").getDouble(0.0); - double reverseAxis = DriverStation.getAlliance() == Alliance.Red ? -1.0 : 1.0; - if (tv != 0.0) { - drivetrain.drive(MathUtil.clamp(reverseAxis*xController.calculate(camPose.getX(), targetPose.getX()),-1.5,1.5), - (MathUtil.clamp(reverseAxis*yController.calculate(camPose.getY(),targetPose.getY()),-1.5,1.5)), - 0.0); - } else drivetrain.drive (Math.pow(10,-7),Math.pow(10,-7),Math.pow(10,-7)); - } - @Override - public boolean isFinished() { - return false; - } - @Override - public void end(boolean interrupted) { - drivetrain.setSpinLock(false); - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeRetractSpinot.java b/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeRetractSpinot.java deleted file mode 100644 index c38fe98..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeRetractSpinot.java +++ /dev/null @@ -1,39 +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 team3176.robot.commands.superstructure.intakecube; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.IntakeCube; - -public class IntakeRetractSpinot extends CommandBase { - /** Creates a new IntakeRetractSpinot. */ - IntakeCube m_IntakeCube = IntakeCube.getInstance(); - public IntakeRetractSpinot() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_IntakeCube); - } - - // 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() - { - m_IntakeCube.Retract(); - m_IntakeCube.spinIntake(0); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/subsystems/RobotState.java b/src/main/java/team3176/robot/subsystems/RobotState.java index 5bf8de0..a7bf92d 100644 --- a/src/main/java/team3176/robot/subsystems/RobotState.java +++ b/src/main/java/team3176/robot/subsystems/RobotState.java @@ -372,7 +372,7 @@ public void update() { } } - public void setColorWantState(int LEDState) { + private void setColorWantState(int LEDState) { //System.out.println("WAS CALLED"); wantedLEDState = LEDState; if (wantedLEDState == 0) { @@ -404,6 +404,10 @@ public static RobotState getInstance() { return instance; } + public Command setColorWantedState(int state) { + return this.runOnce(() -> this.setColorWantState(state)); + } + @Override public void periodic() { io.updateInputs(inputs); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java index 9c4d611..710ecb3 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java @@ -131,7 +131,9 @@ public Command extendAndSpin() { this.spinIntake(0.0); }); } - + public Command retractSpinNot() { + return this.runOnce(() -> {this.Retract(); this.spinIntake(0.0);}); + } public Command extendAndFreeSpin() { return this.startEnd(() ->{ From 0642c87753e54057a7bf49a06d64fbefcf14e411 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 20 Jul 2023 11:00:58 -0600 Subject: [PATCH 05/24] drivetrain modifications --- simgui-ds.json | 5 +-- simgui-window.json | 14 +++--- src/main/java/team3176/robot/Constants.java | 2 +- .../java/team3176/robot/RobotContainer.java | 7 ++- .../drivetrain/CoordTypeFieldCentricOn.java | 32 ------------- .../drivetrain/CoordTypeRobotCentricOn.java | 32 ------------- .../team3176/robot/subsystems/RobotState.java | 2 +- .../subsystems/drivetrain/Drivetrain.java | 37 +++++++++------ .../subsystems/drivetrain/SwervePod.java | 45 +++++++++++-------- 9 files changed, 64 insertions(+), 112 deletions(-) delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/CoordTypeFieldCentricOn.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/CoordTypeRobotCentricOn.java diff --git a/simgui-ds.json b/simgui-ds.json index 17e0f72..4726e2d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,12 +90,11 @@ } ], "robotJoysticks": [ - {}, { - "guid": "Keyboard1" + "guid": "Keyboard0" }, { - "guid": "Keyboard0" + "guid": "Keyboard1" } ] } diff --git a/simgui-window.json b/simgui-window.json index 81ae590..378007d 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -5,13 +5,13 @@ "MainWindow": { "GLOBAL": { "fps": "120", - "height": "720", - "maximized": "0", + "height": "1016", + "maximized": "1", "style": "0", "userScale": "2", - "width": "1280", - "xpos": "537", - "ypos": "110" + "width": "1920", + "xpos": "1101", + "ypos": "322" } }, "Window": { @@ -32,12 +32,12 @@ }, "###Other Devices": { "Collapsed": "0", - "Pos": "1025,20", + "Pos": "941,20", "Size": "250,695" }, "###System Joysticks": { "Collapsed": "0", - "Pos": "5,350", + "Pos": "20,319", "Size": "192,218" }, "###Timing": { diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index 67e298f..ffc104e 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -11,7 +11,7 @@ import java.util.Map; public final class Constants { - private static final RobotType robot = RobotType.ROBOT_2023C; + private static final RobotType robot = RobotType.ROBOT_SIMBOT; public static final double LOOP_PERIODIC_SECS = 0.02; public static final boolean TUNING_MODE = true; diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index f387836..5290c76 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import team3176.robot.commands.*; import team3176.robot.commands.drivetrain.*; import team3176.robot.commands.superstructure.claw.ClawIdle; import team3176.robot.commands.superstructure.intakecube.*; @@ -111,13 +110,13 @@ private void configureBindings() { controller.transStick.button(4).whileTrue(superstructure.prepareScoreHigh()) .onFalse((superstructure.prepareCarry())); //controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); - controller.transStick.button(10).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); + controller.transStick.button(10).onTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(drivetrain.swerveDefenseCommand())); //m_Controller.getTransStick_Button10() // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); //m_Controller.getRotStick_Button2().whileTrue(new FlipField); - controller.transStick.button(14).and(controller.transStick.button(15).onTrue(new CoordTypeFieldCentricOn())); - controller.transStick.button(14).and(controller.transStick.button(16).onTrue(new CoordTypeRobotCentricOn())); + controller.transStick.button(14).and(controller.transStick.button(15)).onTrue(drivetrain.setFieldCentric()); + controller.transStick.button(14).and(controller.transStick.button(16)).onTrue(drivetrain.setRobotCentric()); diff --git a/src/main/java/team3176/robot/commands/drivetrain/CoordTypeFieldCentricOn.java b/src/main/java/team3176/robot/commands/drivetrain/CoordTypeFieldCentricOn.java deleted file mode 100644 index 5035895..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/CoordTypeFieldCentricOn.java +++ /dev/null @@ -1,32 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.coordType; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class CoordTypeFieldCentricOn extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - public CoordTypeFieldCentricOn() { - addRequirements(drivetrain); - } - - @Override - public void initialize() { - drivetrain.setCoordType(coordType.FIELD_CENTRIC); - drivetrain.setSpinLock(false); - //drivetrain.setCoastMode(); - } - - @Override - public void execute() { - } - - @Override - public boolean isFinished() { return true; } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/CoordTypeRobotCentricOn.java b/src/main/java/team3176/robot/commands/drivetrain/CoordTypeRobotCentricOn.java deleted file mode 100644 index cd0b1e2..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/CoordTypeRobotCentricOn.java +++ /dev/null @@ -1,32 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.coordType; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class CoordTypeRobotCentricOn extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - public CoordTypeRobotCentricOn() { - addRequirements(drivetrain); - } - - @Override - public void initialize() { - drivetrain.setCoordType(coordType.ROBOT_CENTRIC); - drivetrain.setSpinLock(false); - //drivetrain.setCoastMode(); - } - - @Override - public void execute() { - } - - @Override - public boolean isFinished() { return true; } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/subsystems/RobotState.java b/src/main/java/team3176/robot/subsystems/RobotState.java index a7bf92d..383ffc3 100644 --- a/src/main/java/team3176/robot/subsystems/RobotState.java +++ b/src/main/java/team3176/robot/subsystems/RobotState.java @@ -405,7 +405,7 @@ public static RobotState getInstance() { } public Command setColorWantedState(int state) { - return this.runOnce(() -> this.setColorWantState(state)); + return this.runOnce(() -> this.setColorWantState(state)).withName("setColorWantedState"); } @Override diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 656ad51..32ea8fb 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -258,28 +258,27 @@ private void calculateNSetPodPositions() { SmartDashboard.putNumber("SpinLockYaw",getPoseYawWrapped().getDegrees()); } SwerveModuleState[] podStates = DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates(currChassisSpeeds); - Logger.getInstance().recordOutput("Drive/pod0", podStates[0].angle.getDegrees()); SwerveDriveKinematics.desaturateWheelSpeeds(podStates, DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND); SwerveModuleState[] optimizedStates = new SwerveModuleState[4]; - SwerveModuleState[] realStates = new SwerveModuleState[4]; for (int idx = 0; idx < (pods.size()); idx++) { optimizedStates[idx]=pods.get(idx).setModule(podStates[idx]); - realStates[idx] = new SwerveModuleState(pods.get(idx).getVelocity(),Rotation2d.fromDegrees(pods.get(idx).getAzimuth())); } - //Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates); - //Logger.getInstance().recordOutput("SwerveStates/real", realStates); - //Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); - //Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); - SmartDashboard.putNumber("spinCommand", spinCommand); - SmartDashboard.putNumber("pod0 m/s", podStates[0].speedMetersPerSecond); + Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates); + Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); + Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); } else { // Enter defensive position double smallNum = Math.pow(10, -5); - pods.get(0).setModule(smallNum, Rotation2d.fromRadians(1.0 * Math.PI / 8.0)); - pods.get(1).setModule(smallNum, Rotation2d.fromRadians(-1.0 * Math.PI / 8.0)); - pods.get(2).setModule(smallNum, Rotation2d.fromRadians(-3.0 * Math.PI / 8.0)); - pods.get(3).setModule(smallNum, Rotation2d.fromRadians(3.0 * Math.PI / 8.0)); + pods.get(0).setModulePositionOnly(Rotation2d.fromDegrees(-45)); + pods.get(1).setModulePositionOnly(Rotation2d.fromDegrees(45)); + pods.get(2).setModulePositionOnly(Rotation2d.fromDegrees(-45)); + pods.get(3).setModulePositionOnly(Rotation2d.fromDegrees(45)); } + SwerveModuleState[] realStates = new SwerveModuleState[4]; + for (int idx = 0; idx < (pods.size()); idx++) { + realStates[idx] = new SwerveModuleState(pods.get(idx).getVelocity(),Rotation2d.fromDegrees(pods.get(idx).getAzimuth())); + } + Logger.getInstance().recordOutput("SwerveStates/real", realStates); } @@ -459,6 +458,18 @@ public Command setCubeChaseOff() { return new InstantCommand(() -> setDriveMode(driveMode.DRIVE)); } */ + public Command setFieldCentric() { + // using instant command because I do not want these to interupt other drivetrain commands + return new InstantCommand(() -> this.setCoordType(coordType.FIELD_CENTRIC)).withName("setFieldCentric"); + } + public Command setRobotCentric() { + // using instant command because I do not want these to interupt other drivetrain commands + return new InstantCommand(() -> this.setCoordType(coordType.ROBOT_CENTRIC)).withName("setRobotCentric"); + } + public Command swerveDefenseCommand() { + return this.runEnd(() -> this.setDriveMode(driveMode.DEFENSE), () -> this.setDriveMode(driveMode.DRIVE)); + } + @Override public void periodic() { diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 3c9cd98..920306c 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -12,19 +12,18 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; - +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Subsystem; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.constants.DrivetrainHardwareMap; import team3176.robot.util.LoggedTunableNumber; import team3176.robot.util.God.*; -public class SwervePod { +public class SwervePod implements Subsystem{ /** Class Object holding the Motor controller for Drive Motor on the SwervePod */ /** Current value in radians of the azimuthEncoder's position */ - double azimuthEncoderRelPosition; - double azimuthEncoderAbsPosition; double desiredOptimizedAzimuthPosition; double velTicsPer100ms; boolean lastHasResetOccurred; @@ -78,6 +77,7 @@ public SwervePod(int id, SwervePodIO io) { turningPIDController.setP(this.kPAzimuth.get()); turningPIDController.setI(this.kIAzimuth.get()); turningPIDController.setD(this.kDAzimuth); + CommandScheduler.getInstance().registerSubsystem(this); } @@ -85,23 +85,25 @@ public SwervePod(int id, SwervePodIO io) { public void setModule(double speedMetersPerSecond, Rotation2d angle) { setModule(new SwerveModuleState(speedMetersPerSecond,angle)); } + public void setModulePositionOnly(Rotation2d angle) { + SwerveModuleState desiredState = new SwerveModuleState(0.0,angle); + SwerveModuleState desiredOptimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); + double turnOutput = turningPIDController.calculate(inputs.turnAbsolutePositionDegrees, desiredOptimized.angle.getDegrees()); + io.setTurn(MathUtil.clamp(turnOutput, -0.4, 0.4)); + } /** * alternative method for setting swervepod in line with WPILIB standard library * @param desiredState */ public SwerveModuleState setModule(SwerveModuleState desiredState) { - io.updateInputs(inputs); - Logger.getInstance().processInputs("Drive/Module" + Integer.toString(this.id), inputs); - - this.azimuthEncoderAbsPosition = inputs.turnAbsolutePositionDegrees; - SwerveModuleState desiredOptimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(this.azimuthEncoderAbsPosition)); + SwerveModuleState desiredOptimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); this.desiredOptimizedAzimuthPosition = desiredOptimized.angle.getDegrees(); double turnOutput; if (desiredState.speedMetersPerSecond > (-Math.pow(10,-10)) && desiredState.speedMetersPerSecond < (Math.pow(10,-10))) { - turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, this.lastEncoderPos); + turnOutput = turningPIDController.calculate(inputs.turnAbsolutePositionDegrees, this.lastEncoderPos); } else { - turnOutput = turningPIDController.calculate(this.azimuthEncoderAbsPosition, desiredOptimized.angle.getDegrees()); + turnOutput = turningPIDController.calculate(inputs.turnAbsolutePositionDegrees, desiredOptimized.angle.getDegrees()); this.lastEncoderPos = desiredOptimized.angle.getDegrees(); } // reduce output if the error is high @@ -109,23 +111,16 @@ public SwerveModuleState setModule(SwerveModuleState desiredState) { this.delta = currentDistance - this.lastDistance; this.lastDistance = currentDistance; - desiredOptimized.speedMetersPerSecond *= Math.abs(Math.cos(desiredOptimized.angle.minus(Rotation2d.fromDegrees(azimuthEncoderAbsPosition)).getRadians())); + desiredOptimized.speedMetersPerSecond *= Math.abs(Math.cos(desiredOptimized.angle.minus(Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)).getRadians())); //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "", id); - io.setTurn(MathUtil.clamp(turnOutput, -0.4, 0.4)); Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/error",turningPIDController.getPositionError()); //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/setpoint",turningPIDController.getSetpoint().position); this.velTicsPer100ms = Units3176.mps2ums(desiredOptimized.speedMetersPerSecond); io.setDrive(desiredOptimized.speedMetersPerSecond); - - if(kPAzimuth.hasChanged(hashCode()) || kIAzimuth.hasChanged(hashCode())) { - turningPIDController.setP(kPAzimuth.get()); - turningPIDController.setI(kIAzimuth.get()); - } // if(velAcc.hasChanged(hashCode()) || velMax.hasChanged(hashCode())){ // turningPIDController.setConstraints(new Constraints(velMax.get(),velAcc.get())); // } - return desiredOptimized; } /* @@ -171,6 +166,18 @@ public double getThrustEncoderVelocity() { return inputs.driveVelocityRadPerSec; } + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Drive/Module" + Integer.toString(this.id), inputs); + + if(kPAzimuth.hasChanged(hashCode()) || kIAzimuth.hasChanged(hashCode())) { + turningPIDController.setP(kPAzimuth.get()); + turningPIDController.setI(kIAzimuth.get()); + } + + } + public void setupShuffleboard() { Shuffleboard.getTab(this.idString) .add(idString+"/podAzimuth_setpoint_angle",DrivetrainHardwareMap.AZIMUTH_ABS_ENCODER_OFFSET_POSITION[id]) From 965a1a6ff16ba89069c86553798b8029eaa54f19 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 20 Jul 2023 13:46:16 -0600 Subject: [PATCH 06/24] superstructure cleaning --- simgui-window.json | 4 +- src/main/java/team3176/robot/Constants.java | 2 +- .../java/team3176/robot/RobotContainer.java | 34 ++--- .../robot/commands/autons/CenterAuto.java | 37 ----- .../drivetrain/AutoBalanceTeleop.java | 49 ------ .../commands/drivetrain/BlockingDelay.java | 37 ----- .../commands/drivetrain/FeederAlign.java | 96 ------------ .../robot/commands/drivetrain/FeederPID.java | 140 ------------------ .../commands/drivetrain/PathPlannerAuto.java | 5 +- .../commands/drivetrain/PathPlannerGen.java | 69 --------- .../commands/drivetrain/SwerveDefense.java | 33 ----- .../commands/drivetrain/SwerveDrive.java | 43 ------ .../robot/commands/drivetrain/TeleopPath.java | 56 ------- .../robot/commands/drivetrain/Turbo.java | 42 ------ .../commands/drivetrain/TurtleSpeed.java | 42 ------ .../superstructure/arm/ArmAnalogDown.java | 39 ----- .../superstructure/arm/ArmAnalogIdle.java | 33 ----- .../superstructure/arm/ArmAnalogUp.java | 32 ---- .../arm/ArmFollowTrajectory.java | 44 ------ .../arm/ManuallyPositionArm.java | 53 ------- .../superstructure/claw/ClawIdle.java | 29 ---- .../superstructure/claw/ClawInhaleCone.java | 52 ------- .../superstructure/claw/ClawInhaleCube.java | 51 ------- .../intakecone/IntakeConeExtendSpin.java | 53 ------- .../intakecone/IntakeConeRetractSpinot.java | 40 ----- .../superstructure/intakecone/PoopCone.java | 50 ------- .../superstructure/intakecube/PoopCube.java | 56 ------- .../subsystems/drivetrain/Drivetrain.java | 5 +- .../robot/subsystems/superstructure/Claw.java | 49 +++--- .../subsystems/superstructure/ClawIO.java | 9 +- .../superstructure/ClawIOSpark.java | 51 +++++++ .../subsystems/superstructure/IntakeCube.java | 5 +- .../superstructure/Superstructure.java | 34 +---- .../team3176/robot/util/DelayedBoolean.java | 36 +++++ 34 files changed, 147 insertions(+), 1263 deletions(-) delete mode 100644 src/main/java/team3176/robot/commands/autons/CenterAuto.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/AutoBalanceTeleop.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/BlockingDelay.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/FeederAlign.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/FeederPID.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/PathPlannerGen.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/SwerveDefense.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/Turbo.java delete mode 100644 src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCone.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCube.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeExtendSpin.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeRetractSpinot.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/intakecone/PoopCone.java delete mode 100644 src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java create mode 100644 src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java create mode 100644 src/main/java/team3176/robot/util/DelayedBoolean.java diff --git a/simgui-window.json b/simgui-window.json index 378007d..cb3d93f 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "1101", - "ypos": "322" + "xpos": "1150", + "ypos": "203" } }, "Window": { diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index ffc104e..67e298f 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -11,7 +11,7 @@ import java.util.Map; public final class Constants { - private static final RobotType robot = RobotType.ROBOT_SIMBOT; + private static final RobotType robot = RobotType.ROBOT_2023C; public static final double LOOP_PERIODIC_SECS = 0.02; public static final boolean TUNING_MODE = true; diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 5290c76..59155b1 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import team3176.robot.commands.drivetrain.*; -import team3176.robot.commands.superstructure.claw.ClawIdle; import team3176.robot.commands.superstructure.intakecube.*; import team3176.robot.constants.Hardwaremap; import team3176.robot.subsystems.RobotState; @@ -76,10 +75,10 @@ public RobotContainer() { vision = VisionCubeChase.getInstance(); superstructure = Superstructure.getInstance(); - drivetrain.setDefaultCommand(new SwerveDrive( - controller::getForward, - controller::getStrafe, - controller::getSpin)); + drivetrain.setDefaultCommand(drivetrain.swerveDrive( + () -> controller.getForward() * 0.7, + () -> controller.getStrafe() * 0.7, + () -> controller.getSpin() * 3)); //arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); autonChooser = new SendableChooser<>(); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); @@ -110,7 +109,7 @@ private void configureBindings() { controller.transStick.button(4).whileTrue(superstructure.prepareScoreHigh()) .onFalse((superstructure.prepareCarry())); //controller.transStick.button(5).onTrue(new InstantCommand(drivetrain::resetPoseToVision,drivetrain)); - controller.transStick.button(10).onTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(drivetrain.swerveDefenseCommand())); + controller.transStick.button(10).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(drivetrain.swerveDefenseCommand()).withName("swerveDefense")); //m_Controller.getTransStick_Button10() // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_Drivetrain)); @@ -121,11 +120,10 @@ private void configureBindings() { //controller.rotStick.button(1).whileTrue(new CubeChase( - controller.rotStick.button(1).whileTrue(new Turbo( - controller::getForward, - controller::getStrafe, - controller::getSpin - )); + controller.rotStick.button(1).whileTrue(drivetrain.swerveDrive( + () -> controller.getForward() * 1.0, + () -> controller.getStrafe() * 1.0, + () -> controller.getSpin() * 7)); controller.rotStick.button(2).whileTrue(new SpinLockDrive( controller::getForward, @@ -133,7 +131,7 @@ private void configureBindings() { ); - controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense()).withName("setBrakeMode")); + controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(drivetrain.swerveDefenseCommand()).withName("setBrakeMode")); controller.rotStick.button(4).whileTrue(superstructure.intakeCubeHumanPlayer()); controller.rotStick.button(4).onFalse(superstructure.prepareCarry()); @@ -168,15 +166,15 @@ private void configureBindings() { controller.operator.y().onTrue(robotState.setColorWantedState(0)); controller.operator.y().whileTrue(claw.scoreGamePiece()); - controller.operator.y().onFalse(new ClawIdle()); + controller.operator.y().onFalse(claw.idleCommand()); - controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onTrue(robotState.setColorWantedState(3)); - controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).whileTrue(new IntakeGroundCube()); - controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onFalse(intakeCube.retractSpinNot()); + // controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onTrue(robotState.setColorWantedState(3)); + // controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).whileTrue(new IntakeGroundCube()); + // controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onFalse(intakeCube.retractSpinNot()); //m_Controller.operator.rightBumper().and(m_Controller.operator.leftBumper().negate()).onFalse(m_Superstructure.prepareCarry()); - controller.operator.leftBumper().and(controller.operator.rightBumper()).whileTrue((new PoopCube())); + controller.operator.leftBumper().and(controller.operator.rightBumper()).whileTrue(superstructure.poopCube()); // m_Controller.operator.leftBumper().and(m_Controller.operator.rightBumper()).onFalse(new InstantCommand( () -> m_IntakeCone.idle())); @@ -228,7 +226,7 @@ public void printCanFaults(){ public Command getAutonomousCommand() { // An example command will be run in autonomous String chosen = autonChooser.getSelected(); - + chosen = "barrier_cone_exit_HP"; PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); } diff --git a/src/main/java/team3176/robot/commands/autons/CenterAuto.java b/src/main/java/team3176/robot/commands/autons/CenterAuto.java deleted file mode 100644 index 8cdc1ff..0000000 --- a/src/main/java/team3176/robot/commands/autons/CenterAuto.java +++ /dev/null @@ -1,37 +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. - -/** - * Center Auto: - * 1. actuate out intake - * 2. Extend Arm and spit cone - * 3. Retract Arm - * 4. Reverse about a foot or 2 - * 5. Spin 180 - * 6. Drive forward slowly to bring down ramp and drive up - * 7. Balance - * 8. Wait for auto to end - * - * (If we want to get the most points we actually want to fully cross the - * ramp and leave the community to get the 3 point for that before - * climbing the ramp. I dont think that is a realistic goal for week 1 - * tho. It would also be easier to do from a side position where we - * actually drive around the ramp and then drive up from the field side) - */ - -package team3176.robot.commands.autons; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class CenterAuto extends SequentialCommandGroup { - /** Creates a new CenterAuto. */ - public CenterAuto() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands(); - } -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/AutoBalanceTeleop.java b/src/main/java/team3176/robot/commands/drivetrain/AutoBalanceTeleop.java deleted file mode 100644 index cbc1552..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/AutoBalanceTeleop.java +++ /dev/null @@ -1,49 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - -public class AutoBalanceTeleop extends CommandBase { - private Drivetrain m_Drivetrain; - private boolean isDone = false; - private int num_balanced = 0; - public AutoBalanceTeleop() { - m_Drivetrain = Drivetrain.getInstance(); - addRequirements(m_Drivetrain); - } - @Override - public void initialize() { - // TODO Auto-generated method stub - m_Drivetrain.setBrakeMode(); - num_balanced=0; - } - @Override - public void execute() { - //double Kp = 0.1; - - //Bang Bang controller! - double forward = 0.0; - double deadbandDegrees = 6; - SmartDashboard.putNumber("pitch", m_Drivetrain.getChassisPitch()); - if(m_Drivetrain.getChassisPitch() > 0 + deadbandDegrees) { - forward = 0.40 * Math.pow(.96,num_balanced); - } else if(m_Drivetrain.getChassisPitch() < 0 - deadbandDegrees) { - forward = -0.40 * Math.pow(.96,num_balanced); - } else if(Math.abs(m_Drivetrain.getChassisPitch()) < 2){ - num_balanced ++; - } - m_Drivetrain.drive(forward, 0, 0, Drivetrain.coordType.FIELD_CENTRIC); - } - @Override - public void end(boolean interrupted) { - m_Drivetrain.setDriveMode(driveMode.DEFENSE); - } - @Override - public boolean isFinished() { - return false; - } - -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/BlockingDelay.java b/src/main/java/team3176/robot/commands/drivetrain/BlockingDelay.java deleted file mode 100644 index 770164c..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/BlockingDelay.java +++ /dev/null @@ -1,37 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - -// A totally bullshit way to delay execution of non-parallel commands in Auto - -public class BlockingDelay extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - public BlockingDelay() { - addRequirements(drivetrain); - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - } - - @Override - public boolean isFinished() { - return true; - } - - @Override - public void end(boolean interrupted) { - } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/FeederAlign.java b/src/main/java/team3176/robot/commands/drivetrain/FeederAlign.java deleted file mode 100644 index a04ad0f..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/FeederAlign.java +++ /dev/null @@ -1,96 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.DoubleTopic; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; -import team3176.robot.subsystems.vision.Vision; -import edu.wpi.first.networktables.DoubleTopic; -import edu.wpi.first.networktables.DoubleSubscriber; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class FeederAlign { - private Drivetrain m_Drivetrain; - public NetworkTableInstance tableInstance; - public NetworkTable limelightTable; - public DoubleTopic aprilIDTopic; - public DoubleSubscriber aprilID; - public Double wantedAprilID, wantedYaw, wantedXpos, wantedYpos; - public Alliance alliance; - public PathPlannerTrajectory traja; - public PPSwerveControllerCommand swerveCommand; - - public FeederAlign(){ - m_Drivetrain = Drivetrain.getInstance(); - } - - public void initialize(){ - m_Drivetrain.setBrakeMode(); - tableInstance = NetworkTableInstance.getDefault(); - limelightTable = tableInstance.getTable("limelight"); - if (DriverStation.isFMSAttached() && (alliance == null)) { - alliance = DriverStation.getAlliance(); - if (alliance == Alliance.Red){ - wantedAprilID = 5.0; - } else if (alliance == Alliance.Blue){ - wantedAprilID = 4.0; - } else if (alliance == Alliance.Invalid){ - wantedAprilID = 9.0; - } - } - if (wantedAprilID == 5.0) { - double wantedXpos = 68; //placeholder until we get the bot - double wantedYpos = 68; //placeholder until we get the bot - double wantedYaw = 0;//placeholder until we get the bot - } else if (wantedAprilID == 4.0) { - double wantedXpos = 8; //placeholder until we get the bot - double wantedYpos = 8; //placeholder until we get the bot - double wantedYaw = 0; //placeholder until we get the bot - } - - } - public void execute(){ - Pose2d pose = m_Drivetrain.getPose(); - double xpos = pose.getX(); - double ypos = pose.getY(); - - aprilIDTopic = limelightTable.getDoubleTopic("tid"); - aprilID = aprilIDTopic.subscribe(0.0); - double forward = 0.0; - if (aprilID.getAsDouble() == wantedAprilID ){ - - traja = PathPlanner.generatePath( - new PathConstraints(1, 1), - new PathPoint(new Translation2d(wantedXpos, wantedYpos),new Rotation2d(0), pose.getRotation()), // position, heading - new PathPoint(new Translation2d(wantedXpos,wantedYpos),new Rotation2d(0), new Rotation2d(wantedYaw)) // position, heading - ); - swerveCommand = new PPSwerveControllerCommand(traja, m_Drivetrain::getPose, DrivetrainConstants.DRIVE_KINEMATICS, // SwerveDriveKinematics - new PIDController(5.0, 0, 0), // X controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - new PIDController(5.0, 0, 0), // Y controller (usually the same values as X controller) - new PIDController(0.5, 0, 0), // Rotation controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - m_Drivetrain::setModuleStates, // Module states consumer - false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true - m_Drivetrain); - swerveCommand.initialize(); - } - } -} - diff --git a/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java b/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java deleted file mode 100644 index c9c34ca..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/FeederPID.java +++ /dev/null @@ -1,140 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.InterpolatingTreeMap; -import edu.wpi.first.wpilibj2.command.CommandBase; - - -import team3176.robot.subsystems.drivetrain.Drivetrain; - -public class FeederPID extends CommandBase{ - Drivetrain drivetrain; - PIDController xController = new PIDController(2.0,0.0,0.0); - PIDController yController = new PIDController(.5,0.0,0.0); - PIDController wController = new PIDController(.5,0.0,0.0); - double ltx, rtx, lty, rty, lta, rta, ltv, rtv, tx, ty, ta, tv = 0; - int numLimelights = 2; - double deadband, txSetpoint, wSetpoint; - NetworkTable vision, limelight_lfov, limelight_rfov; - InterpolatingTreeMap offsetTreeR = new InterpolatingTreeMap(); - InterpolatingTreeMap offsetTreeL = new InterpolatingTreeMap(); - InterpolatingTreeMap offsetTree; - Alliance alliance; - String side; - public FeederPID(String side) { - this.side = side; - alliance = DriverStation.getAlliance(); - drivetrain = Drivetrain.getInstance(); - addRequirements(drivetrain); - //vision = NetworkTableInstance.getDefault().getTable("limelight"); - limelight_lfov = NetworkTableInstance.getDefault().getTable("limelight-lfov"); - limelight_rfov = NetworkTableInstance.getDefault().getTable("limelight-rfov"); - - offsetTreeR.put(1.8,-20.0); - offsetTreeR.put(0.33,-5.9); - offsetTreeR.put(0.11,-0.8); - offsetTreeR.put(0.0,0.0); - - offsetTreeL.put(1.6,22.0); - offsetTreeL.put(0.46,7.1); - offsetTreeL.put(0.19,2.0); - offsetTreeL.put(0.0,0.0); - - if(side.equals("right")) { - offsetTree = offsetTreeR; - } else { - offsetTree = offsetTreeL; - } - } - @Override - public void initialize(){ - //xController.setP(2.0); - deadband = 1; - - txSetpoint = 0.0; - if (ta > 1.1 ){ - if(side == "right") { - txSetpoint = 0 ; //-20; - } else { - txSetpoint = 0 ; //20; - } - } - - drivetrain.setSpinLock(true); - if(alliance == Alliance.Red) { - wSetpoint = 0; - drivetrain.setSpinLockAngle(wSetpoint); - } else { - wSetpoint = 0; - drivetrain.setSpinLockAngle(wSetpoint); - }; - - } - @Override - public void execute() { - ltv = limelight_lfov.getEntry("tv").getDouble(0.0); - rtv = limelight_rfov.getEntry("tv").getDouble(0.0); - tv = (ltv == 1 || rtv == 1) ? 1 : 0; - lta = limelight_lfov.getEntry("ta").getDouble(0.0); - rta = limelight_rfov.getEntry("ta").getDouble(0.0); - if (ltv == 1 && rtv == 1) { - ta = (lta + rta) / numLimelights; - } else if (ltv == 1 && rtv == 0) { - ta = lta; - } else if (ltv == 0 && rtv == 1) { - ta = rta; - } - ltx = limelight_lfov.getEntry("tx").getDouble(0.0); - rtx = limelight_rfov.getEntry("tx").getDouble(0.0); - if (ltv == 1 && rtv == 1) { - tx = (ltx + rtx) / numLimelights; - } else if (ltv == 1 && rtv == 0) { - tx = ltx; - } else if (ltv == 0 && rtv == 1) { - tx = rtx; - } - lty = limelight_lfov.getEntry("ty").getDouble(0.0); - rty = limelight_rfov.getEntry("ty").getDouble(0.0); - if (ltv == 1 && rtv == 1) { - ty = (lty + rty) / numLimelights; - } else if (ltv == 1 && rtv == 0) { - ty = lty; - } else if (ltv == 0 && rtv == 1) { - ty = rty; - } - SmartDashboard.putNumber("tx", tx); - SmartDashboard.putNumber("ty", ty); - SmartDashboard.putNumber("ta", ta); - SmartDashboard.putNumber("tv", tv); - SmartDashboard.putNumber("yawWrapped", drivetrain.getPoseYawWrapped().getDegrees()); - - //if (Math.abs(m_Drivetrain.getPoseYawWrapped().getDegrees()) > 0 && tv != 0.0) { - // m_Drivetrain.drive (MathUtil.clamp(xController.calculate(ta, 1.5),-1.5,1.5), - // (MathUtil.clamp(yController.calculate(tx,txSetpoint),-1.5,1.5)), - // 0.0, coordType.ROBOT_CENTRIC); - if ((tx < (txSetpoint-deadband) || (tx > (txSetpoint+deadband)))) { - drivetrain.drive(0, - (MathUtil.clamp(-1 * yController.calculate(tx,txSetpoint), -1.5, 1.5)), - (MathUtil.clamp(-1 * wController.calculate(drivetrain.getPoseYawWrapped().getDegrees(), wSetpoint), -1.5, 1.5))); - } - //} else m_Drivetrain.drive (Math.pow(10,-7),Math.pow(10,-7),Math.pow(10,-7)); - } - - @Override - public boolean isFinished() { - if ((tx > (txSetpoint-deadband) && (tx < (txSetpoint+deadband)))) { - return true; - } else { return false; } - } - - @Override - public void end(boolean interrupted) { - drivetrain.setSpinLock(false); - } -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java b/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java index dbad6a2..73f7cdc 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java +++ b/src/main/java/team3176/robot/commands/drivetrain/PathPlannerAuto.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.commands.superstructure.intakecube.PoopCube; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.subsystems.drivetrain.Drivetrain; import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; @@ -31,12 +30,12 @@ public PathPlannerAuto(String autoPathName, Command doBefore) { HashMap eventMap = new HashMap<>(); eventMap.put("scoreHighFirst", superstructure.scoreGamePieceAuto()); eventMap.put("scoreHigh", superstructure.scoreGamePieceAuto()); - eventMap.put("autoBalance", new AutoBalance().andThen(new SwerveDefense()).finallyDo((b) -> { + eventMap.put("autoBalance", new AutoBalance().andThen(driveSubsystem.swerveDefenseCommand()).finallyDo((b) -> { driveSubsystem.setDriveMode(driveMode.DEFENSE); driveSubsystem.drive(0.0,0.0,0.0); })); eventMap.put("groundCube",superstructure.groundCube().withTimeout(3)); - eventMap.put("poopCube",new PoopCube().withTimeout(.7)); + eventMap.put("poopCube",superstructure.poopCube().withTimeout(.7)); eventMap.put("CubeAlignAtPoint", new CubeAlignAtPoint()); eventMap.put("CubeChaseOn", new CubeChaseAutonOn()); eventMap.put("CubeChaseOff", new CubeChaseOff()); diff --git a/src/main/java/team3176/robot/commands/drivetrain/PathPlannerGen.java b/src/main/java/team3176/robot/commands/drivetrain/PathPlannerGen.java deleted file mode 100644 index 8f54340..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/PathPlannerGen.java +++ /dev/null @@ -1,69 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.HashMap; -import java.util.List; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.auto.PIDConstants; -import com.pathplanner.lib.auto.SwerveAutoBuilder; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.commands.superstructure.intakecube.PoopCube; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; -import team3176.robot.subsystems.superstructure.Superstructure; - -public class PathPlannerGen { - Command auto; - public PathPlannerGen(String autoPathName, Command doBefore) { - Drivetrain driveSubsystem = Drivetrain.getInstance(); - Superstructure superstructure = Superstructure.getInstance(); - List pathGroup = PathPlanner.loadPathGroup(autoPathName, - new PathConstraints(4.5,2.0)); //2.0, 1.5 - //System.out.println("length" + pathGroup.size()); - // This is just an example event map. It would be better to have a constant, global event map - // in your code that will be used by all path following commands. - HashMap eventMap = new HashMap<>(); - eventMap.put("scoreHighFirst", superstructure.scoreGamePieceAuto()); - eventMap.put("scoreHigh", superstructure.scoreGamePieceAuto()); - eventMap.put("autoBalance", new AutoBalance().andThen(new SwerveDefense()).finallyDo((b) -> { - driveSubsystem.setDriveMode(driveMode.DEFENSE); - driveSubsystem.drive(0.0,0.0,0.0); - })); - eventMap.put("groundCube",superstructure.groundCube().withTimeout(3)); - eventMap.put("poopCube",new PoopCube().withTimeout(.7)); - eventMap.put("CubeAlignAtPoint", new CubeAlignAtPoint()); - eventMap.put("CubeChaseOn", new CubeChaseAutonOn()); - eventMap.put("CubeChaseOff", new CubeChaseOff()); - eventMap.put("delay", new WaitCommand(10.0)); - // eventMap.put("intakeDown", new IntakeDown()); - // Create the AutoBuilder. This only needs to be created once when robot code starts, not every time you want to create an auto command. A good place to put this is in RobotContainer along with your subsystems. - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( - driveSubsystem::getPose, - driveSubsystem::resetPose, - DrivetrainConstants.DRIVE_KINEMATICS, - new PIDConstants(5.0,0.0,0.0), - new PIDConstants(3.5,0.0,0.0), - driveSubsystem::setModuleStates, - eventMap, - true, driveSubsystem); - if (doBefore != null){ - auto = doBefore.andThen(autoBuilder.fullAuto(pathGroup)); - } else { - auto = autoBuilder.fullAuto(pathGroup); - } - - } - public PathPlannerGen(String autoPathName) { - this(autoPathName,null); - } - public Command getauto(){ - return auto; - } - -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/SwerveDefense.java b/src/main/java/team3176/robot/commands/drivetrain/SwerveDefense.java deleted file mode 100644 index 9f6f431..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/SwerveDefense.java +++ /dev/null @@ -1,33 +0,0 @@ - -package team3176.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - -public class SwerveDefense extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - public SwerveDefense() { - addRequirements(drivetrain); - } - - @Override - public void initialize() { - drivetrain.setDriveMode(driveMode.DEFENSE); - } - - @Override - public void execute() { - drivetrain.drive(0.0, 0.0, 0.0); - } - - @Override - public boolean isFinished() { return false; } - - @Override - public void end(boolean interrupted) { - drivetrain.setDriveMode(driveMode.DRIVE); - } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java b/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java deleted file mode 100644 index eb5e3b4..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/SwerveDrive.java +++ /dev/null @@ -1,43 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class SwerveDrive extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - - private DoubleSupplier forwardCommand; - private DoubleSupplier strafeCommand; - private DoubleSupplier spinCommand; - - public SwerveDrive( DoubleSupplier forwardCommand, DoubleSupplier strafeCommand, DoubleSupplier spinCommand) { - this.forwardCommand = forwardCommand; - this.strafeCommand = strafeCommand; - this.spinCommand = spinCommand; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - if (drivetrain.getCurrentDriveMode() != driveMode.CUBECHASETELEOP) { - drivetrain.setDriveMode(driveMode.DRIVE); - } - drivetrain.setSpinLock(false); - //drivetrain.setCoastMode(); - } - - @Override - public void execute() { - drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, - strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.7, - spinCommand.getAsDouble()*3); - } - - @Override - public boolean isFinished() { return false; } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java b/src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java deleted file mode 100644 index f1270d1..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/TeleopPath.java +++ /dev/null @@ -1,56 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import com.pathplanner.lib.*; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; - -public class TeleopPath extends CommandBase{ - Drivetrain drivetrain; - PathPlannerTrajectory traj1; - PPSwerveControllerCommand swerveCommand; - public TeleopPath() { - drivetrain = Drivetrain.getInstance(); - addRequirements(drivetrain); - } - @Override - public void initialize(){ - Pose2d pose = drivetrain.getPose(); - double xposition = pose.getX(); - double yposition = pose.getY(); - //System.out.println("pose" + xposition + "," + yposition); - traj1 = PathPlanner.generatePath( - new PathConstraints(1, 1), - new PathPoint(new Translation2d(xposition, yposition), pose.getRotation(), pose.getRotation(), drivetrain.getCurrentChassisSpeed()), // position, heading - new PathPoint(new Translation2d( 1.6, 6.74),Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180),.01), // position, heading - new PathPoint(new Translation2d( 1.1, 6.74),Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180)) - ); - //System.out.println("traj" + traj1.getTotalTimeSeconds()); - swerveCommand = new PPSwerveControllerCommand(traj1, drivetrain::getPose, DrivetrainConstants.DRIVE_KINEMATICS, // SwerveDriveKinematics - new PIDController(5.0, 0, 0), // X controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - new PIDController(5.0, 0, 0), // Y controller (usually the same values as X controller) - new PIDController(0.5, 0, 0), // Rotation controller. Tune these values for your robot. Leaving them 0 will only use feedforwards. - drivetrain::setModuleStates, // Module states consumer - false, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true - drivetrain); - swerveCommand.initialize(); - } - @Override - public void execute() { - swerveCommand.execute(); - } - @Override - public boolean isFinished() { - return swerveCommand.isFinished(); - } - @Override - public void end(boolean interrupted) { - swerveCommand.end(interrupted); - } -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/Turbo.java b/src/main/java/team3176/robot/commands/drivetrain/Turbo.java deleted file mode 100644 index 3d5a2ab..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/Turbo.java +++ /dev/null @@ -1,42 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class Turbo extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - - private DoubleSupplier forwardCommand; - private DoubleSupplier strafeCommand; - private DoubleSupplier spinCommand; - - public Turbo( DoubleSupplier forwardCommand, DoubleSupplier strafeCommand, DoubleSupplier spinCommand) { - this.forwardCommand = forwardCommand; - this.strafeCommand = strafeCommand; - this.spinCommand = spinCommand; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - drivetrain.setDriveMode(driveMode.DRIVE); - drivetrain.setSpinLock(false); - //drivetrain.setCoastMode(); - } - - @Override - public void execute() { - drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, - strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND * 1.0, - spinCommand.getAsDouble()*7); - } - - @Override - public boolean isFinished() { return false; } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java b/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java deleted file mode 100644 index 080e544..0000000 --- a/src/main/java/team3176/robot/commands/drivetrain/TurtleSpeed.java +++ /dev/null @@ -1,42 +0,0 @@ -package team3176.robot.commands.drivetrain; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.constants.DrivetrainConstants; -import team3176.robot.subsystems.drivetrain.Drivetrain; -import team3176.robot.subsystems.drivetrain.Drivetrain.driveMode; - - -public class TurtleSpeed extends CommandBase { - private Drivetrain drivetrain = Drivetrain.getInstance(); - - - private DoubleSupplier forwardCommand; - private DoubleSupplier strafeCommand; - private DoubleSupplier spinCommand; - - public TurtleSpeed( DoubleSupplier forwardCommand, DoubleSupplier strafeCommand, DoubleSupplier spinCommand) { - this.forwardCommand = forwardCommand; - this.strafeCommand = strafeCommand; - this.spinCommand = spinCommand; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - drivetrain.setDriveMode(driveMode.DRIVE); - drivetrain.setSpinLock(false); - } - - @Override - public void execute() { - drivetrain.drive(forwardCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.3, - strafeCommand.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND *0.3, - spinCommand.getAsDouble()*6); - } - - @Override - public boolean isFinished() { return false; } - -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java deleted file mode 100644 index 6e02378..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogDown.java +++ /dev/null @@ -1,39 +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 team3176.robot.commands.superstructure.arm; - - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Arm; - -public class ArmAnalogDown extends CommandBase { - /** Creates a new IntakeExtendSpin. */ - private Arm arm = Arm.getInstance(); - // private DoubleSupplier analogInput; - // private Double analogInputDeadband; - - public ArmAnalogDown() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(arm); - - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - arm.armAnalogDown(); - } - - //if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 + analogInputDeadband))) { - // m_Arm. - //} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java deleted file mode 100644 index 78f29d9..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogIdle.java +++ /dev/null @@ -1,33 +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 team3176.robot.commands.superstructure.arm; - - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Arm; - -public class ArmAnalogIdle extends CommandBase { - /** Creates a new IntakeExtendSpin. */ - private Arm arm = Arm.getInstance(); - - public ArmAnalogIdle() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(arm); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - arm.idle(); - - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java deleted file mode 100644 index 4154ef7..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/arm/ArmAnalogUp.java +++ /dev/null @@ -1,32 +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 team3176.robot.commands.superstructure.arm; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Arm; - -public class ArmAnalogUp extends CommandBase { - /** Creates a new IntakeExtendSpin. */ - private Arm arm = Arm.getInstance(); - - public ArmAnalogUp() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(arm); - - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - arm.armAnalogUp(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java b/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java deleted file mode 100644 index 85beda0..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/arm/ArmFollowTrajectory.java +++ /dev/null @@ -1,44 +0,0 @@ -package team3176.robot.commands.superstructure.arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Arm; - -public class ArmFollowTrajectory extends CommandBase { - /** Creates a new IntakeExtendSpin. */ - private Arm arm = Arm.getInstance(); - TrapezoidProfile traj; - double goalAngle; - Timer timeElapsed; - - public ArmFollowTrajectory(double goalAngle) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(arm); - this.goalAngle = goalAngle; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - traj = new TrapezoidProfile(new Constraints(20, 20), - new State(goalAngle,0.0), - new State(arm.getArmPosition(),0.0)); - timeElapsed.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - State setpoint = traj.calculate(timeElapsed.get()); - arm.setAngleSetpoint(setpoint.position); - } - - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return traj.isFinished(timeElapsed.get()); - } - } \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java b/src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java deleted file mode 100644 index 6e93310..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/arm/ManuallyPositionArm.java +++ /dev/null @@ -1,53 +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 team3176.robot.commands.superstructure.arm; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Arm; - -public class ManuallyPositionArm extends CommandBase { - /** Creates a new IntakeExtendSpin. */ - private Arm arm = Arm.getInstance(); - private DoubleSupplier analogInput; - private Double analogInputDeadband; - - public ManuallyPositionArm(DoubleSupplier analogInput) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(arm); - this.analogInput = analogInput; - this.analogInputDeadband = 0.01; - - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - Double updatedAnalogInput = this.analogInput.getAsDouble(); - if (updatedAnalogInput > (0 + analogInputDeadband)) { - arm.armAnalogUpCommand(); - } - - if (updatedAnalogInput < (0 - analogInputDeadband)) { - arm.armAnalogDownCommand(); - } - - //if ((updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 + analogInputDeadband))) { - // m_Arm. - //} - - } - - - // Returns true when the command should end. - @Override - public boolean isFinished() { - Double updatedAnalogInput = this.analogInput.getAsDouble(); - return (updatedAnalogInput < (0 + analogInputDeadband)) && (updatedAnalogInput > (0 - analogInputDeadband)); - } - -} diff --git a/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java b/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java deleted file mode 100644 index 2cf2f3e..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/claw/ClawIdle.java +++ /dev/null @@ -1,29 +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 team3176.robot.commands.superstructure.claw; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Claw; - -public class ClawIdle extends CommandBase { - /** Creates a new ClawInhale. */ - Claw claw = Claw.getInstance(); - - public ClawIdle() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(claw); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - claw.idle(); - } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCone.java b/src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCone.java deleted file mode 100644 index 1def2df..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCone.java +++ /dev/null @@ -1,52 +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 team3176.robot.commands.superstructure.claw; - - - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; - -public class ClawInhaleCone extends CommandBase { - /** Creates a new ClawInhale. */ - Claw m_Claw = Claw.getInstance(); - Timer continueRunningTimer = new Timer(); - public ClawInhaleCone() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Claw); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_Claw.setCurrentGamePiece(GamePiece.CONE); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - m_Claw.intake(); - if(m_Claw.getLinebreakTwo()) { - continueRunningTimer.restart(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - new WaitCommand(5.0); - m_Claw.hold(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return continueRunningTimer.get() > 1.0; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCube.java b/src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCube.java deleted file mode 100644 index de9110d..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/claw/ClawInhaleCube.java +++ /dev/null @@ -1,51 +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 team3176.robot.commands.superstructure.claw; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.subsystems.superstructure.IntakeCube; -import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; - -public class ClawInhaleCube extends CommandBase { - /** Creates a new ClawInhale. */ - Claw m_Claw = Claw.getInstance(); - IntakeCube m_IntakeCube = IntakeCube.getInstance(); - Timer continueRunningTimer = new Timer(); - public ClawInhaleCube() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Claw); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_Claw.setCurrentGamePiece(GamePiece.CUBE); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - m_Claw.intake(); - if(m_Claw.getLinebreakOne()) { - continueRunningTimer.restart(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_Claw.hold(); - m_IntakeCube.spinConveyor(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return continueRunningTimer.get() > 0.5; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeExtendSpin.java b/src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeExtendSpin.java deleted file mode 100644 index a5f6dee..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeExtendSpin.java +++ /dev/null @@ -1,53 +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 team3176.robot.commands.superstructure.intakecone; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.IntakeCone; -import team3176.robot.subsystems.superstructure.Claw; - -public class IntakeConeExtendSpin extends CommandBase { - /** Creates a new IntakeConeExtend. */ - IntakeCone m_IntakeCone; - Claw m_Claw; - Timer continueRunningTimer; - public IntakeConeExtendSpin() { - // Use addRequirements() here to declare subsystem dependencies. - m_IntakeCone = IntakeCone.getInstance(); - m_Claw = Claw.getInstance(); - continueRunningTimer = new Timer(); - addRequirements(m_IntakeCone, m_Claw); - } - - // 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() - { - m_IntakeCone.Extend(); - m_IntakeCone.spinVelocityPercent(.85, 25); - if (m_IntakeCone.getLinebreak() == true) - { - continueRunningTimer.restart(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) - { - m_IntakeCone.spinVelocityPercent(-.14 * 2, 5); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return continueRunningTimer.get() > 5; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeRetractSpinot.java b/src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeRetractSpinot.java deleted file mode 100644 index eeb0ef3..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/intakecone/IntakeConeRetractSpinot.java +++ /dev/null @@ -1,40 +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 team3176.robot.commands.superstructure.intakecone; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.IntakeCone; - -public class IntakeConeRetractSpinot extends CommandBase { - /** Creates a new IntakeConeRetractSpinot. */ - IntakeCone m_IntakeCone; - public IntakeConeRetractSpinot() { - // Use addRequirements() here to declare subsystem dependencies. - m_IntakeCone = IntakeCone.getInstance(); - addRequirements(m_IntakeCone); - } - - // 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() - { - m_IntakeCone.Retract(); - m_IntakeCone.spinVelocityPercent(0, 0); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecone/PoopCone.java b/src/main/java/team3176/robot/commands/superstructure/intakecone/PoopCone.java deleted file mode 100644 index 358dbe9..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/intakecone/PoopCone.java +++ /dev/null @@ -1,50 +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 team3176.robot.commands.superstructure.intakecone; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.IntakeCone; -import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.constants.SuperStructureConstants; -import team3176.robot.subsystems.superstructure.Arm; - -public class PoopCone extends CommandBase { - /** Creates a new PoopCone. */ - IntakeCone m_IntakeCone = IntakeCone.getInstance(); - Claw m_Claw = Claw.getInstance(); - Arm m_Arm = Arm.getInstance(); - public PoopCone() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_IntakeCone, m_Claw, m_Arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() - { - m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS); - // m_Claw.setClawMotor(1, 20); - // m_IntakeCone.spinVelocityPercent(-.85, 25); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - m_IntakeCone.Extend(); - m_Claw.setClawMotor(1, 20); - m_IntakeCone.spinVelocityPercent(-.85, 25); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java b/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java deleted file mode 100644 index c468315..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/intakecube/PoopCube.java +++ /dev/null @@ -1,56 +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 team3176.robot.commands.superstructure.intakecube; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.IntakeCube; -import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.constants.SuperStructureConstants; -import team3176.robot.subsystems.superstructure.Arm; - -public class PoopCube extends CommandBase { - /** Creates a new IntakeCubeSpit. */ - IntakeCube m_IntakeCube = IntakeCube.getInstance(); - Claw m_Claw = Claw.getInstance(); - Arm m_Arm = Arm.getInstance(); - public PoopCube() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_IntakeCube, m_Arm, m_Claw); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() - { - m_Arm.armSetPositionOnce(SuperStructureConstants.ARM_ZERO_POS); - m_IntakeCube.spinConveyor(0.4); - m_IntakeCube.spinIntake(1); - m_Claw.setClawMotor(-0.6, 5); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - m_IntakeCube.spinConveyor(0.6); - m_IntakeCube.spinIntake(1); - m_Claw.setClawMotor(-0.6, 5); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) - { - m_IntakeCube.spinConveyor(0); - m_IntakeCube.spinIntake(0); - m_Claw.setClawMotor(0, 0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 32ea8fb..389999f 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -38,6 +38,7 @@ import team3176.robot.subsystems.vision.VisionDual; import java.util.ArrayList; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -469,7 +470,9 @@ public Command setRobotCentric() { public Command swerveDefenseCommand() { return this.runEnd(() -> this.setDriveMode(driveMode.DEFENSE), () -> this.setDriveMode(driveMode.DRIVE)); } - + public Command swerveDrive(DoubleSupplier forward, DoubleSupplier strafe, DoubleSupplier spin) { + return this.runOnce(() -> this.setDriveMode(driveMode.DRIVE)).andThen(this.run(() -> drive(forward.getAsDouble(),strafe.getAsDouble(),spin.getAsDouble()))); + } @Override public void periodic() { diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java index 1f55413..46a6dc9 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java @@ -9,9 +9,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; +import team3176.robot.Constants; +import team3176.robot.Constants.Mode; +import team3176.robot.Constants.RobotType; import team3176.robot.constants.Hardwaremap; import team3176.robot.constants.SuperStructureConstants; import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; +import team3176.robot.util.DelayedBoolean; import team3176.robot.subsystems.superstructure.ClawIO.ClawIOInputs; import org.littletonrobotics.junction.Logger; @@ -31,48 +35,41 @@ private Claw(ClawIO io) { linebreakTwo = new DigitalInput(2); linebreakThree = new DigitalInput(1); } - public void setClawMotor(double percent, int amps) { - clawSpark.set(percent); - clawSpark.setSmartCurrentLimit(amps); - SmartDashboard.putNumber("intake power (%)", percent); - SmartDashboard.putNumber("intake motor current (amps)", clawSpark.getOutputCurrent()); - SmartDashboard.putNumber("intake motor temperature (C)", clawSpark.getMotorTemperature()); - } //states now implemented as functions public void intake() { //System.out.println("m_Claw.intake()"); if(currentGamePiece == GamePiece.CUBE) { - setClawMotor(SuperStructureConstants.CLAW_OUTPUT_POWER_CUBE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); + io.setClawMotor(SuperStructureConstants.CLAW_OUTPUT_POWER_CUBE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); } else if(currentGamePiece == GamePiece.CONE) { - setClawMotor(-SuperStructureConstants.CLAW_OUTPUT_POWER_CONE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); + io.setClawMotor(-SuperStructureConstants.CLAW_OUTPUT_POWER_CONE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); } } public void hold() { //System.out.println("m_Claw.hold()"); if(currentGamePiece == GamePiece.CUBE) { - setClawMotor(SuperStructureConstants.CLAW_HOLD_POWER,SuperStructureConstants.CLAW_HOLD_CURRENT_LIMIT_A); + io.setClawMotor(SuperStructureConstants.CLAW_HOLD_POWER,SuperStructureConstants.CLAW_HOLD_CURRENT_LIMIT_A); //idle(); } else { - setClawMotor(-SuperStructureConstants.CLAW_HOLD_POWER * SuperStructureConstants.CLAW_HOLD_CONE_FACTOR,SuperStructureConstants.CLAW_HOLD_CURRENT_LIMIT_A); + io.setClawMotor(-SuperStructureConstants.CLAW_HOLD_POWER * SuperStructureConstants.CLAW_HOLD_CONE_FACTOR,SuperStructureConstants.CLAW_HOLD_CURRENT_LIMIT_A); } } public void score() { //System.out.println("m_Claw.score()"); if(currentGamePiece == GamePiece.CUBE) { - setClawMotor(-SuperStructureConstants.CLAW_OUTPUT_POWER_CUBE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); + io.setClawMotor(-SuperStructureConstants.CLAW_OUTPUT_POWER_CUBE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); } else if(currentGamePiece == GamePiece.CONE) { - setClawMotor(SuperStructureConstants.CLAW_OUTPUT_POWER_CONE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); + io.setClawMotor(SuperStructureConstants.CLAW_OUTPUT_POWER_CONE,SuperStructureConstants.CLAW_CURRENT_LIMIT_A); } } public void idle() { //System.out.println("m_Claw.idle()"); - setClawMotor(0, 0); + io.setClawMotor(0, 0); } public void setCurrentGamePiece(GamePiece piece) { @@ -102,7 +99,12 @@ public boolean isEmpty() { public static Claw getInstance() { if (instance == null ) { - instance = new Claw(new ClawIO() {}); + if(Constants.getMode() == Mode.REAL) { + instance = new Claw(new ClawIOSpark()); + } else { + instance = new Claw(new ClawIO() {}); + } + } return instance; } @@ -128,10 +130,10 @@ public Command scoreGamePieceTeleop() { } //more examples of command composition and why its awesome!! public Command intakeCone() { - return this.intakeGamePiece(GamePiece.CONE).until(this::getLinebreakTwo).withName("intakeCone"); + return this.intakeGamePiece(GamePiece.CONE).until(new DelayedBoolean(0.5,this::getLinebreakTwo)::get).withName("intakeCone"); } public Command intakeCube() { - return this.intakeGamePiece(GamePiece.CUBE).until(this::getLinebreakOne).withName("intakeCube"); + return this.intakeGamePiece(GamePiece.CUBE).until(new DelayedBoolean(0.5,this::getLinebreakOne)::get ).withName("intakeCube"); } public Command determineGamePiece() { return this.runOnce( () -> { @@ -144,6 +146,9 @@ public Command determineGamePiece() { } }).withName("determineGamePiece"); } + public Command idleCommand() { + return this.runOnce(this::idle); + } @@ -177,14 +182,4 @@ public boolean getIsLinebreakTwo() { return inputs.isLinebreakTwo; } - - public void runVoltage(double volts) - { - io.setVoltage(volts); - } - - public void setVelocity(double velocity) - { - io.setVelocity(velocity); - } } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ClawIO.java b/src/main/java/team3176/robot/subsystems/superstructure/ClawIO.java index 63c4627..0a8a5ba 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/ClawIO.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/ClawIO.java @@ -48,12 +48,5 @@ public void fromLog(LogTable table) { public default void updateInputs(ClawIOInputs inputs) {} /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} - - /** - * Run closed loop at the specified velocity. - * - * @param velocityRadPerSec Velocity setpoint. - */ - public default void setVelocity(double velocityRadPerSec) {} + public default void setClawMotor(double percent, int amps) {} } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java b/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java new file mode 100644 index 0000000..fde1fd8 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java @@ -0,0 +1,51 @@ +// 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. +// 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 team3176.robot.subsystems.superstructure; + +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import team3176.robot.constants.Hardwaremap; +import edu.wpi.first.wpilibj.DigitalInput; + +/** Template hardware interface for a closed loop subsystem. */ +public class ClawIOSpark implements ClawIO{ + private CANSparkMax clawSpark; + private DigitalInput linebreakOne; + private DigitalInput linebreakTwo; + private DigitalInput linebreakThree; + /** Contains all of the input data received from hardware. */ + public ClawIOSpark() { + clawSpark = new CANSparkMax(Hardwaremap.claw_CID, MotorType.kBrushless); + linebreakOne = new DigitalInput(0); + linebreakTwo = new DigitalInput(2); + linebreakThree = new DigitalInput(1); + } + + + /** Updates the set of loggable inputs. */ + @Override + public void updateInputs(ClawIOInputs inputs) { + inputs.isLinebreakOne = linebreakOne.get(); + inputs.isLinebreakTwo = linebreakTwo.get(); + inputs.velocity = clawSpark.getEncoder().getVelocity(); + inputs.appliedVolts = clawSpark.getAppliedOutput() * clawSpark.getBusVoltage(); + inputs.currentAmps = new double[] {clawSpark.getOutputCurrent()}; + inputs.tempCelcius = new double[] {clawSpark.getMotorTemperature()}; + } + + + /** Run open loop at the specified voltage. */ + @Override + public void setClawMotor(double percent, int amps) { + clawSpark.set(percent); + clawSpark.setSmartCurrentLimit(amps); + } +} diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java index 710ecb3..2e373b5 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java @@ -121,7 +121,10 @@ public void setVelocity(double velocity) { io.setVelocity(velocity); } - + public Command spit() { + return this.runEnd(() -> {spinConveyor(0.6); spinIntake(1);}, + () -> {spinConveyor(0.0);spinIntake(0);}); + } public Command extendAndSpin() { return this.startEnd(() ->{ this.Extend(); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java index f2469fe..b4ee315 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java @@ -6,10 +6,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; -import team3176.robot.commands.superstructure.claw.ClawInhaleCone; -import team3176.robot.commands.superstructure.claw.ClawInhaleCube; -import team3176.robot.commands.superstructure.intakecone.IntakeConeExtendSpin; -import team3176.robot.commands.superstructure.intakecone.IntakeConeRetractSpinot; import team3176.robot.commands.superstructure.intakecube.*; import team3176.robot.constants.SuperStructureConstants; @@ -18,10 +14,12 @@ public class Superstructure extends SubsystemBase { private Arm arm; private Claw claw; private IntakeCone intakeCone; + private IntakeCube intakeCube; public Superstructure() { arm = Arm.getInstance(); claw = Claw.getInstance(); intakeCone = IntakeCone.getInstance(); + intakeCube = IntakeCube.getInstance(); } public static Superstructure getInstance() { if (instance == null){instance = new Superstructure();} @@ -37,32 +35,16 @@ public Command groundCube() { return new IntakeGroundCube().andThen(this.prepareCarry()).withName("groundCube"); } - public Command groundCone() - { - return new ParallelCommandGroup(arm.armSetPositionOnce(SuperStructureConstants.ARM_CATCH_POS), - new IntakeConeExtendSpin(), - new ClawInhaleCone()) - .until(() -> this.claw.getLinebreakThree() == false) - .andThen(intakeCone.coneToClaw()) - .andThen(new IntakeConeRetractSpinot()) - .andThen(this.prepareCarry()).withName("groundCone"); - } - public Command clawIntakeCube() { return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)).withName("clawIntakeCube"); } - /* + public Command poopCube() { - return new ParallelCommandGroup(new IntakeExtendFreeSpin()) - .andThen(this.preparePoop()) - .andThen(m_Claw.scoreGamePiece()) - .andThen(this.prepareCarry()) - .andThen(new IntakeRetractSpinot()); - //May need to add Wait Cmds in the above logic - } - */ + return new ParallelCommandGroup(arm.armSetPosition(SuperStructureConstants.ARM_ZERO_POS),intakeCube.spit(),claw.scoreGamePiece()); + } + public Command scoreGamePieceAuto() { return claw.determineGamePiece() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(1.5) @@ -100,12 +82,12 @@ public Command scoreGamePieceLowAuto() .andThen(this.prepareCarry()).withName("scoreGamePieceLowAuto"); } public Command intakeCubeHumanPlayer() { - return new ParallelCommandGroup(new ClawInhaleCube(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) + return new ParallelCommandGroup(claw.intakeCube(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeCubeHumanPlayer"); } public Command intakeConeHumanPlayer() { - return new ParallelCommandGroup(new ClawInhaleCone(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) + return new ParallelCommandGroup(claw.intakeCone(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeConeHumanPlayer"); } diff --git a/src/main/java/team3176/robot/util/DelayedBoolean.java b/src/main/java/team3176/robot/util/DelayedBoolean.java new file mode 100644 index 0000000..01f18d4 --- /dev/null +++ b/src/main/java/team3176/robot/util/DelayedBoolean.java @@ -0,0 +1,36 @@ +package team3176.robot.util; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.Timer; + +public class DelayedBoolean { + private BooleanSupplier trigger; + private double delayTime; + Timer timer = new Timer(); + boolean hasTriggered = false; + /** + * return true after a set delay of calling get for the first time + * @param seconds + * */ + public DelayedBoolean(double seconds) { + delayTime = seconds; + trigger = () -> true; + } + /** + * return true after a set delay after trigger is true + * @param seconds + * @param trigger + */ + public DelayedBoolean(double seconds, BooleanSupplier trigger) { + delayTime = seconds; + this.trigger = trigger; + } + public boolean get() { + if(trigger.getAsBoolean() && !hasTriggered) { + hasTriggered = true; + timer.restart(); + } + return timer.get() > delayTime; + } +} From 305b019617a764856c6405795fbfb5abbdc1458b Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 20 Jul 2023 14:21:04 -0600 Subject: [PATCH 07/24] fixing swervedrive scaling error --- src/main/java/team3176/robot/RobotContainer.java | 6 +++--- .../team3176/robot/subsystems/drivetrain/Drivetrain.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 59155b1..48b0ab4 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -75,7 +75,7 @@ public RobotContainer() { vision = VisionCubeChase.getInstance(); superstructure = Superstructure.getInstance(); - drivetrain.setDefaultCommand(drivetrain.swerveDrive( + drivetrain.setDefaultCommand(drivetrain.swerveDrivePercent( () -> controller.getForward() * 0.7, () -> controller.getStrafe() * 0.7, () -> controller.getSpin() * 3)); @@ -120,7 +120,7 @@ private void configureBindings() { //controller.rotStick.button(1).whileTrue(new CubeChase( - controller.rotStick.button(1).whileTrue(drivetrain.swerveDrive( + controller.rotStick.button(1).whileTrue(drivetrain.swerveDrivePercent( () -> controller.getForward() * 1.0, () -> controller.getStrafe() * 1.0, () -> controller.getSpin() * 7)); @@ -226,7 +226,7 @@ public void printCanFaults(){ public Command getAutonomousCommand() { // An example command will be run in autonomous String chosen = autonChooser.getSelected(); - chosen = "barrier_cone_exit_HP"; + chosen = "wall_3nSteal_3"; PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 389999f..77fbbe2 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -470,8 +470,8 @@ public Command setRobotCentric() { public Command swerveDefenseCommand() { return this.runEnd(() -> this.setDriveMode(driveMode.DEFENSE), () -> this.setDriveMode(driveMode.DRIVE)); } - public Command swerveDrive(DoubleSupplier forward, DoubleSupplier strafe, DoubleSupplier spin) { - return this.runOnce(() -> this.setDriveMode(driveMode.DRIVE)).andThen(this.run(() -> drive(forward.getAsDouble(),strafe.getAsDouble(),spin.getAsDouble()))); + public Command swerveDrivePercent(DoubleSupplier forward, DoubleSupplier strafe, DoubleSupplier spin) { + return this.runOnce(() -> this.setDriveMode(driveMode.DRIVE)).andThen(this.run(() -> drive(forward.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND,strafe.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND,spin.getAsDouble() * DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND))); } @Override From c09eaf4f66864eb26bcb5486faac732e83907de4 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 20 Jul 2023 14:45:07 -0600 Subject: [PATCH 08/24] removing smells --- .../java/team3176/robot/RobotContainer.java | 9 +- .../java/team3176/robot/Robot_example.txt | 346 --------------- .../commands/drivetrain/CubeChaseAuto.java | 2 +- .../intakecube/IntakeExtendFreeSpin.java | 43 -- .../intakecube/IntakeGroundCube.java | 2 +- .../intakecube/IntakeGroundCubeGuided.java | 2 +- .../robot/commands/vision/pipeSwitch.java | 44 -- .../robot/commands/vision/switchLED.java | 50 --- .../robot/constants/LoggerConstants.java | 13 - .../team3176/robot/subsystems/RobotState.java | 16 +- .../subsystems/controller/Controller.java | 193 +-------- .../subsystems/drivetrain/Drivetrain.java | 4 - .../robot/subsystems/superstructure/Arm.java | 2 +- .../robot/subsystems/superstructure/Claw.java | 47 +-- .../superstructure/ClawIOSpark.java | 5 - .../subsystems/superstructure/IntakeCone.java | 2 +- .../robot/subsystems/vision/Vision.java | 399 ------------------ 17 files changed, 21 insertions(+), 1158 deletions(-) delete mode 100644 src/main/java/team3176/robot/Robot_example.txt delete mode 100644 src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeExtendFreeSpin.java delete mode 100644 src/main/java/team3176/robot/commands/vision/pipeSwitch.java delete mode 100644 src/main/java/team3176/robot/commands/vision/switchLED.java delete mode 100644 src/main/java/team3176/robot/constants/LoggerConstants.java delete mode 100644 src/main/java/team3176/robot/subsystems/vision/Vision.java diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 48b0ab4..e03126d 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -8,14 +8,12 @@ import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import team3176.robot.commands.drivetrain.*; import team3176.robot.commands.superstructure.intakecube.*; import team3176.robot.constants.Hardwaremap; @@ -29,8 +27,6 @@ import team3176.robot.subsystems.superstructure.IntakeCone; import team3176.robot.subsystems.superstructure.Superstructure; -//import team3176.robot.subsystems.vision.VisionDual; -import team3176.robot.subsystems.vision.VisionCubeChase; /** * This class is where the bulk of the robot should be declared. Since @@ -54,7 +50,6 @@ public class RobotContainer { // is this why we don't have a compressor? private final Compressor m_Compressor private final Drivetrain drivetrain; - private final VisionCubeChase vision; private final Superstructure superstructure; private final RobotState robotState; private SendableChooser autonChooser; @@ -73,13 +68,11 @@ public RobotContainer() { robotState = RobotState.getInstance(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); - vision = VisionCubeChase.getInstance(); superstructure = Superstructure.getInstance(); drivetrain.setDefaultCommand(drivetrain.swerveDrivePercent( () -> controller.getForward() * 0.7, () -> controller.getStrafe() * 0.7, () -> controller.getSpin() * 3)); - //arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); autonChooser = new SendableChooser<>(); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); for (File f : paths.listFiles()) { @@ -226,7 +219,7 @@ public void printCanFaults(){ public Command getAutonomousCommand() { // An example command will be run in autonomous String chosen = autonChooser.getSelected(); - chosen = "wall_3nSteal_3"; + PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); } diff --git a/src/main/java/team3176/robot/Robot_example.txt b/src/main/java/team3176/robot/Robot_example.txt deleted file mode 100644 index a30fe09..0000000 --- a/src/main/java/team3176/robot/Robot_example.txt +++ /dev/null @@ -1,346 +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; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Robot_example extends TimedRobot { - /* - * Autonomous selection options. - */ - private RobotContainer m_robotContainer; - private static final String kNothingAuto = "do nothing"; - private static final String kConeAuto = "cone"; - private static final String kCubeAuto = "cube"; - private String m_autoSelected; - private final SendableChooser m_chooser = new SendableChooser<>(); - - /* - * Drive motor controller instances. - * - * Change the id's to match your robot. - * Change kBrushed to kBrushless if you are using NEO's. - * Use the appropriate other class if you are using different controllers. - */ - CANSparkMax driveLeftSpark = new CANSparkMax(1, MotorType.kBrushed); - CANSparkMax driveRightSpark = new CANSparkMax(2, MotorType.kBrushed); - VictorSPX driveLeftVictor = new VictorSPX(3); - VictorSPX driveRightVictor = new VictorSPX(4); - - /* - * Mechanism motor controller instances. - * - * Like the drive motors, set the CAN id's to match your robot or use different - * motor controller classses (TalonFX, TalonSRX, Spark, VictorSP) to match your - * robot. - * - * The arm is a NEO on Everybud. - * The intake is a NEO 550 on Everybud. - */ - CANSparkMax arm = new CANSparkMax(5, MotorType.kBrushless); - CANSparkMax intake = new CANSparkMax(6, MotorType.kBrushless); - - /** - * The starter code uses the most generic joystick class. - * - * The reveal video was filmed using a logitech gamepad set to - * directinput mode (switch set to D on the bottom). You may want - * to use the XBoxController class with the gamepad set to XInput - * mode (switch set to X on the bottom) or a different controller - * that you feel is more comfortable. - */ - Joystick j = new Joystick(0); - - /* - * Magic numbers. Use these to adjust settings. - */ - - /** - * How many amps the arm motor can use. - */ - static final int ARM_CURRENT_LIMIT_A = 20; - - /** - * Percent output to run the arm up/down at - */ - static final double ARM_OUTPUT_POWER = 0.4; - - /** - * How many amps the intake can use while picking up - */ - static final int INTAKE_CURRENT_LIMIT_A = 25; - - /** - * How many amps the intake can use while holding - */ - static final int INTAKE_HOLD_CURRENT_LIMIT_A = 5; - - /** - * Percent output for intaking - */ - static final double INTAKE_OUTPUT_POWER = 1.0; - - /** - * Percent output for holding - */ - static final double INTAKE_HOLD_POWER = 0.07; - - /** - * Time to extend or retract arm in auto - */ - static final double ARM_EXTEND_TIME_S = 2.0; - - /** - * Time to throw game piece in auto - */ - static final double AUTO_THROW_TIME_S = 0.375; - - /** - * Time to drive back in auto - */ - static final double AUTO_DRIVE_TIME = 6.0; - - /** - * Speed to drive backwards in auto - */ - static final double AUTO_DRIVE_SPEED = -0.25; - - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - /** - * This method is run once when the robot is first started up. - */ - @Override - public void robotInit() { - m_chooser.setDefaultOption("do nothing", kNothingAuto); - m_chooser.addOption("cone and mobility", kConeAuto); - m_chooser.addOption("cube and mobility", kCubeAuto); - SmartDashboard.putData("Auto choices", m_chooser); - - /* - * You will need to change some of these from false to true. - * - * In the setDriveMotors method, comment out all but 1 of the 4 calls - * to the set() methods. Push the joystick forward. Reverse the motor - * if it is going the wrong way. Repeat for the other 3 motors. - */ - driveLeftSpark.setInverted(false); - driveLeftVictor.setInverted(false); - driveRightSpark.setInverted(false); - driveRightVictor.setInverted(false); - - /* - * Set the arm and intake to brake mode to help hold position. - * If either one is reversed, change that here too. Arm out is defined - * as positive, arm in is negative. - */ - arm.setInverted(true); - arm.setIdleMode(IdleMode.kBrake); - arm.setSmartCurrentLimit(ARM_CURRENT_LIMIT_A); - intake.setInverted(false); - intake.setIdleMode(IdleMode.kBrake); - } - - /** - * Calculate and set the power to apply to the left and right - * drive motors. - * - * @param forward Desired forward speed. Positive is forward. - * @param turn Desired turning speed. Positive is counter clockwise from - * above. - */ - public void setDriveMotors(double forward, double turn) { - SmartDashboard.putNumber("drive forward power (%)", forward); - SmartDashboard.putNumber("drive turn power (%)", turn); - - /* - * positive turn = counter clockwise, so the left side goes backwards - */ - double left = forward - turn; - double right = forward + turn; - - SmartDashboard.putNumber("drive left power (%)", left); - SmartDashboard.putNumber("drive right power (%)", right); - - // see note above in robotInit about commenting these out one by one to set - // directions. - driveLeftSpark.set(left); - driveLeftVictor.set(ControlMode.PercentOutput, left); - driveRightSpark.set(right); - driveRightVictor.set(ControlMode.PercentOutput, right); - } - - /** - * Set the arm output power. Positive is out, negative is in. - * - * @param percent - */ - public void setArmMotor(double percent) { - arm.set(percent); - SmartDashboard.putNumber("arm power (%)", percent); - SmartDashboard.putNumber("arm motor current (amps)", arm.getOutputCurrent()); - SmartDashboard.putNumber("arm motor temperature (C)", arm.getMotorTemperature()); - } - - /** - * Set the arm output power. - * - * @param percent desired speed - * @param amps current limit - */ - public void setIntakeMotor(double percent, int amps) { - intake.set(percent); - intake.setSmartCurrentLimit(amps); - SmartDashboard.putNumber("intake power (%)", percent); - SmartDashboard.putNumber("intake motor current (amps)", intake.getOutputCurrent()); - SmartDashboard.putNumber("intake motor temperature (C)", intake.getMotorTemperature()); - } - - /** - * This method is called every 20 ms, no matter the mode. It runs after - * the autonomous and teleop specific period methods. - */ - @Override - public void robotPeriodic() { - SmartDashboard.putNumber("Time (seconds)", Timer.getFPGATimestamp()); - } - - double autonomousStartTime; - double autonomousIntakePower; - - @Override - public void autonomousInit() { - driveLeftSpark.setIdleMode(IdleMode.kBrake); - driveLeftVictor.setNeutralMode(NeutralMode.Brake); - driveRightSpark.setIdleMode(IdleMode.kBrake); - driveRightVictor.setNeutralMode(NeutralMode.Brake); - - m_autoSelected = m_chooser.getSelected(); - System.out.println("Auto selected: " + m_autoSelected); - - if (m_autoSelected == kConeAuto) { - autonomousIntakePower = INTAKE_OUTPUT_POWER; - } else if (m_autoSelected == kCubeAuto) { - autonomousIntakePower = -INTAKE_OUTPUT_POWER; - } - - autonomousStartTime = Timer.getFPGATimestamp(); - } - - @Override - public void autonomousPeriodic() { - if (m_autoSelected == kNothingAuto) { - setArmMotor(0.0); - setIntakeMotor(0.0, INTAKE_CURRENT_LIMIT_A); - setDriveMotors(0.0, 0.0); - return; - } - - double timeElapsed = Timer.getFPGATimestamp() - autonomousStartTime; - - if (timeElapsed < ARM_EXTEND_TIME_S) { - setArmMotor(ARM_OUTPUT_POWER); - setIntakeMotor(0.0, INTAKE_CURRENT_LIMIT_A); - setDriveMotors(0.0, 0.0); - } else if (timeElapsed < ARM_EXTEND_TIME_S + AUTO_THROW_TIME_S) { - setArmMotor(0.0); - setIntakeMotor(autonomousIntakePower, INTAKE_CURRENT_LIMIT_A); - setDriveMotors(0.0, 0.0); - } else if (timeElapsed < ARM_EXTEND_TIME_S + AUTO_THROW_TIME_S + ARM_EXTEND_TIME_S) { - setArmMotor(-ARM_OUTPUT_POWER); - setIntakeMotor(0.0, INTAKE_CURRENT_LIMIT_A); - setDriveMotors(0.0, 0.0); - } else if (timeElapsed < ARM_EXTEND_TIME_S + AUTO_THROW_TIME_S + ARM_EXTEND_TIME_S + AUTO_DRIVE_TIME) { - setArmMotor(0.0); - setIntakeMotor(0.0, INTAKE_CURRENT_LIMIT_A); - setDriveMotors(AUTO_DRIVE_SPEED, 0.0); - } else { - setArmMotor(0.0); - setIntakeMotor(0.0, INTAKE_CURRENT_LIMIT_A); - setDriveMotors(0.0, 0.0); - } - } - - /** - * Used to remember the last game piece picked up to apply some holding power. - */ - static final int CONE = 1; - static final int CUBE = 2; - static final int NOTHING = 3; - int lastGamePiece; - - @Override - public void teleopInit() { - driveLeftSpark.setIdleMode(IdleMode.kCoast); - driveLeftVictor.setNeutralMode(NeutralMode.Coast); - driveRightSpark.setIdleMode(IdleMode.kCoast); - driveRightVictor.setNeutralMode(NeutralMode.Coast); - - lastGamePiece = NOTHING; - } - - @Override - public void teleopPeriodic() { - double armPower; - if (j.getRawButton(7)) { - // lower the arm - armPower = -ARM_OUTPUT_POWER; - } else if (j.getRawButton(5)) { - // raise the arm - armPower = ARM_OUTPUT_POWER; - } else { - // do nothing and let it sit where it is - armPower = 0.0; - } - setArmMotor(armPower); - - double intakePower; - int intakeAmps; - if (j.getRawButton(8)) { - // cube in or cone out - intakePower = INTAKE_OUTPUT_POWER; - intakeAmps = INTAKE_CURRENT_LIMIT_A; - lastGamePiece = CUBE; - } else if (j.getRawButton(6)) { - // cone in or cube out - intakePower = -INTAKE_OUTPUT_POWER; - intakeAmps = INTAKE_CURRENT_LIMIT_A; - lastGamePiece = CONE; - } else if (lastGamePiece == CUBE) { - intakePower = INTAKE_HOLD_POWER; - intakeAmps = INTAKE_HOLD_CURRENT_LIMIT_A; - } else if (lastGamePiece == CONE) { - intakePower = -INTAKE_HOLD_POWER; - intakeAmps = INTAKE_HOLD_CURRENT_LIMIT_A; - } else { - intakePower = 0.0; - intakeAmps = 0; - } - setIntakeMotor(intakePower, intakeAmps); - - /* - * Negative signs here because the values from the analog sticks are backwards - * from what we want. Forward returns a negative when we want it positive. - */ - setDriveMotors(-j.getRawAxis(1), -j.getRawAxis(2)); - } -} diff --git a/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java b/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java index adb2178..8ef9aee 100644 --- a/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java +++ b/src/main/java/team3176/robot/commands/drivetrain/CubeChaseAuto.java @@ -54,7 +54,7 @@ public void execute() { @Override public boolean isFinished() { - if (claw.getLinebreakOne()) { + if (claw.getIsLinebreakOne()) { return true; } else return false; } diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeExtendFreeSpin.java b/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeExtendFreeSpin.java deleted file mode 100644 index d7eb719..0000000 --- a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeExtendFreeSpin.java +++ /dev/null @@ -1,43 +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 team3176.robot.commands.superstructure.intakecube; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.superstructure.IntakeCube; - -public class IntakeExtendFreeSpin extends CommandBase { - /** Creates a new IntakeExtendSpin. */ - IntakeCube m_IntakeCube = IntakeCube.getInstance(); - public IntakeExtendFreeSpin() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_IntakeCube); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_IntakeCube.setCoastMode(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - m_IntakeCube.Extend(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_IntakeCube.setBrakeMode(); - m_IntakeCube.Retract(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCube.java b/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCube.java index 1f37604..b859766 100644 --- a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCube.java +++ b/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCube.java @@ -70,6 +70,6 @@ public void end(boolean interrupted) // Returns true when the command should end. @Override public boolean isFinished() { - return !m_Claw.getLinebreakOne(); + return !m_Claw.getIsLinebreakOne(); } } diff --git a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCubeGuided.java b/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCubeGuided.java index 2c11bfe..5256517 100644 --- a/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCubeGuided.java +++ b/src/main/java/team3176/robot/commands/superstructure/intakecube/IntakeGroundCubeGuided.java @@ -76,6 +76,6 @@ public void end(boolean interrupted) // Returns true when the command should end. @Override public boolean isFinished() { - return !m_Claw.getLinebreakOne(); + return !m_Claw.getIsLinebreakOne(); } } diff --git a/src/main/java/team3176/robot/commands/vision/pipeSwitch.java b/src/main/java/team3176/robot/commands/vision/pipeSwitch.java deleted file mode 100644 index 61bad77..0000000 --- a/src/main/java/team3176/robot/commands/vision/pipeSwitch.java +++ /dev/null @@ -1,44 +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 team3176.robot.commands.vision; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.vision.Vision; - -public class pipeSwitch extends CommandBase { - private final Vision m_Vision = Vision.getInstance(); - public double desiredPipe; - public boolean done; - /** Creates a new pipeSwitch. */ - public pipeSwitch(double chosenPipe) { - // Use addRequirements() here to declare subsystem dependencies. - desiredPipe = chosenPipe; - addRequirements(m_Vision); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - done = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - done = true; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_Vision.pipeSetter.set(desiredPipe); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return done; - } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/commands/vision/switchLED.java b/src/main/java/team3176/robot/commands/vision/switchLED.java deleted file mode 100644 index 4ea8190..0000000 --- a/src/main/java/team3176/robot/commands/vision/switchLED.java +++ /dev/null @@ -1,50 +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 team3176.robot.commands.vision; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import team3176.robot.subsystems.vision.Vision; -import team3176.robot.subsystems.vision.Vision.LEDState; - -public class switchLED extends CommandBase { - private final Vision m_Vision = Vision.getInstance(); - public LEDState lState; - public boolean done; - /** Creates a new switchLED. */ - public switchLED() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_Vision); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - if(m_Vision.getLEDState() == 1){ - lState = LEDState.ON; - } - else{ - lState = LEDState.OFF; - } - done = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - done = true; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_Vision.switchLEDs(lState); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return done; - } -} \ No newline at end of file diff --git a/src/main/java/team3176/robot/constants/LoggerConstants.java b/src/main/java/team3176/robot/constants/LoggerConstants.java deleted file mode 100644 index 3adcd4f..0000000 --- a/src/main/java/team3176/robot/constants/LoggerConstants.java +++ /dev/null @@ -1,13 +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 team3176.robot.constants; - -public final class LoggerConstants{ - public static final boolean IS_PRACTICE_BOT = false; - public static final boolean IS_TUNING_MODE = false; - public static final boolean IS_LOGGER_ACTIVE = false; - public static final boolean IS_CMD_SCH_LOGGING_ACTIVE = true; - public static final double FULL_MATCH_TIME = 215; -} diff --git a/src/main/java/team3176/robot/subsystems/RobotState.java b/src/main/java/team3176/robot/subsystems/RobotState.java index 383ffc3..601d8a9 100644 --- a/src/main/java/team3176/robot/subsystems/RobotState.java +++ b/src/main/java/team3176/robot/subsystems/RobotState.java @@ -23,8 +23,6 @@ import team3176.robot.constants.SignalingConstants; import team3176.robot.subsystems.superstructure.Claw; -import team3176.robot.subsystems.vision.Vision.LEDState; - public class RobotState extends SubsystemBase { @@ -210,7 +208,7 @@ public void setSegment(int start, int end, Color color) { public void setallyellow() { - if (isFlashing = true) + if (isFlashing) { allflash = true; } @@ -220,7 +218,7 @@ public void setallyellow() public void setallpurple() { - if (isFlashing = true) + if (isFlashing) { allflash = true; } @@ -230,7 +228,7 @@ public void setallpurple() public void setallred() { - if (isFlashing = true) + if (isFlashing) { allflash = true; } @@ -414,7 +412,7 @@ public void periodic() { Logger.getInstance().processInputs("Intake", inputs); //(m_Claw.getLinebreakOne() == false || m_Claw.getLinebreakTwo() == false) - if (m_Claw.getLinebreakOne() == false || m_Claw.getLinebreakTwo() == false) { + if (!m_Claw.getIsLinebreakOne() || !m_Claw.getIsLinebreakTwo()) { isSolid = true; isFlashing = false; if (wantedLEDState == 1) { @@ -424,13 +422,13 @@ else if (wantedLEDState == 2 || wantedLEDState == 3) { setallpurple(); } } - else if ((m_Claw.getLinebreakOne() == true && m_Claw.getLinebreakTwo() == true) && isSolid == true) + else if ((m_Claw.getIsLinebreakOne() && m_Claw.getIsLinebreakTwo()) && isSolid) { setallblack(); wantedLEDState = 0; isSolid = false; } - else if (isSolid == false && isFlashing == true){ + else if (!isSolid && isFlashing){ // System.out.println("isSolid == false and isFlashing = true"); // if (wantedLEDState == 1) // { @@ -448,6 +446,4 @@ else if (isSolid == false && isFlashing == true){ } } - @Override - public void simulationPeriodic() {} } \ No newline at end of file diff --git a/src/main/java/team3176/robot/subsystems/controller/Controller.java b/src/main/java/team3176/robot/subsystems/controller/Controller.java index e51bd99..7b2ee6b 100644 --- a/src/main/java/team3176/robot/subsystems/controller/Controller.java +++ b/src/main/java/team3176/robot/subsystems/controller/Controller.java @@ -7,11 +7,9 @@ import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import team3176.robot.constants.ControllerConstants; -// import team3176.robot.util.XboxController.XboxAxisAsButton; -// import team3176.robot.util.XboxController.*; public class Controller { - private static Controller instance = new Controller(); + private static Controller instance; public static Controller getInstance() { if (instance == null ) { @@ -33,19 +31,6 @@ public Controller() { transStick = new CommandJoystick(ControllerConstants.TRANS_ID); rotStick = new CommandJoystick(ControllerConstants.ROT_ID); operator = new CommandXboxController(ControllerConstants.OP_ID); - - - - - /** - * The DPAD of the Xbox Controller - * The values are 0 as UP going in a 360 circle CCW - */ - - // op_DPAD_Up = new POVButton(operator, 0); - // op_DPAD_Right = new POVButton(operator, 90); - // op_DPAD_Down = new POVButton(operator, 180); - // op_DPAD_Left = new POVButton(operator, 270); } /** @@ -75,184 +60,8 @@ public double getStrafe() { * Deadband of 0.06 * @return The scales magnitude vector of the X axis of RotStick if it breaks deadband */ - public double getSpin() { if(Math.abs(rotStick.getX()) < 0.06) return 0.0; return ControllerConstants.SPIN_AXIS_INVERSION * rotStick.getX(); } - - /** - * Joystick response curve: Linear - * @return (f(x) = x) - */ - public static double joyResponseLinear(double joyInput) { - return Math.pow(joyInput, 1); - } - - /** - * Joystick response curve: 8-2 Cubic - * @return (f(x) = 0.8x+0.2x^3) - */ - public static double joyResponse82Cubic(double joyInput) { - double a = 0.8; - double b = 0.2; - return ((a * Math.pow(joyInput,1)) + (b * Math.pow(joyInput,3))); - } - - /** - * Joystick response curve: 6-4 Cubic - * @return (f(x) = 0.6x+0.4x^3) - */ - public static double joyResponse64Cubic(double joyInput) { - double a = 0.6; - double b = 0.4; - return ((a * Math.pow(joyInput,1)) + (b * Math.pow(joyInput,3))); - } - - /** - * Joystick response curve: 4-6 Cubic - * @return (f(x) = 0.4x+0.6x^3) - */ - public static double joyResponse46Cubic(double joyInput) { - double a = 0.4; - double b = 0.6; - return ((a * Math.pow(joyInput,1)) + (b * Math.pow(joyInput,3))); - } - - /** - * Joystick response curve: 2-8 Cubic - * @return (f(x) = 0.4x+0.6x^3) - */ - public static double joyResponse28Cubic(double joyInput) { - double a = 0.2; - double b = 0.8; - return ((a * Math.pow(joyInput,1)) + (b * Math.pow(joyInput,3))); - } - - /** - * Joystick response curve: Full Cubic - * @return (f(x) = x^3) - */ - public static double joyResponseFullCubic(double joyInput) { - return (Math.pow(joyInput,3)); - } - - /** - * Joystick response curve: 2parm25 - * @return (f(x) = a + (1 - a) * (b*x^3+b*x)) - * a = 0.2 - * b = 0.5 - */ - public static double joyResponse2parm25(double joyInput) { - double a = 0.2; - double b = 0.5; - if (joyInput < 0) { - return ( -a + (1 - a) * (b * Math.pow(joyInput,3) + b * joyInput)); - } else { - return ( a + (1 - a) * (b * Math.pow(joyInput,3) + b * joyInput)); - } - } - - /** - * Joystick response curve: 2parm28 - * @return (f(x) = a + (1 - a) * (b*x^3+b*x)) - * a = 0.2 - * b = 0.8 - */ - public static double joyResponse2parm28(double joyInput) { - double a = 0.2; - double b = 0.8; - if (joyInput < 0) { - return ( -a + (1 - a) * (b * Math.pow(joyInput,3) + b * joyInput)); - } else { - return ( a + (1 - a) * (b * Math.pow(joyInput,3) + b * joyInput)); - } - } - - - - - - - - - - /** - * Scale is the power of 1 - * Deadband of 0.06 - * @return The scales magnitude vector of the Y axis of RotStick if it breaks deadband - */ - - public double getOrbitSpeed() { //TODO: FIND IF WE NEED - if(Math.abs(rotStick.getY()) < 0.06) return 0.0; - return Math.pow(rotStick.getY(), 1); - } - - /** - * Scale is the power of 1 - * @return The value of the y axis of the left joystick of the Xbox Controller - */ - - public double getOp_LeftY() { - if(Math.abs(operator.getLeftY()) < 0.06) return 0.0; - return Math.pow(operator.getLeftY(), 1); - } - - /** - * Scale is the power of 1 - * @return The value of the x axis of the left joystick of the Xbox Controller - */ - - public double getOp_LeftX() { - if(Math.abs(operator.getLeftX()) < 0.06) return 0.0; - return Math.pow(operator.getLeftX(), 1); - } - - /** - * Scale is the power of 1 - * @return The value of the y axis of the right joystick of the Xbox Controller - */ - - public double getOp_RightY() { - if(Math.abs(operator.getRightY()) < 0.06) return 0.0; - return Math.pow(operator.getLeftY(), 1); - } - - /** - * Scale is the power of 1 - * @return The scales value of the x axis of the right joystick of the Xbox Controller - */ - - public double getOp_RightX() { - if(Math.abs(operator.getRightX()) < 0.06) return 0.0; - return Math.pow(operator.getRightX(), 1); - } - - - - // public Trigger getOp_A() {return op_A;} - // public Trigger getOp_A_FS() {return op_A_Shift;} - // public Trigger getOp_A_DS() {return op_A_Double_Shift;} - // public Trigger getOp_B() {return op_B;} - // public Trigger getOp_B_FS() {return op_B_Shift;} - // public Trigger getOp_B_DS() {return op_B_Double_Shift;} - // public Trigger getOp_X() {return op_X;} - // public Trigger getOp_X_FS() {return op_X_Shift;} - // public Trigger getOp_X_DS() {return op_X_Double_Shift;} - // public Trigger getOp_Y() {return op_Y;} - // public Trigger getOp_Y_FS() {return op_Y_Shift;} - // public Trigger getOp_Y_DS() {return op_Y_Double_Shift;} - // public Trigger getOp_Start() {return op_Start;} - // public Trigger getOp_Start_FS() {return op_Start_Shift;} - // public Trigger getOp_Start_DS() {return op_Start_Double_Shift;} - // public Trigger getOp_Back() {return op_Back;} - // public Trigger getOp_Back_FS() {return op_Back_Shift;} - // public Trigger getOp_Back_DS() {return op_Back_Double_Shift;} - // public Trigger getOp_LeftTrigger() {return op_LTrigger;} - // public Trigger getOp_RightTrigger() {return op_RTrigger;} - - // public POVButton getOp_DPAD_UP() {return op_DPAD_Up;} - // public POVButton getOp_DPAD_RIGHT() {return op_DPAD_Right;} - // public POVButton getOp_DPAD_DOWN() {return op_DPAD_Down;} - // public POVButton getOp_DPAD_LEFT() {return op_DPAD_Left;} } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 77fbbe2..20f5300 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -8,14 +8,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -35,7 +33,6 @@ import team3176.robot.constants.DrivetrainHardwareMap; import team3176.robot.constants.SwervePodHardwareID; import team3176.robot.subsystems.drivetrain.GyroIO.GyroIOInputs; -import team3176.robot.subsystems.vision.VisionDual; import java.util.ArrayList; import java.util.function.DoubleSupplier; @@ -269,7 +266,6 @@ private void calculateNSetPodPositions() { Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); } else { // Enter defensive position - double smallNum = Math.pow(10, -5); pods.get(0).setModulePositionOnly(Rotation2d.fromDegrees(-45)); pods.get(1).setModulePositionOnly(Rotation2d.fromDegrees(45)); pods.get(2).setModulePositionOnly(Rotation2d.fromDegrees(-45)); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java index d642bcb..fa62017 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java @@ -135,7 +135,7 @@ public void setAngleSetpoint(double setpointAngle) { * Commands */ public Command armSetPosition(double angleInDegrees) { - SmartDashboard.putNumber("armSetPosition",angleInDegrees); + Logger.getInstance().recordOutput("Arm/armSetPosition",angleInDegrees); return this.run(() -> setPIDPosition(angleInDegrees)).withName("armSetPosition"+angleInDegrees); } public Command armSetPositionBlocking(double angleInDegrees) { diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java index 46a6dc9..33d4df2 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java @@ -1,18 +1,11 @@ package team3176.robot.subsystems.superstructure; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import team3176.robot.Constants; import team3176.robot.Constants.Mode; -import team3176.robot.Constants.RobotType; -import team3176.robot.constants.Hardwaremap; import team3176.robot.constants.SuperStructureConstants; import team3176.robot.subsystems.superstructure.Superstructure.GamePiece; import team3176.robot.util.DelayedBoolean; @@ -20,20 +13,12 @@ import org.littletonrobotics.junction.Logger; public class Claw extends SubsystemBase { - private CANSparkMax clawSpark; - private DigitalInput linebreakOne; - private DigitalInput linebreakTwo; - private DigitalInput linebreakThree; private static Claw instance; private final ClawIO io; private final ClawIOInputs inputs = new ClawIOInputs(); - public GamePiece currentGamePiece = GamePiece.CONE; + private GamePiece currentGamePiece = GamePiece.CONE; private Claw(ClawIO io) { this.io = io; - clawSpark = new CANSparkMax(Hardwaremap.claw_CID, MotorType.kBrushless); - linebreakOne = new DigitalInput(0); - linebreakTwo = new DigitalInput(2); - linebreakThree = new DigitalInput(1); } //states now implemented as functions @@ -77,23 +62,8 @@ public void setCurrentGamePiece(GamePiece piece) { this.currentGamePiece = piece; } - public boolean getLinebreakOne() - { - return linebreakOne.get(); - } - - public boolean getLinebreakTwo() - { - return linebreakTwo.get(); - } - - public boolean getLinebreakThree() - { - return linebreakThree.get(); - } - public boolean isEmpty() { - return getLinebreakOne() || getLinebreakTwo(); + return getIsLinebreakOne() || getIsLinebreakTwo(); } public static Claw getInstance() @@ -130,17 +100,17 @@ public Command scoreGamePieceTeleop() { } //more examples of command composition and why its awesome!! public Command intakeCone() { - return this.intakeGamePiece(GamePiece.CONE).until(new DelayedBoolean(0.5,this::getLinebreakTwo)::get).withName("intakeCone"); + return this.intakeGamePiece(GamePiece.CONE).until(new DelayedBoolean(0.5,() -> !getIsLinebreakTwo())::get).withName("intakeCone"); } public Command intakeCube() { - return this.intakeGamePiece(GamePiece.CUBE).until(new DelayedBoolean(0.5,this::getLinebreakOne)::get ).withName("intakeCube"); + return this.intakeGamePiece(GamePiece.CUBE).until(new DelayedBoolean(0.5,() -> !getIsLinebreakOne())::get ).withName("intakeCube"); } public Command determineGamePiece() { return this.runOnce( () -> { - if(this.linebreakOne.get()) { + if(this.getIsLinebreakOne()) { this.currentGamePiece = GamePiece.CONE; hold(); - } else if(this.linebreakTwo.get()) { + } else if(this.getIsLinebreakTwo()) { this.currentGamePiece = GamePiece.CUBE; hold(); } @@ -161,9 +131,8 @@ public void periodic() { Logger.getInstance().recordOutput("Claw/LinebreakOne", getIsLinebreakOne()); Logger.getInstance().recordOutput("Claw/LinebreakTwo", getIsLinebreakTwo()); // Code stating if something is in the Intake - SmartDashboard.putBoolean("linebreakOne",linebreakOne.get()); - SmartDashboard.putBoolean("linebreakTwo",linebreakTwo.get()); - SmartDashboard.putBoolean("linebreakThree", linebreakThree.get()); + SmartDashboard.putBoolean("linebreakOne",getIsLinebreakOne()); + SmartDashboard.putBoolean("linebreakTwo",getIsLinebreakTwo()); // SmartDashboard.putBoolean("isExtended", isExtended); } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java b/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java index fde1fd8..304a379 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java @@ -7,9 +7,6 @@ package team3176.robot.subsystems.superstructure; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import team3176.robot.constants.Hardwaremap; @@ -20,13 +17,11 @@ public class ClawIOSpark implements ClawIO{ private CANSparkMax clawSpark; private DigitalInput linebreakOne; private DigitalInput linebreakTwo; - private DigitalInput linebreakThree; /** Contains all of the input data received from hardware. */ public ClawIOSpark() { clawSpark = new CANSparkMax(Hardwaremap.claw_CID, MotorType.kBrushless); linebreakOne = new DigitalInput(0); linebreakTwo = new DigitalInput(2); - linebreakThree = new DigitalInput(1); } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java index b4e6d1e..42be3e2 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java @@ -103,7 +103,7 @@ public void periodic() { public Command coneToClaw() { return this.run(() -> {spit();}) - .until(() -> this.claw.getLinebreakTwo() == false) + .until(() -> this.claw.getIsLinebreakTwo() == false) .andThen(new WaitCommand(0.5)) .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()).withName("coneToClaw"); } diff --git a/src/main/java/team3176/robot/subsystems/vision/Vision.java b/src/main/java/team3176/robot/subsystems/vision/Vision.java deleted file mode 100644 index 37e6478..0000000 --- a/src/main/java/team3176/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,399 +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 team3176.robot.subsystems.vision; - -import java.util.ArrayList; -import java.util.Arrays; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleArrayTopic; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.DoubleTopic; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import team3176.robot.constants.VisionConstants; -import edu.wpi.first.math.controller.PIDController; -public class Vision extends SubsystemBase { - - private static Vision instance = new Vision(); - - public NetworkTableInstance tableInstance; - public NetworkTable limelightTable; - public DoubleTopic tgtValid; - public DoubleSubscriber tv; - public DoubleTopic xOffset; - public DoubleSubscriber tx; - public DoubleTopic yOffset; - public DoubleSubscriber ty; - public DoubleTopic tShort; - public DoubleSubscriber tshort; - public DoubleTopic tLong; - public DoubleSubscriber tlong; - public DoubleTopic tHor; - public DoubleSubscriber thor; - public DoubleTopic tVert; - public DoubleSubscriber tvert; - public DoubleArrayTopic tCornXY; - public DoubleArraySubscriber tcornxy; - private DoubleTopic tL; - private DoubleSubscriber tl; - private DoubleTopic Pipeline; - private DoubleSubscriber pipeline; - private DoubleTopic camMODE; - private DoubleSubscriber camMode; - private DoubleTopic LEDMode; - private DoubleSubscriber ledMode; - private DoublePublisher camSetter; - private DoublePublisher ledSetter; - public DoublePublisher pipeSetter; - public DoubleTopic idTopic; - public DoubleSubscriber idSub; - - private double activePipeline = 1; - private double startTime; - - private double deltaXCam; - private double radius; - - // initializing variables for kinematic calculations - private final double gravity = 9.81; // m/s^2 - private double deltaX; // m - private double deltaY; // m - private double initialVelocity; // m/s - public double[] initialAngle = {45, 50, 55, 60, 65, 70, 75, 80, 85, 90}; // deg from horizontal - private double finalAngle; // radians - private double xVelocity; // m/s - private double initialYVelocity; // m/s - private double finalYVelocity; // m/s - private double time; // seconds - - private ArrayList tcornx = new ArrayList<>(4); - private ArrayList tcorny = new ArrayList<>(4); - - private double[] information = new double[2]; - - private PIDController visionSpinLockPID; - private boolean isVisionSpinCorrectionOn = false; - private double visionSpinCorrection; - - //private int ballLocation = -999; // -999=no ball detected, 0=ball to left, 1=ball exactly 0 degrees forward, 2=ball to right - //private double ballDegrees = -999; // degrees away from Limelight where ball is located. Positive = to left. Negative = to right. Zero = straight ahead. - - /** - * Creates the default references for VisionClient, specifically for Limelight values - */ - public Vision(){ - tableInstance = NetworkTableInstance.getDefault(); - limelightTable = tableInstance.getTable("limelight"); - updateVisionData(); - - limelightTable.getEntry("pipeline").setNumber(activePipeline); - - tcornx.ensureCapacity(4); - tcorny.ensureCapacity(4); - - visionSpinLockPID = new PIDController(0.01, 0.0, 0.0); - } - - public static Vision getInstance(){ - if (instance == null) { - instance = new Vision(); - } - return instance; - } - - /** - * Can be called to force update of VisionClient data structure - */ - public void updateVisionData(){ - tgtValid = limelightTable.getDoubleTopic("tv"); - tv = tgtValid.subscribe( 0.0); - xOffset = limelightTable.getDoubleTopic("tx"); - tx = xOffset.subscribe(0.0); - yOffset = limelightTable.getDoubleTopic("ty"); - ty = yOffset.subscribe(0.0); - tShort = limelightTable.getDoubleTopic("tshort"); - tshort = tShort.subscribe(0.0); - tLong = limelightTable.getDoubleTopic("tlong"); - tlong = tLong.subscribe(0.0); - tHor = limelightTable.getDoubleTopic("thor"); - thor = tHor.subscribe(0.0); - tVert = limelightTable.getDoubleTopic("tvert"); - tvert = tVert.subscribe(0.0); - tCornXY = limelightTable.getDoubleArrayTopic("tcornxy"); - tcornxy = tCornXY.subscribe(new double[]{}); - tL = limelightTable.getDoubleTopic("tl"); - tl = tL.subscribe(0.0); - Pipeline = limelightTable.getDoubleTopic("pipeline"); - pipeline = Pipeline.subscribe(0.0); - camMODE = limelightTable.getDoubleTopic("camMode"); - camMode = camMODE.subscribe(0.0); - LEDMode = limelightTable.getDoubleTopic("ledMode"); - ledMode = LEDMode.subscribe(0.0); - activePipeline = pipeline.get(); - camSetter = camMODE.publish(); - pipeSetter = Pipeline.publish(); - ledSetter = LEDMode.publish(); - idTopic = limelightTable.getDoubleTopic("tid"); - idSub = idTopic.subscribe(0.0); - } - public double getTargetID(){ - return idSub.get(); - } - public double getLEDState(){ - double LEDState = ledMode.get(); - return LEDState; - } - public void targetRecogControlLoop(){ - // used to calculate latency - startTime = Timer.getFPGATimestamp(); - - updateVisionData(); - - separateCornArray(); - SmartDashboard.putBoolean("Has Run?", true); - SmartDashboard.putBoolean("Empty?", tcornx.isEmpty()); - SmartDashboard.putNumber("Info", tcornx.isEmpty() ? 0 : tcornx.size()); - if(tcornx.size() < 4 || tcornx.size() > 5){ - return; - } - - deltaXCam = findDeltaX(); - SmartDashboard.putNumber("Delta X Cam", deltaXCam); - - calculateTargetDistance(); - - // get the initial velocity and angle of ball - information = findInitialAngleAndVelocity((int) Math.ceil(initialAngle.length / 2)); - - publishAllData(); - - SmartDashboard.putNumber("Latency (ms)", ((Timer.getFPGATimestamp() - startTime) * 1000) + tl.get() + 11); - } - - public void separateCornArray(){ - if(!tcornx.isEmpty() && !tcorny.isEmpty()){ - tcornx.clear(); - tcorny.clear(); - } - for(int i = 0; i < 4; i++){ - tcornx.add(limelightTable.getEntry("tx" + i).getDouble(-999.0)); - tcorny.add(limelightTable.getEntry("ty" + i).getDouble(-999.0)); - } - tcornx.remove(-999.0); - tcorny.remove(-999.0); - } - - public double findDeltaX(){ - Double[] tempCorn = tcornx.toArray(new Double[0]); - Arrays.sort(tempCorn); - return tempCorn[tempCorn.length - 1] - tempCorn[0]; - } - - public void calculateTargetDistance(){ - double rawDistance = (VisionConstants.VISION_CONSTANT / deltaXCam); - SmartDashboard.putNumber("Raw Distance", rawDistance); - deltaX = rawDistance * Math.cos(VisionConstants.cameraAngle * VisionConstants.DEG2RAD); //+ (10 * VisionConstants.INCHES2METERS); - deltaY = rawDistance * Math.sin(VisionConstants.cameraAngle * VisionConstants.DEG2RAD); - SmartDashboard.putNumber("DeltaX", deltaX); - SmartDashboard.putNumber("DeltaY", deltaY); - } - - public double[] findInitialAngleAndVelocity(int angleIdx){ - if(angleIdx >= initialAngle.length || angleIdx < 0){ - return null; - } - - findInitialVelocity(angleIdx); - - // Haha, get it? Because this variable "doublechecks" the result. Programming jokes are the best. - double check = -Math.sqrt(Math.pow(initialVelocity * Math.sin(initialAngle[angleIdx]), 2) + 2 * gravity * deltaY); - - if(check > 0){ - time = (check - initialVelocity) / gravity; - double fullCalculatedDistance = initialVelocity * Math.cos(initialAngle[angleIdx]) * time; - if(fullCalculatedDistance > deltaX){ - return findInitialAngleAndVelocity(angleIdx + 1); - } else{ - return findInitialAngleAndVelocity(angleIdx - 1); - } - } - - solveOtherVariablesFromVelocity(angleIdx); - - double[] arrayToSend = {initialVelocity, initialAngle[angleIdx]}; - return arrayToSend; - } - - private void findInitialVelocity(int angleIdx){ - double term1 = deltaX / (Math.cos(initialAngle[angleIdx])); - double term2 = -gravity / (2 * (deltaX * Math.tan(initialAngle[angleIdx]) - deltaY)); - initialVelocity = term1 * Math.sqrt(term2); - } - - private void solveOtherVariablesFromVelocity(int angleIdx){ - xVelocity = initialVelocity * Math.cos(initialAngle[angleIdx]); - initialYVelocity = initialVelocity * Math.sin(initialAngle[angleIdx]); - finalYVelocity = Math.sqrt(Math.pow(initialYVelocity, 2) + 2 * -gravity * deltaY); - time = deltaX / xVelocity; - } - - public void setVisionProcessing(boolean imageProcessing){ - if(imageProcessing){ - camSetter.set(0); - } else{ - camSetter.set(1); - } - } - - public boolean getVisionProcessing(){ - return camMode.get() == 0.0; - } - - public void setActivePipeline(int newPipeline){ - if(newPipeline > -1 && newPipeline < 4){ - activePipeline = newPipeline; - pipeSetter.set(activePipeline); - } else{ - System.out.println("Invalid Pipeline Requested, No Change Was Made"); - } - } - - private void publishAllData(){ - SmartDashboard.putNumber("initialVelocity", initialVelocity); - - SmartDashboard.putBoolean("Has Targets", (tv.get() == 1)); - SmartDashboard.putNumber("tshort", tshort.get()); - SmartDashboard.putNumber("tvert", tvert.get()); - - double numTargets = tcornx.size(); - SmartDashboard.putNumber("Number of Targets", numTargets); - - SmartDashboard.putNumber("Radius", radius); - SmartDashboard.putNumber("Horizontal Distance", deltaX); - SmartDashboard.putNumber("Vertical Distance", deltaY); - - SmartDashboard.putNumber("Distance According to Camera", deltaXCam); - - SmartDashboard.putNumber("Approx. Latency (ms)", ((Timer.getFPGATimestamp() - startTime) * 1000) + tl.get() + 11); - } - - public double getCurrentPipeline(){ - return pipeline.get(); - } - - public void switchLEDs(LEDState newLEDState){ - if(newLEDState == LEDState.OFF){ - ledSetter.set(1); - } else if(newLEDState == LEDState.ON){ - ledSetter.set(3); - } else if(newLEDState == LEDState.BLINK){ - ledSetter.set(2); - } else{ - System.out.println("Invalid LED State Requested, No Change Made"); - } - } - - - public enum LEDState { - OFF, ON, BLINK, NULL - } - - ArrayList testValues = new ArrayList(); - - /*public void averageMeasurements(double newValue){ - testValues.add(newValue); - double total = 0; - for(double value : testValues) { - total += value; - } - SmartDashboard.putNumber("Average Distance", total / testValues.size()); - System.out.println("DONE!"); - }*/ - - public double[] getVisionInformation(){ - return information; - } - - public double[] TestVisionKinematics(int angleIdx, double testDeltaX, double testDeltaY){ - if(angleIdx >= initialAngle.length || angleIdx < 0){ - SmartDashboard.putBoolean("Has Run?", true); - return null; - } - - double term1 = testDeltaX / (Math.cos(initialAngle[angleIdx])); - double term2 = gravity / (2 * ((testDeltaX * Math.tan(initialAngle[angleIdx])) - testDeltaY)); - double testInitialVelocity = term1 * Math.sqrt(term2); - SmartDashboard.putNumber("Term 1", term1); - SmartDashboard.putNumber("Term 2", term2); - SmartDashboard.putNumber("TestInitialVelocity", testInitialVelocity); - - // Haha, get it? Because this variable "doublechecks" the result. Programming jokes are the best. - double check = -Math.sqrt(Math.pow(testInitialVelocity * Math.sin(initialAngle[angleIdx]), 2) + 2 * -gravity * testDeltaY); - SmartDashboard.putNumber("Check", check); - - if(check > 0){ - double timeCheck = (check - testInitialVelocity) / gravity; - double fullCalculatedDistance = testInitialVelocity * Math.cos(initialAngle[angleIdx]) * timeCheck; - if(fullCalculatedDistance > testDeltaX){ - return TestVisionKinematics(angleIdx + 1, testDeltaX, testDeltaY); - } else{ - return TestVisionKinematics(angleIdx - 1, testDeltaX, testDeltaY); - } - } - - double testXVelocity = testInitialVelocity * Math.cos(initialAngle[angleIdx]); - double testInitialYVelocity = testInitialVelocity * Math.sin(initialAngle[angleIdx]); - double testFinalYVelocity = Math.sqrt(Math.pow(testInitialYVelocity, 2) + 2 * -gravity * testDeltaY); - double testTime = testDeltaX / testXVelocity; - - SmartDashboard.putNumber("Test X Velocity", testXVelocity); - SmartDashboard.putNumber("Test Initial Y Velocity", testInitialYVelocity); - SmartDashboard.putNumber("Test Final Y Velocity", testFinalYVelocity); - SmartDashboard.putNumber("Test Time", testTime); - - double[] arrayToSend = {testInitialVelocity, initialAngle[angleIdx]}; - return arrayToSend; - } - - public boolean getIsVisionSpinCorrectionOn(){ - return this.isVisionSpinCorrectionOn; - } - - public void setVisionSpinCorrection(boolean onOrOff){ - this.isVisionSpinCorrectionOn = onOrOff; - SmartDashboard.putBoolean("VisionSpinCorrectionOn", isVisionSpinCorrectionOn); - } - public void setVisionSpinCorrectionOn(){ - if (this.tv.get() == 1) { - setVisionSpinCorrection(true); - SmartDashboard.putBoolean("VisionSpinCorrectionOn", true); - } - } - - public void setVisionSpinCorrectionOff(){ - setVisionSpinCorrection(false); - SmartDashboard.putBoolean("VisionSpinCorrectionOn", false); - } - - - public void toggleVisionSpinCorrectionOnOff(){ - setVisionSpinCorrection(!isVisionSpinCorrectionOn); - SmartDashboard.putBoolean("VisionSpinCorrectionOn", isVisionSpinCorrectionOn); - } - - public double getVisionSpinCorrection() { - updateVisionData(); - this.visionSpinCorrection = 1 * visionSpinLockPID.calculate(this.tx.get(), 0); - return visionSpinCorrection; - } -} \ No newline at end of file From 885a1c05f3f54b63220a198909f2d98a1c63458e Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 20 Jul 2023 15:34:08 -0600 Subject: [PATCH 09/24] drivetrain refactor to more of a statemachine like --- simgui-window.json | 4 +- src/main/java/team3176/robot/Constants.java | 2 +- .../java/team3176/robot/RobotContainer.java | 2 +- .../subsystems/drivetrain/Drivetrain.java | 186 ++++++------------ 4 files changed, 61 insertions(+), 133 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 11a29ec..25c9d8d 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "1150", - "ypos": "203" + "xpos": "1148", + "ypos": "264" } }, "Window": { diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index 67e298f..ffc104e 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -11,7 +11,7 @@ import java.util.Map; public final class Constants { - private static final RobotType robot = RobotType.ROBOT_2023C; + private static final RobotType robot = RobotType.ROBOT_SIMBOT; public static final double LOOP_PERIODIC_SECS = 0.02; public static final boolean TUNING_MODE = true; diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index c42d8da..b0c14e5 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -245,7 +245,7 @@ public Command getAutonomousCommand() { if(choosenAutonomousCommand == null) { //this is if for some reason checkAutonomousSelection is never called String chosen = autonChooser.get(); - + chosen = "wall_3nSteal_3"; PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 20f5300..26ea54f 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -82,7 +82,7 @@ public enum coordType { double angleAvgRollingWindow; public enum driveMode { - DEFENSE, DRIVE, VISION, CUBECHASETELEOP, CUBECHASEAUTON + DEFENSE, DRIVE, CUBECHASETELEOP, CUBECHASEAUTON } private SwervePod podFR; @@ -221,7 +221,21 @@ public void drive(double forwardCommand, double strafeCommand, double spinComman drive(forwardCommand, strafeCommand, spinCommand, currentCoordType); } - + private ChassisSpeeds getCurrentChassisSpeedRequest() { + ChassisSpeeds currChassisSpeeds = new ChassisSpeeds(forwardCommand, strafeCommand, spinCommand); + if (this.currentCoordType == coordType.FIELD_CENTRIC) { + Rotation2d fieldOffset = this.getPose().getRotation(); + if (DriverStation.getAlliance() == Alliance.Red) { + fieldOffset.plus(Rotation2d.fromDegrees(180)); + } + currChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(currChassisSpeeds, fieldOffset); + } + if (isSpinLocked) { + currChassisSpeeds.omegaRadiansPerSecond = spinLockPID.calculate(getPoseYawWrapped().getDegrees(), spinLockAngle.getDegrees()); + SmartDashboard.putNumber("SpinLockYaw",getPoseYawWrapped().getDegrees()); + } + return currChassisSpeeds; + } /** * Robot Centric Forward, strafe, and spin to set individual pods commanded spin * speed and drive speed @@ -231,46 +245,19 @@ public void drive(double forwardCommand, double strafeCommand, double spinComman * @param spinCommand meters per second */ private void calculateNSetPodPositions() { - if (currentDriveMode != driveMode.DEFENSE) { - if (currentDriveMode == driveMode.CUBECHASETELEOP) { - if (LimelightHelpers.getTV("limelight-three")) { - this.spinCommand = calcCubeChaseSpinCommand(); - } - this.forwardCommand = -1 * this.forwardCommand; - //this.strafeCommand = -1 * this.strafeCommand; - this.strafeCommand = 0.0; - } - if (currentDriveMode == driveMode.CUBECHASEAUTON) { - this.spinCommand = calcCubeChaseSpinCommand(); - } - ChassisSpeeds currChassisSpeeds = new ChassisSpeeds(forwardCommand, strafeCommand, spinCommand); - if (this.currentCoordType == coordType.FIELD_CENTRIC) { - Rotation2d fieldOffset = this.getPose().getRotation(); - if (DriverStation.getAlliance() == Alliance.Red) { - fieldOffset.plus(Rotation2d.fromDegrees(180)); - } - currChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(currChassisSpeeds, fieldOffset); - } - if (isSpinLocked) { - currChassisSpeeds.omegaRadiansPerSecond = spinLockPID.calculate(getPoseYawWrapped().getDegrees(), spinLockAngle.getDegrees()); - SmartDashboard.putNumber("SpinLockYaw",getPoseYawWrapped().getDegrees()); - } - SwerveModuleState[] podStates = DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates(currChassisSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(podStates, DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND); - SwerveModuleState[] optimizedStates = new SwerveModuleState[4]; - for (int idx = 0; idx < (pods.size()); idx++) { - optimizedStates[idx]=pods.get(idx).setModule(podStates[idx]); - } - Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates); - Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); - Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); - - } else { // Enter defensive position - pods.get(0).setModulePositionOnly(Rotation2d.fromDegrees(-45)); - pods.get(1).setModulePositionOnly(Rotation2d.fromDegrees(45)); - pods.get(2).setModulePositionOnly(Rotation2d.fromDegrees(-45)); - pods.get(3).setModulePositionOnly(Rotation2d.fromDegrees(45)); + ChassisSpeeds currChassisSpeeds = getCurrentChassisSpeedRequest(); + SwerveModuleState[] podStates = DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates(currChassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(podStates, DrivetrainConstants.MAX_WHEEL_SPEED_METERS_PER_SECOND); + SwerveModuleState[] optimizedStates = new SwerveModuleState[4]; + for (int idx = 0; idx < (pods.size()); idx++) { + optimizedStates[idx]=pods.get(idx).setModule(podStates[idx]); } + Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates); + Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); + Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand); + } + + private void recordRealPods() { SwerveModuleState[] realStates = new SwerveModuleState[4]; for (int idx = 0; idx < (pods.size()); idx++) { realStates[idx] = new SwerveModuleState(pods.get(idx).getVelocity(),Rotation2d.fromDegrees(pods.get(idx).getAzimuth())); @@ -475,39 +462,6 @@ public void periodic() { io.updateInputs(inputs); Logger.getInstance().processInputs("Drive/gyro", inputs); - - - //vision_lfov_pose = NetworkTableInstance.getDefault().getTable("limelight-lfov").getEntry("botpose_wpiblue"); - //vision_rfov_pose = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue"); - - // double[] vision_pose_array=vision_pose.getDoubleArray(new double[6]); - // //System.out.println(vision_pose_array[0]); - // Pose2d cam_pose =new Pose2d(vision_pose_array[0],vision_pose_array[1],Rotation2d.fromDegrees(vision_pose_array[5])); - // //poseEstimator.addVisionMeasurement(cam_pose, Timer.getFPGATimestamp() - (15) / 1000); - - //commenting out because I believe we should update the limelight apriltag map - - // double xoffset = Units.inchesToMeters(285.16+ 40.45); - // double yoffset = Units.inchesToMeters(115.59 + 42.49); - // cam_pose = cam_pose.transformBy(new Transform2d(new Translation2d(xoffset,yoffset),new Rotation2d())); - - //update the pose estimator with correct timestamped values - - - - - //testing new limelight command - //LimelightHelpers.LimelightResults r = LimelightHelpers.getLatestResults("limelight"); - // LimelightHelpers.LimelightResults r = LimelightHelpers.getLatestResults("limelight"); - // SmartDashboard.putNumber("lastTimeStamp",r.targetingResults.timestamp_LIMELIGHT_publish); - // if(lastVisionTimeStamp != r.targetingResults.timestamp_LIMELIGHT_publish) { - // lastVisionTimeStamp = r.targetingResults.timestamp_LIMELIGHT_publish; - // Pose2d cam_pose = r.targetingResults.getBotPose2d(); - // //adding a fudge factor for pipeline and capture of 15 ms - // poseEstimator.addVisionMeasurement(cam_pose, Timer.getFPGATimestamp() - (15) / 1000); - - // SmartDashboard.putNumber("camX",cam_pose.getX()); - // } lastPose = odom.getPoseMeters(); SwerveModulePosition[] deltas = new SwerveModulePosition[4]; for(int i=0;i< pods.size(); i++) { @@ -521,65 +475,39 @@ public void periodic() { Logger.getInstance().recordOutput("Drive/Odom", getPose()); SmartDashboard.putNumber("NavYaw",getPoseYawWrapped().getDegrees()); - //Liam and Andrews work! - //double[] visionPoseArray = NetworkTableInstance.getDefault().getTable("limelight-rfov").getEntry("botpose_wpiblue").getDoubleArray(new double[6]); - //Pose3d visionPose3dNT = new Pose3d(visionPoseArray[0], visionPoseArray[1], visionPoseArray[2], new Rotation3d( Units.degreesToRadians(visionPoseArray[3]), Units.degreesToRadians(visionPoseArray[4]), Units.degreesToRadians(visionPoseArray[5]))); - //Logger.getInstance().recordOutput("Drive/vision_pose", visionPose3dNT); - - //new vision proposal - //visionPose3d = VisionDual.getInstance().getPose3d(); - - // double[] default_pose = {0.0,0.0,0.0,0.0,0.0,0.0}; - // try { - // double[] vision_pose_array = vision_pose.getDoubleArray(default_pose); - // Pose2d cam_pose =new Pose2d(vision_pose_array[0],vision_pose_array[1],Rotation2d.fromDegrees(vision_pose_array[5])); - // //store x value to check if its the same data as before - - // double camera_inovation_error = cam_pose.getTranslation().minus(poseEstimator.getEstimatedPosition().getTranslation()).getNorm(); - // SmartDashboard.putNumber("camInovationError",camera_inovation_error); - // if(camera_inovation_error < 1.0 && lastVisionX != cam_pose.getX() && cam_pose.getX() != 0.0){ - // lastVisionX = cam_pose.getX(); - // Transform2d diff = last_pose.minus(odom.getPoseMeters()); - // double norm = Math.abs(diff.getRotation().getRadians()) + diff.getTranslation().getNorm(); - // if(!(getPose().getX() > 3.5 && getPose().getX() < 10.5)){ - // double distanceToGrid = getPose().getX() < 7.0 ? getPose().getX() - 1.8 : 14.6 - getPose().getX(); - // double translation_cov = MathUtil.clamp(distanceToGrid, 0.9, 3.0); - // SmartDashboard.putNumber("camTransCov",translation_cov); - // //poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(translation_cov, translation_cov, translation_cov)); - // //poseEstimator.addVisionMeasurement(cam_pose, Timer.getFPGATimestamp() - vision_pose_array[6] / 1000.0, VecBuilder.fill(translation_cov, translation_cov, translation_cov)); - // } - // } - // SmartDashboard.putNumber("camX",cam_pose.getX()); - // SmartDashboard.putNumber("camY",cam_pose.getY()); - // SmartDashboard.putNumber("camW",cam_pose.getRotation().getDegrees()); - // //System.out.println("cam_pose"+cam_pose.getX()); - // //SmartDashboard.putNumber("camX",cam_pose.getX()); - // //SmartDashboard.putNumber("camY",cam_pose.getY()); - // } - // catch (ClassCastException e) { - // System.out.println("vision error" + e); - // } - calculateNSetPodPositions(); + //set pods + recordRealPods(); + switch(currentDriveMode) { + case CUBECHASEAUTON: + this.spinCommand = calcCubeChaseSpinCommand(); + calculateNSetPodPositions(); + break; + case CUBECHASETELEOP: + if (LimelightHelpers.getTV("limelight-three")) { + this.spinCommand = calcCubeChaseSpinCommand(); + } + this.forwardCommand = -1 * this.forwardCommand; + //this.strafeCommand = -1 * this.strafeCommand; + this.strafeCommand = 0.0; + calculateNSetPodPositions(); + break; + case DEFENSE: + pods.get(0).setModulePositionOnly(Rotation2d.fromDegrees(-45)); + pods.get(1).setModulePositionOnly(Rotation2d.fromDegrees(45)); + pods.get(2).setModulePositionOnly(Rotation2d.fromDegrees(-45)); + pods.get(3).setModulePositionOnly(Rotation2d.fromDegrees(45)); + break; + case DRIVE: + calculateNSetPodPositions(); + break; + default: + calculateNSetPodPositions(); + break; + } field.setRobotPose(getPose()); SmartDashboard.putData(field); - - // This method will be called once per scheduler every 500ms - - /* - this.arraytrack++; - if (this.arraytrack > 3) { - this.arraytrack = 0; - } - */ - - - SmartDashboard.putNumber("odomx", getPose().getX()); - SmartDashboard.putNumber("odomy", getPose().getY()); - SmartDashboard.putNumber("v_odomx", poseEstimator.getEstimatedPosition().getX()); - SmartDashboard.putNumber("v_odomy", poseEstimator.getEstimatedPosition().getY()); - SmartDashboard.putBoolean("Turbo", isTurboOn); //publishSwervePodPIDErrors(); // SmartDashboard.putBoolean("Defense", currentDriveMode == driveMode.DEFENSE); } From 5c324cba5514e3e3309147a139c1744f21c32c8d Mon Sep 17 00:00:00 2001 From: Jonathan Date: Sun, 6 Aug 2023 13:43:00 -0600 Subject: [PATCH 10/24] photon sim initial --- build.gradle | 3 + simgui-window.json | 16 ++-- simgui.json | 23 ++++- .../java/team3176/robot/RobotContainer.java | 3 + .../robot/subsystems/vision/SimVision.java | 92 +++++++++++++++++++ vendordeps/photonlib.json | 41 +++++++++ 6 files changed, 169 insertions(+), 9 deletions(-) create mode 100644 src/main/java/team3176/robot/subsystems/vision/SimVision.java create mode 100644 vendordeps/photonlib.json diff --git a/build.gradle b/build.gradle index 2980c0f..af5d810 100644 --- a/build.gradle +++ b/build.gradle @@ -4,6 +4,9 @@ plugins { id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } +repositories { + mavenLocal() +} sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 diff --git a/simgui-window.json b/simgui-window.json index 25c9d8d..e5ee90f 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "1148", - "ypos": "264" + "xpos": "1216", + "ypos": "272" } }, "Window": { @@ -22,17 +22,17 @@ }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,465", - "Size": "796,155" + "Pos": "43,465", + "Size": "351,155" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "244,55", - "Size": "750,185" + "Pos": "43,454", + "Size": "475,435" }, "###Other Devices": { "Collapsed": "0", - "Pos": "941,20", + "Pos": "43,20", "Size": "250,695" }, "###System Joysticks": { @@ -47,7 +47,7 @@ }, "Debug##Default": { "Collapsed": "0", - "Pos": "60,60", + "Pos": "43,60", "Size": "400,400" }, "Robot State": { diff --git a/simgui.json b/simgui.json index 08ac180..857bdcb 100644 --- a/simgui.json +++ b/simgui.json @@ -32,7 +32,28 @@ "/SmartDashboard/AutoSelector": "String Chooser", "/SmartDashboard/Auton Choice": "String Chooser", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Scheduler": "Scheduler" + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/VisionSystemSim-photonvision/Sim Field": "Field2d", + "/SmartDashboard/photonvision Sim Field": "Field2d" + } + }, + "NetworkTables": { + "transitory": { + "northstar": { + "calibration": { + "open": true + }, + "open": true + }, + "photonvision": { + "camera1": { + "open": true + }, + "open": true, + "photonvision": { + "open": true + } + } } } } diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index b0c14e5..f44cbad 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -31,6 +31,7 @@ import team3176.robot.subsystems.superstructure.IntakeCone; import team3176.robot.subsystems.superstructure.Superstructure; +import team3176.robot.subsystems.vision.SimVision; /** * This class is where the bulk of the robot should be declared. Since @@ -56,6 +57,7 @@ public class RobotContainer { private final Drivetrain drivetrain; private final Superstructure superstructure; private final RobotState robotState; + private final SimVision vision_sim; private LoggedDashboardChooser autonChooser = new LoggedDashboardChooser<>("AutoSelector"); private String choosenAutonomousString = ""; private Command choosenAutonomousCommand; @@ -73,6 +75,7 @@ public RobotContainer() { intakeCube = IntakeCube.getInstance(); intakeCone = IntakeCone.getInstance(); robotState = RobotState.getInstance(); + vision_sim = new SimVision(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); superstructure = Superstructure.getInstance(); diff --git a/src/main/java/team3176/robot/subsystems/vision/SimVision.java b/src/main/java/team3176/robot/subsystems/vision/SimVision.java new file mode 100644 index 0000000..8fc7344 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/SimVision.java @@ -0,0 +1,92 @@ +package team3176.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.List; + +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VideoSimUtil; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import team3176.robot.subsystems.drivetrain.Drivetrain; + +public class SimVision extends SubsystemBase{ + // Simulated Vision System. + // Configure these to match your PhotonVision Camera, + // pipeline, and LED setup. + static double camDiagFOV = 101; // degrees - assume wide-angle camera + static double camPitch = Units.degreesToRadians(10); // radians + static double camHeightOffGround = 0.0; // meters + static double maxLEDRange = 20; // meters + static int camResolutionWidth = 960; // pixels + static int camResolutionHeight = 720; // pixels + static double minTargetArea = 20; // square pixels + static Transform3d cameratrans = new Transform3d( + new Translation3d(0.0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)); + VisionSystemSim simVision = new VisionSystemSim("photonvision"); + PhotonCamera realCam = new PhotonCamera("camera1"); + PhotonCameraSim simCam; + public SimVision() { + double targetHeight = Units.inchesToMeters(18.22); // meters + double tgtXPos = Units.inchesToMeters(610.77); + double tgtYPos = Units.inchesToMeters(42.19); + Pose3d farTargetPose = + new Pose3d( + new Translation3d(tgtXPos, tgtYPos, targetHeight), + new Rotation3d(0.0, 0.0, 0.0)); + SimCameraProperties props = SimCameraProperties.LL2_960_720(); + simCam = new PhotonCameraSim(realCam, SimCameraProperties.LL2_960_720(),0.05,20); + simVision.addCamera(simCam, cameratrans); + VisionTargetSim t1 = new VisionTargetSim(farTargetPose, TargetModel.kTag16h5,1); + Pose3d t2pose = new Pose3d(1.0, 0.0, 0.8, new Rotation3d()); + //simVision.addVisionTargets(t1); + //simVision.addVisionTargets(new VisionTargetSim(t2pose,TargetModel.kTag16h5,2)); + try { + simVision.addVisionTargets(AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField()); + } + catch(Exception e) { + System.out.println("woops can't load the field"); + } + //PhotonCameraSim cam = new PhotonCameraSim(realCam,); + //simVision.addCamera(null, cameratrasimCamns); + // SimVisionTarget t1 = new SimVisionTarget(farTargetPose, Units.inchesToMeters(6), Units.inchesToMeters(6), 1); + // simVision.addSimVisionTarget(t1); + } + + @Override + public void periodic() { + + Pose2d currentPose = Drivetrain.getInstance().getPose(); + Pose3d current3d = new Pose3d(new Translation3d(currentPose.getX(),currentPose.getY(),0.0), new Rotation3d(0.0, 0.0, currentPose.getRotation().getRadians())); + simVision.update(currentPose); + //System.out.println(simVision.getVisionTargets().size()); + var results = realCam.getLatestResult(); + if (results.hasTargets()) { + ArrayList targets = new ArrayList(); + for(PhotonTrackedTarget t :realCam.getLatestResult().getTargets()) { + targets.add(current3d.transformBy(cameratrans).transformBy(t.getBestCameraToTarget())); + } + + Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); + //Logger.getInstance().recordOutput("photonvision/targetposeraw", new Pose3d().transformBy(cameratrans).transformBy(results.getBestTarget().getBestCameraToTarget())); + } + else { + Logger.getInstance().recordOutput("photonvision/targetpose", new Pose3d[] {}); + } + } + +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..69f803b --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "dev-v2023.4.2-30-g8397b43b", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "dev-v2023.4.2-30-g8397b43b", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "dev-v2023.4.2-30-g8397b43b" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "dev-v2023.4.2-30-g8397b43b" + } + ] + } From efaec359abfddde90cd2212b155642d849f1cccb Mon Sep 17 00:00:00 2001 From: Jonathan Date: Sun, 6 Aug 2023 14:32:51 -0600 Subject: [PATCH 11/24] cleaned for release --- .../java/team3176/robot/RobotContainer.java | 6 +-- .../{SimVision.java => SimPhotonVision.java} | 44 +++++-------------- 2 files changed, 14 insertions(+), 36 deletions(-) rename src/main/java/team3176/robot/subsystems/vision/{SimVision.java => SimPhotonVision.java} (53%) diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index f44cbad..569d47f 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -31,7 +31,7 @@ import team3176.robot.subsystems.superstructure.IntakeCone; import team3176.robot.subsystems.superstructure.Superstructure; -import team3176.robot.subsystems.vision.SimVision; +import team3176.robot.subsystems.vision.SimPhotonVision; /** * This class is where the bulk of the robot should be declared. Since @@ -57,7 +57,7 @@ public class RobotContainer { private final Drivetrain drivetrain; private final Superstructure superstructure; private final RobotState robotState; - private final SimVision vision_sim; + private final SimPhotonVision vision_sim; private LoggedDashboardChooser autonChooser = new LoggedDashboardChooser<>("AutoSelector"); private String choosenAutonomousString = ""; private Command choosenAutonomousCommand; @@ -75,7 +75,7 @@ public RobotContainer() { intakeCube = IntakeCube.getInstance(); intakeCone = IntakeCone.getInstance(); robotState = RobotState.getInstance(); - vision_sim = new SimVision(); + vision_sim = new SimPhotonVision(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); superstructure = Superstructure.getInstance(); diff --git a/src/main/java/team3176/robot/subsystems/vision/SimVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java similarity index 53% rename from src/main/java/team3176/robot/subsystems/vision/SimVision.java rename to src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 8fc7344..33c43c9 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -24,36 +24,20 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import team3176.robot.subsystems.drivetrain.Drivetrain; -public class SimVision extends SubsystemBase{ - // Simulated Vision System. - // Configure these to match your PhotonVision Camera, - // pipeline, and LED setup. - static double camDiagFOV = 101; // degrees - assume wide-angle camera - static double camPitch = Units.degreesToRadians(10); // radians - static double camHeightOffGround = 0.0; // meters - static double maxLEDRange = 20; // meters - static int camResolutionWidth = 960; // pixels - static int camResolutionHeight = 720; // pixels - static double minTargetArea = 20; // square pixels - static Transform3d cameratrans = new Transform3d( +public class SimPhotonVision extends SubsystemBase{ + // Simulated Vision System. + + double camPitch = Units.degreesToRadians(10); // radians + double camHeightOffGround = 0.8; // meters + Transform3d cameratrans = new Transform3d( new Translation3d(0.0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)); VisionSystemSim simVision = new VisionSystemSim("photonvision"); - PhotonCamera realCam = new PhotonCamera("camera1"); + PhotonCamera realCam; PhotonCameraSim simCam; - public SimVision() { - double targetHeight = Units.inchesToMeters(18.22); // meters - double tgtXPos = Units.inchesToMeters(610.77); - double tgtYPos = Units.inchesToMeters(42.19); - Pose3d farTargetPose = - new Pose3d( - new Translation3d(tgtXPos, tgtYPos, targetHeight), - new Rotation3d(0.0, 0.0, 0.0)); - SimCameraProperties props = SimCameraProperties.LL2_960_720(); + public SimPhotonVision() { + realCam = new PhotonCamera("camera1"); simCam = new PhotonCameraSim(realCam, SimCameraProperties.LL2_960_720(),0.05,20); simVision.addCamera(simCam, cameratrans); - VisionTargetSim t1 = new VisionTargetSim(farTargetPose, TargetModel.kTag16h5,1); - Pose3d t2pose = new Pose3d(1.0, 0.0, 0.8, new Rotation3d()); - //simVision.addVisionTargets(t1); //simVision.addVisionTargets(new VisionTargetSim(t2pose,TargetModel.kTag16h5,2)); try { simVision.addVisionTargets(AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField()); @@ -61,19 +45,14 @@ public SimVision() { catch(Exception e) { System.out.println("woops can't load the field"); } - //PhotonCameraSim cam = new PhotonCameraSim(realCam,); - //simVision.addCamera(null, cameratrasimCamns); - // SimVisionTarget t1 = new SimVisionTarget(farTargetPose, Units.inchesToMeters(6), Units.inchesToMeters(6), 1); - // simVision.addSimVisionTarget(t1); } @Override public void periodic() { Pose2d currentPose = Drivetrain.getInstance().getPose(); - Pose3d current3d = new Pose3d(new Translation3d(currentPose.getX(),currentPose.getY(),0.0), new Rotation3d(0.0, 0.0, currentPose.getRotation().getRadians())); + Pose3d current3d = new Pose3d(currentPose); simVision.update(currentPose); - //System.out.println(simVision.getVisionTargets().size()); var results = realCam.getLatestResult(); if (results.hasTargets()) { ArrayList targets = new ArrayList(); @@ -82,10 +61,9 @@ public void periodic() { } Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); - //Logger.getInstance().recordOutput("photonvision/targetposeraw", new Pose3d().transformBy(cameratrans).transformBy(results.getBestTarget().getBestCameraToTarget())); } else { - Logger.getInstance().recordOutput("photonvision/targetpose", new Pose3d[] {}); + Logger.getInstance().recordOutput("photonvision/targetposes", new Pose3d[] {}); } } From 2d1cba2010879a934ad974135cb12ba5071228fc Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 7 Aug 2023 16:56:11 -0600 Subject: [PATCH 12/24] solve multi tag pnp --- simgui-window.json | 6 ++-- .../subsystems/vision/SimPhotonVision.java | 30 +++++++++++++++++-- 2 files changed, 31 insertions(+), 5 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index e5ee90f..39f334b 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "1216", - "ypos": "272" + "xpos": "1600", + "ypos": "231" } }, "Window": { @@ -23,7 +23,7 @@ "###Joysticks": { "Collapsed": "0", "Pos": "43,465", - "Size": "351,155" + "Size": "536,155" }, "###NetworkTables": { "Collapsed": "0", diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 33c43c9..7b0ae3a 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -1,10 +1,14 @@ package team3176.robot.subsystems.vision; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.estimation.PNPResults; import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; @@ -12,9 +16,11 @@ import org.photonvision.simulation.VisionSystemSim; import org.photonvision.simulation.VisionTargetSim; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -34,13 +40,20 @@ public class SimPhotonVision extends SubsystemBase{ VisionSystemSim simVision = new VisionSystemSim("photonvision"); PhotonCamera realCam; PhotonCameraSim simCam; + AprilTagFieldLayout field; + HashMap targetLookup = new HashMap<>(); + public SimPhotonVision() { realCam = new PhotonCamera("camera1"); simCam = new PhotonCameraSim(realCam, SimCameraProperties.LL2_960_720(),0.05,20); simVision.addCamera(simCam, cameratrans); //simVision.addVisionTargets(new VisionTargetSim(t2pose,TargetModel.kTag16h5,2)); try { - simVision.addVisionTargets(AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField()); + field = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + simVision.addVisionTargets(field); + for (VisionTargetSim tgt: simVision.getVisionTargets()) { + targetLookup.put(tgt.fiducialID, tgt); + } } catch(Exception e) { System.out.println("woops can't load the field"); @@ -56,14 +69,27 @@ public void periodic() { var results = realCam.getLatestResult(); if (results.hasTargets()) { ArrayList targets = new ArrayList(); + ArrayList estimates = new ArrayList(); + ArrayList vertices = new ArrayList<>(); + ArrayList corners = new ArrayList<>(); for(PhotonTrackedTarget t :realCam.getLatestResult().getTargets()) { targets.add(current3d.transformBy(cameratrans).transformBy(t.getBestCameraToTarget())); + estimates.add(PhotonUtils.estimateFieldToRobotAprilTag(t.getBestCameraToTarget(), field.getTagPose(t.getFiducialId()).get() , cameratrans.inverse())); + vertices.addAll(targetLookup.get(t.getFiducialId()).getFieldVertices()); + corners.addAll(t.getDetectedCorners()); } - + + PNPResults pnpresult = OpenCVHelp.solvePNP_SQPNP(simCam.prop.getIntrinsics(), simCam.prop.getDistCoeffs(), vertices , corners); + Transform3d camera2target = pnpresult.best; + Pose3d est = new Pose3d(camera2target.inverse().getTranslation(),camera2target.inverse().getRotation()).transformBy(cameratrans.inverse()); Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); + Logger.getInstance().recordOutput("photonvision/poseEstimates", estimates.toArray(new Pose3d[estimates.size()])); + Logger.getInstance().recordOutput("photonvision/poseEstimateCustom", est); } + else { Logger.getInstance().recordOutput("photonvision/targetposes", new Pose3d[] {}); + Logger.getInstance().recordOutput("photonvision/poseEstimates", new Pose3d[] {}); } } From f6874ae28034a4b64220664be3b4926de0d51b35 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 7 Aug 2023 22:11:13 -0600 Subject: [PATCH 13/24] changed to poseEstimator --- simgui-window.json | 6 +++--- .../robot/subsystems/vision/SimPhotonVision.java | 16 ++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 39f334b..cabe512 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "1600", - "ypos": "231" + "xpos": "1266", + "ypos": "290" } }, "Window": { @@ -23,7 +23,7 @@ "###Joysticks": { "Collapsed": "0", "Pos": "43,465", - "Size": "536,155" + "Size": "554,155" }, "###NetworkTables": { "Collapsed": "0", diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 7b0ae3a..9576d9f 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -5,7 +5,9 @@ import java.util.List; import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonUtils; import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.PNPResults; @@ -42,6 +44,7 @@ public class SimPhotonVision extends SubsystemBase{ PhotonCameraSim simCam; AprilTagFieldLayout field; HashMap targetLookup = new HashMap<>(); + PhotonPoseEstimator estimator; public SimPhotonVision() { realCam = new PhotonCamera("camera1"); @@ -58,6 +61,7 @@ public SimPhotonVision() { catch(Exception e) { System.out.println("woops can't load the field"); } + estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, cameratrans); } @Override @@ -68,23 +72,19 @@ public void periodic() { simVision.update(currentPose); var results = realCam.getLatestResult(); if (results.hasTargets()) { + EstimatedRobotPose poseEst = estimator.update(results).orElse(new EstimatedRobotPose(current3d, 0.0,null)); + ArrayList targets = new ArrayList(); ArrayList estimates = new ArrayList(); - ArrayList vertices = new ArrayList<>(); - ArrayList corners = new ArrayList<>(); for(PhotonTrackedTarget t :realCam.getLatestResult().getTargets()) { targets.add(current3d.transformBy(cameratrans).transformBy(t.getBestCameraToTarget())); estimates.add(PhotonUtils.estimateFieldToRobotAprilTag(t.getBestCameraToTarget(), field.getTagPose(t.getFiducialId()).get() , cameratrans.inverse())); - vertices.addAll(targetLookup.get(t.getFiducialId()).getFieldVertices()); - corners.addAll(t.getDetectedCorners()); } - PNPResults pnpresult = OpenCVHelp.solvePNP_SQPNP(simCam.prop.getIntrinsics(), simCam.prop.getDistCoeffs(), vertices , corners); - Transform3d camera2target = pnpresult.best; - Pose3d est = new Pose3d(camera2target.inverse().getTranslation(),camera2target.inverse().getRotation()).transformBy(cameratrans.inverse()); + + Logger.getInstance().recordOutput("photonvision/multitag", poseEst.estimatedPose); Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); Logger.getInstance().recordOutput("photonvision/poseEstimates", estimates.toArray(new Pose3d[estimates.size()])); - Logger.getInstance().recordOutput("photonvision/poseEstimateCustom", est); } else { From cd5d00d45f725568e8d2a32cc2230c8bc6084793 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 7 Aug 2023 22:18:54 -0600 Subject: [PATCH 14/24] refined Optional --- simgui-window.json | 6 +++--- .../robot/subsystems/vision/SimPhotonVision.java | 10 +++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index cabe512..fd04bef 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "1266", - "ypos": "290" + "xpos": "1465", + "ypos": "308" } }, "Window": { @@ -23,7 +23,7 @@ "###Joysticks": { "Collapsed": "0", "Pos": "43,465", - "Size": "554,155" + "Size": "796,155" }, "###NetworkTables": { "Collapsed": "0", diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 9576d9f..9be07d3 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.List; +import java.util.Optional; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; @@ -72,7 +73,8 @@ public void periodic() { simVision.update(currentPose); var results = realCam.getLatestResult(); if (results.hasTargets()) { - EstimatedRobotPose poseEst = estimator.update(results).orElse(new EstimatedRobotPose(current3d, 0.0,null)); + + Optional poseEst = estimator.update(); ArrayList targets = new ArrayList(); ArrayList estimates = new ArrayList(); @@ -81,8 +83,10 @@ public void periodic() { estimates.add(PhotonUtils.estimateFieldToRobotAprilTag(t.getBestCameraToTarget(), field.getTagPose(t.getFiducialId()).get() , cameratrans.inverse())); } - - Logger.getInstance().recordOutput("photonvision/multitag", poseEst.estimatedPose); + if(poseEst.isPresent()){ + Logger.getInstance().recordOutput("photonvision/multitag", poseEst.get().estimatedPose); + } + Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); Logger.getInstance().recordOutput("photonvision/poseEstimates", estimates.toArray(new Pose3d[estimates.size()])); } From 4414acd26bb3635a1de9d3c732f4c81f2ffc5ebf Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 8 Aug 2023 12:10:21 -0600 Subject: [PATCH 15/24] kalman filter and odometry noise --- simgui-window.json | 6 +- .../java/team3176/robot/RobotContainer.java | 5 +- .../subsystems/drivetrain/Drivetrain.java | 54 +++++++++--- .../subsystems/drivetrain/SwervePod.java | 27 ++++-- .../subsystems/vision/PhotonVisionSystem.java | 87 +++++++++++++++++++ .../subsystems/vision/SimPhotonVision.java | 66 ++------------ 6 files changed, 165 insertions(+), 80 deletions(-) create mode 100644 src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java diff --git a/simgui-window.json b/simgui-window.json index fd04bef..e7cd0e3 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -6,12 +6,12 @@ "GLOBAL": { "fps": "120", "height": "1016", - "maximized": "1", + "maximized": "0", "style": "0", "userScale": "2", "width": "1920", - "xpos": "1465", - "ypos": "308" + "xpos": "351", + "ypos": "906" } }, "Window": { diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 569d47f..489500b 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -31,6 +31,7 @@ import team3176.robot.subsystems.superstructure.IntakeCone; import team3176.robot.subsystems.superstructure.Superstructure; +import team3176.robot.subsystems.vision.PhotonVisionSystem; import team3176.robot.subsystems.vision.SimPhotonVision; /** @@ -57,7 +58,7 @@ public class RobotContainer { private final Drivetrain drivetrain; private final Superstructure superstructure; private final RobotState robotState; - private final SimPhotonVision vision_sim; + private final PhotonVisionSystem vision; private LoggedDashboardChooser autonChooser = new LoggedDashboardChooser<>("AutoSelector"); private String choosenAutonomousString = ""; private Command choosenAutonomousCommand; @@ -75,7 +76,7 @@ public RobotContainer() { intakeCube = IntakeCube.getInstance(); intakeCone = IntakeCone.getInstance(); robotState = RobotState.getInstance(); - vision_sim = new SimPhotonVision(); + vision = PhotonVisionSystem.getInstance(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); superstructure = Superstructure.getInstance(); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 26ea54f..dce7bb4 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -14,12 +14,16 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.*; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.wpilibj.DriverStation; @@ -35,9 +39,11 @@ import team3176.robot.subsystems.drivetrain.GyroIO.GyroIOInputs; import java.util.ArrayList; +import java.util.List; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; public class Drivetrain extends SubsystemBase { private static Drivetrain instance; @@ -277,6 +283,26 @@ public driveMode getCurrentDriveMode() { public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } + public Pose2d getPoseOdomTrue() { + return odom.getPoseMeters(); + } + public void addVisionPose(EstimatedRobotPose p) { + Matrix cov = new Matrix<>(Nat.N3(), Nat.N1()); + double distance = 0.0; + for(var t : p.targetsUsed) { + distance += t.getBestCameraToTarget().getTranslation().getNorm() / p.targetsUsed.size(); + } + Logger.getInstance().recordOutput("Drivetrain/distance2target", distance); + if (p.targetsUsed.size() > 1) { + //multi tag + double distance2 = Math.max(Math.pow(distance * 0.2,2),0.7); + cov = VecBuilder.fill(distance2,distance2,0.9); + } else { + double distance2 = Math.pow(distance * 0.5, 2); + cov = VecBuilder.fill(distance2,distance2,distance2); + } + poseEstimator.addVisionMeasurement(p.estimatedPose.toPose2d(), p.timestampSeconds,cov); + } public void resetPose(Pose2d pose) { wheelOnlyHeading = pose.getRotation(); @@ -396,14 +422,17 @@ public double getPodAzimuth(int podID) { return pods.get(podID).getAzimuth(); } - public SwerveModulePosition[] getSwerveModulePositions() { + public SwerveModulePosition[] getSwerveModulePositions(boolean addNoiseIfSim) { return new SwerveModulePosition[] { - podFR.getPosition(), - podFL.getPosition(), - podBL.getPosition(), - podBR.getPosition() + podFR.getPosition(addNoiseIfSim), + podFL.getPosition(addNoiseIfSim), + podBL.getPosition(addNoiseIfSim), + podBR.getPosition(addNoiseIfSim) }; } + public SwerveModulePosition[] getSwerveModulePositions() { + return getSwerveModulePositions(true); + } public void setCoastMode() { for (int idx = 0; idx < (pods.size()); idx++) { @@ -461,18 +490,23 @@ public Command swerveDrivePercent(DoubleSupplier forward, DoubleSupplier strafe, public void periodic() { io.updateInputs(inputs); Logger.getInstance().processInputs("Drive/gyro", inputs); - - lastPose = odom.getPoseMeters(); + lastPose = poseEstimator.getEstimatedPosition(); SwerveModulePosition[] deltas = new SwerveModulePosition[4]; for(int i=0;i< pods.size(); i++) { deltas[i] = pods.get(i).getDelta(); } Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); - wheelOnlyHeading = getPose().exp(twist).getRotation(); + wheelOnlyHeading = getPoseOdomTrue().exp(twist).getRotation(); // update encoders - this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions()); - this.odom.update(getSensorYaw(), getSwerveModulePositions()); + this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions(true)); + this.odom.update(getSensorYaw(), getSwerveModulePositions(false)); + Logger.getInstance().recordOutput("Drive/Odom", getPose()); + + if(Constants.getMode() == Mode.SIM) { + Logger.getInstance().recordOutput("Drive/OdomTrue", getPoseOdomTrue()); + } + SmartDashboard.putNumber("NavYaw",getPoseYawWrapped().getDegrees()); //set pods diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 920306c..4219e8a 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -1,6 +1,8 @@ package team3176.robot.subsystems.drivetrain; import java.util.Map; +import java.util.Random; +import java.util.random.RandomGenerator; import org.littletonrobotics.junction.Logger; @@ -14,6 +16,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Subsystem; +import team3176.robot.Constants; +import team3176.robot.Constants.Mode; import team3176.robot.constants.DrivetrainConstants; import team3176.robot.constants.DrivetrainHardwareMap; import team3176.robot.util.LoggedTunableNumber; @@ -48,6 +52,7 @@ public class SwervePod implements Subsystem{ private double kDAzimuth; private double lastDistance =0.0; private double delta = 0.0; + private double simulatedDistanceNoise = 0.0; private LoggedTunableNumber velMax = new LoggedTunableNumber("az_vel"); private LoggedTunableNumber velAcc = new LoggedTunableNumber("az_acc"); @@ -57,6 +62,7 @@ public class SwervePod implements Subsystem{ private SwervePodIO io; private SwervePodIOInputsAutoLogged inputs = new SwervePodIOInputsAutoLogged(); + private Random simNoise = new Random(); public SwervePod(int id, SwervePodIO io) { this.id = id; @@ -107,9 +113,7 @@ public SwerveModuleState setModule(SwerveModuleState desiredState) { this.lastEncoderPos = desiredOptimized.angle.getDegrees(); } // reduce output if the error is high - double currentDistance = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); - this.delta = currentDistance - this.lastDistance; - this.lastDistance = currentDistance; + desiredOptimized.speedMetersPerSecond *= Math.abs(Math.cos(desiredOptimized.angle.minus(Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)).getRadians())); //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "", id); @@ -127,8 +131,15 @@ public SwerveModuleState setModule(SwerveModuleState desiredState) { * odometry calls */ public SwerveModulePosition getPosition() { - double mps = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); - return new SwerveModulePosition(mps,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); + return getPosition(true); + } + public SwerveModulePosition getPosition(boolean addNoiseIfSim) { + double m = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); + if(addNoiseIfSim && Constants.getMode() == Mode.SIM) { + m = this.simulatedDistanceNoise; + return new SwerveModulePosition(m,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees + simNoise.nextGaussian(0.0, 5.0))); + } + return new SwerveModulePosition(m,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); } public SwerveModulePosition getDelta() { return new SwerveModulePosition(this.delta,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); @@ -169,8 +180,12 @@ public double getThrustEncoderVelocity() { @Override public void periodic() { io.updateInputs(inputs); + double currentDistance = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); + this.delta = currentDistance - this.lastDistance; + this.lastDistance = currentDistance; + this.simulatedDistanceNoise += this.delta + (simNoise.nextGaussian(0.0, 4.0) * this.delta * 0.2); Logger.getInstance().processInputs("Drive/Module" + Integer.toString(this.id), inputs); - + Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/simdistance", this.simulatedDistanceNoise); if(kPAzimuth.hasChanged(hashCode()) || kIAzimuth.hasChanged(hashCode())) { turningPIDController.setP(kPAzimuth.get()); turningPIDController.setI(kIAzimuth.get()); diff --git a/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java new file mode 100644 index 0000000..d9992d4 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java @@ -0,0 +1,87 @@ +package team3176.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import team3176.robot.Constants; +import team3176.robot.Constants.Mode; +import team3176.robot.subsystems.drivetrain.Drivetrain; + +public class PhotonVisionSystem extends SubsystemBase{ + static double camPitch = Units.degreesToRadians(10); // radians + static double camHeightOffGround = 0.8; // meters + public static final Transform3d camera2Robot = new Transform3d( + new Translation3d(0.0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)); + private PhotonCamera realCam; + AprilTagFieldLayout field; + PhotonPoseEstimator estimator; + private static PhotonVisionSystem instance; + private SimPhotonVision simInstance; + EstimatedRobotPose currentEstimate; + private PhotonVisionSystem() { + realCam = new PhotonCamera("camera1"); + + if(Constants.getMode() == Mode.SIM) { + simInstance = new SimPhotonVision(realCam,camera2Robot); + } + try { + field = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + } + catch(Exception e) { + System.out.println("woops can't load the field"); + } + estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, camera2Robot); + } + public static PhotonVisionSystem getInstance() { + if (instance == null) { + instance = new PhotonVisionSystem(); + } + return instance; + } + @Override + public void periodic() { + Pose2d currentPose = Drivetrain.getInstance().getPose(); + Pose3d current3d = new Pose3d(currentPose); + var results = realCam.getLatestResult(); + if (results.hasTargets()) { + + Optional poseEst = estimator.update(); + + ArrayList targets = new ArrayList(); + ArrayList estimates = new ArrayList(); + for(PhotonTrackedTarget t :realCam.getLatestResult().getTargets()) { + targets.add(current3d.transformBy(camera2Robot).transformBy(t.getBestCameraToTarget())); + estimates.add(PhotonUtils.estimateFieldToRobotAprilTag(t.getBestCameraToTarget(), field.getTagPose(t.getFiducialId()).get() , camera2Robot.inverse())); + } + + if(poseEst.isPresent()){ + Drivetrain.getInstance().addVisionPose(poseEst.get()); + Logger.getInstance().recordOutput("photonvision/multitag", poseEst.get().estimatedPose); + } + + Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); + Logger.getInstance().recordOutput("photonvision/poseEstimates", estimates.toArray(new Pose3d[estimates.size()])); + } + + else { + Logger.getInstance().recordOutput("photonvision/targetposes", new Pose3d[] {}); + Logger.getInstance().recordOutput("photonvision/poseEstimates", new Pose3d[] {}); + } + } +} diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 9be07d3..42d42d9 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -1,100 +1,48 @@ package team3176.robot.subsystems.vision; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Optional; - -import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonUtils; -import org.photonvision.estimation.OpenCVHelp; -import org.photonvision.estimation.PNPResults; -import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VideoSimUtil; import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.simulation.VisionTargetSim; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.apriltag.AprilTagPoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import team3176.robot.subsystems.drivetrain.Drivetrain; public class SimPhotonVision extends SubsystemBase{ // Simulated Vision System. - double camPitch = Units.degreesToRadians(10); // radians - double camHeightOffGround = 0.8; // meters - Transform3d cameratrans = new Transform3d( - new Translation3d(0.0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)); + Transform3d camera2Robot; VisionSystemSim simVision = new VisionSystemSim("photonvision"); - PhotonCamera realCam; PhotonCameraSim simCam; AprilTagFieldLayout field; - HashMap targetLookup = new HashMap<>(); PhotonPoseEstimator estimator; - public SimPhotonVision() { - realCam = new PhotonCamera("camera1"); - simCam = new PhotonCameraSim(realCam, SimCameraProperties.LL2_960_720(),0.05,20); - simVision.addCamera(simCam, cameratrans); + public SimPhotonVision(PhotonCamera c, Transform3d t) { + camera2Robot = t; + simCam = new PhotonCameraSim(c, SimCameraProperties.LL2_960_720(),0.05,20); + simVision.addCamera(simCam, camera2Robot); //simVision.addVisionTargets(new VisionTargetSim(t2pose,TargetModel.kTag16h5,2)); try { field = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); simVision.addVisionTargets(field); - for (VisionTargetSim tgt: simVision.getVisionTargets()) { - targetLookup.put(tgt.fiducialID, tgt); - } } catch(Exception e) { System.out.println("woops can't load the field"); } - estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, cameratrans); + estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, c, camera2Robot); } @Override public void periodic() { - Pose2d currentPose = Drivetrain.getInstance().getPose(); - Pose3d current3d = new Pose3d(currentPose); + Pose2d currentPose = Drivetrain.getInstance().getPoseOdomTrue(); simVision.update(currentPose); - var results = realCam.getLatestResult(); - if (results.hasTargets()) { - - Optional poseEst = estimator.update(); - - ArrayList targets = new ArrayList(); - ArrayList estimates = new ArrayList(); - for(PhotonTrackedTarget t :realCam.getLatestResult().getTargets()) { - targets.add(current3d.transformBy(cameratrans).transformBy(t.getBestCameraToTarget())); - estimates.add(PhotonUtils.estimateFieldToRobotAprilTag(t.getBestCameraToTarget(), field.getTagPose(t.getFiducialId()).get() , cameratrans.inverse())); - } - - if(poseEst.isPresent()){ - Logger.getInstance().recordOutput("photonvision/multitag", poseEst.get().estimatedPose); - } - - Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); - Logger.getInstance().recordOutput("photonvision/poseEstimates", estimates.toArray(new Pose3d[estimates.size()])); - } - - else { - Logger.getInstance().recordOutput("photonvision/targetposes", new Pose3d[] {}); - Logger.getInstance().recordOutput("photonvision/poseEstimates", new Pose3d[] {}); - } } } From 0956693c5c96549ae60ccb363032e9a40dfd0154 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 8 Aug 2023 14:17:22 -0600 Subject: [PATCH 16/24] moving into simIO noise --- simgui-window.json | 4 +-- .../subsystems/drivetrain/Drivetrain.java | 36 +++++++++++++------ .../subsystems/drivetrain/SwervePod.java | 26 ++++++++------ .../subsystems/drivetrain/SwervePodIO.java | 2 ++ .../subsystems/drivetrain/SwervePodIOSim.java | 15 +++++--- 5 files changed, 55 insertions(+), 28 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index e7cd0e3..581cada 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "351", - "ypos": "906" + "xpos": "150", + "ypos": "927" } }, "Window": { diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index dce7bb4..bcf3153 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -422,16 +422,21 @@ public double getPodAzimuth(int podID) { return pods.get(podID).getAzimuth(); } - public SwerveModulePosition[] getSwerveModulePositions(boolean addNoiseIfSim) { + public SwerveModulePosition[] getSwerveModulePositions() { return new SwerveModulePosition[] { - podFR.getPosition(addNoiseIfSim), - podFL.getPosition(addNoiseIfSim), - podBL.getPosition(addNoiseIfSim), - podBR.getPosition(addNoiseIfSim) + podFR.getPosition(), + podFL.getPosition(), + podBL.getPosition(), + podBR.getPosition() }; } - public SwerveModulePosition[] getSwerveModulePositions() { - return getSwerveModulePositions(true); + public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { + return new SwerveModulePosition[] { + podFR.getPositionSimNoNoise(), + podFL.getPositionSimNoNoise(), + podBL.getPositionSimNoNoise(), + podBR.getPositionSimNoNoise() + }; } public void setCoastMode() { @@ -492,14 +497,25 @@ public void periodic() { Logger.getInstance().processInputs("Drive/gyro", inputs); lastPose = poseEstimator.getEstimatedPosition(); SwerveModulePosition[] deltas = new SwerveModulePosition[4]; + for(int i=0;i< pods.size(); i++) { - deltas[i] = pods.get(i).getDelta(); + if(Constants.getMode() == Mode.SIM) { + deltas[i] = pods.get(i).getDeltaSimNoNoise(); + } else { + deltas[i] = pods.get(i).getDelta(); + } + } Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); wheelOnlyHeading = getPoseOdomTrue().exp(twist).getRotation(); // update encoders - this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions(true)); - this.odom.update(getSensorYaw(), getSwerveModulePositions(false)); + this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions()); + if(Constants.getMode() == Mode.SIM) { + this.odom.update(getSensorYaw(), getSwerveModulePositionsSimNoNoise()); + } else { + this.odom.update(getSensorYaw(), getSwerveModulePositions()); + } + Logger.getInstance().recordOutput("Drive/Odom", getPose()); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 4219e8a..948904d 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -51,8 +51,9 @@ public class SwervePod implements Subsystem{ private LoggedTunableNumber kIAzimuth = new LoggedTunableNumber("kI_azimuth"); private double kDAzimuth; private double lastDistance =0.0; + private double lastDistanceSimNoNoise =0.0; private double delta = 0.0; - private double simulatedDistanceNoise = 0.0; + private double deltaSimNoNoise = 0.0; private LoggedTunableNumber velMax = new LoggedTunableNumber("az_vel"); private LoggedTunableNumber velAcc = new LoggedTunableNumber("az_acc"); @@ -62,7 +63,6 @@ public class SwervePod implements Subsystem{ private SwervePodIO io; private SwervePodIOInputsAutoLogged inputs = new SwervePodIOInputsAutoLogged(); - private Random simNoise = new Random(); public SwervePod(int id, SwervePodIO io) { this.id = id; @@ -119,6 +119,7 @@ public SwerveModuleState setModule(SwerveModuleState desiredState) { //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "", id); io.setTurn(MathUtil.clamp(turnOutput, -0.4, 0.4)); Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/error",turningPIDController.getPositionError()); + Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/deltanonoise",this.deltaSimNoNoise); //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/setpoint",turningPIDController.getSetpoint().position); this.velTicsPer100ms = Units3176.mps2ums(desiredOptimized.speedMetersPerSecond); io.setDrive(desiredOptimized.speedMetersPerSecond); @@ -131,19 +132,19 @@ public SwerveModuleState setModule(SwerveModuleState desiredState) { * odometry calls */ public SwerveModulePosition getPosition() { - return getPosition(true); - } - public SwerveModulePosition getPosition(boolean addNoiseIfSim) { double m = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); - if(addNoiseIfSim && Constants.getMode() == Mode.SIM) { - m = this.simulatedDistanceNoise; - return new SwerveModulePosition(m,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees + simNoise.nextGaussian(0.0, 5.0))); - } return new SwerveModulePosition(m,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); } + public SwerveModulePosition getPositionSimNoNoise() { + double m = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionSimNoNoise / (2*Math.PI)); + return new SwerveModulePosition(m,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegreesSimNoNoise)); + } public SwerveModulePosition getDelta() { return new SwerveModulePosition(this.delta,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegrees)); } + public SwerveModulePosition getDeltaSimNoNoise() { + return new SwerveModulePosition(this.deltaSimNoNoise,Rotation2d.fromDegrees(inputs.turnAbsolutePositionDegreesSimNoNoise)); + } public double getVelocity() { double wheelVelocityInFeetPerSecond = inputs.driveVelocityRadPerSec / (Math.PI *2) * DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI; @@ -183,9 +184,12 @@ public void periodic() { double currentDistance = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); this.delta = currentDistance - this.lastDistance; this.lastDistance = currentDistance; - this.simulatedDistanceNoise += this.delta + (simNoise.nextGaussian(0.0, 4.0) * this.delta * 0.2); + if(Constants.getMode() == Mode.SIM) { + double currentDistanceSimNoNoise = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionSimNoNoise / (2*Math.PI)); + this.deltaSimNoNoise = currentDistanceSimNoNoise - this.lastDistanceSimNoNoise; + this.lastDistanceSimNoNoise = currentDistanceSimNoNoise; + } Logger.getInstance().processInputs("Drive/Module" + Integer.toString(this.id), inputs); - Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "/simdistance", this.simulatedDistanceNoise); if(kPAzimuth.hasChanged(hashCode()) || kIAzimuth.hasChanged(hashCode())) { turningPIDController.setP(kPAzimuth.get()); turningPIDController.setI(kIAzimuth.get()); diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java index b85ad4e..6149fd9 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIO.java @@ -8,11 +8,13 @@ public static class SwervePodIOInputs { public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; + public double drivePositionSimNoNoise = 0.0; public double[] driveCurrentAmpsStator = new double[] {}; public double[] driveCurrentAmpsSupply = new double[] {}; public double[] driveTempCelcius = new double[] {}; public double turnAbsolutePositionDegrees = 0.0; + public double turnAbsolutePositionDegreesSimNoNoise = 0.0; public double turnVelocityRPM = 0.0; public double turnAppliedVolts = 0.0; public double[] turnCurrentAmps = new double[] {}; diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java index cc5dcab..a709671 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java @@ -1,5 +1,7 @@ package team3176.robot.subsystems.drivetrain; +import java.util.Random; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; @@ -16,6 +18,9 @@ public class SwervePodIOSim implements SwervePodIO{ private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; private double currentDriveSpeed = 0.0; + private Random simNoise = new Random(); + private static final double moduleErrorBound = 8.0; + private double moduleOffsetError = Math.floor(simNoise.nextDouble() * moduleErrorBound) - moduleErrorBound/2.0; @Override public void updateInputs(SwervePodIOInputs inputs) { driveSim.update(Constants.LOOP_PERIODIC_SECS); @@ -28,17 +33,17 @@ public void updateInputs(SwervePodIOInputs inputs) { while (turnAbsolutePositionRad > 180) { turnAbsolutePositionRad -= 360; } - - inputs.drivePositionRad = - inputs.drivePositionRad - + (driveSim.getAngularVelocityRadPerSec() * Constants.LOOP_PERIODIC_SECS); + double delta = (driveSim.getAngularVelocityRadPerSec() * Constants.LOOP_PERIODIC_SECS); + inputs.drivePositionSimNoNoise += delta; + inputs.drivePositionRad += delta + simNoise.nextGaussian(0.0, 4.0) * Math.pow(delta,2) * 0.1; inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveCurrentAmpsStator = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; inputs.driveTempCelcius = new double[] {}; currentDriveSpeed = driveSim.getAngularVelocityRadPerSec(); - inputs.turnAbsolutePositionDegrees = turnAbsolutePositionRad; + inputs.turnAbsolutePositionDegreesSimNoNoise = turnAbsolutePositionRad; + inputs.turnAbsolutePositionDegrees = turnAbsolutePositionRad + moduleOffsetError; inputs.turnVelocityRPM = turnSim.getAngularVelocityRPM(); inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; From ad92e82e93144be441cbb7dc1b5506dd37afb17a Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 8 Aug 2023 15:39:51 -0600 Subject: [PATCH 17/24] Moving sim into a seperate class to isolate --- simgui-window.json | 6 +- simgui.json | 6 ++ .../subsystems/drivetrain/Drivetrain.java | 68 ++++++++++++------- .../subsystems/drivetrain/SwervePod.java | 1 + .../subsystems/vision/SimPhotonVision.java | 2 +- 5 files changed, 55 insertions(+), 28 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 581cada..e032060 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1920", - "xpos": "150", - "ypos": "927" + "xpos": "879", + "ypos": "608" } }, "Window": { @@ -27,7 +27,7 @@ }, "###NetworkTables": { "Collapsed": "0", - "Pos": "43,454", + "Pos": "327,29", "Size": "475,435" }, "###Other Devices": { diff --git a/simgui.json b/simgui.json index 857bdcb..7d01860 100644 --- a/simgui.json +++ b/simgui.json @@ -39,6 +39,12 @@ }, "NetworkTables": { "transitory": { + "SmartDashboard": { + "AutoSelector": { + "open": true + }, + "open": true + }, "northstar": { "calibration": { "open": true diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index bcf3153..c3b3b3e 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -107,6 +107,7 @@ public enum driveMode { private GyroIOInputs inputs; Field2d field; Pose3d visionPose3d; + SimNoNoiseOdom simNoNoiseOdom; // private final DrivetrainIOInputs inputs = new DrivetrainIOInputs(); @@ -140,6 +141,7 @@ private Drivetrain(GyroIO io) { podFL = new SwervePod(1, new SwervePodIOSim()); podBL = new SwervePod(2, new SwervePodIOSim()); podBR = new SwervePod(3, new SwervePodIOSim()); + simNoNoiseOdom = new SimNoNoiseOdom(); break; default: break; @@ -283,9 +285,12 @@ public driveMode getCurrentDriveMode() { public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } - public Pose2d getPoseOdomTrue() { + public Pose2d getPoseOdom() { return odom.getPoseMeters(); } + public Pose2d getSimNoNoisePose() { + return simNoNoiseOdom.getPoseTrue(); + } public void addVisionPose(EstimatedRobotPose p) { Matrix cov = new Matrix<>(Nat.N3(), Nat.N1()); double distance = 0.0; @@ -430,14 +435,7 @@ public SwerveModulePosition[] getSwerveModulePositions() { podBR.getPosition() }; } - public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { - return new SwerveModulePosition[] { - podFR.getPositionSimNoNoise(), - podFL.getPositionSimNoNoise(), - podBL.getPositionSimNoNoise(), - podBR.getPositionSimNoNoise() - }; - } + public void setCoastMode() { for (int idx = 0; idx < (pods.size()); idx++) { @@ -499,29 +497,20 @@ public void periodic() { SwerveModulePosition[] deltas = new SwerveModulePosition[4]; for(int i=0;i< pods.size(); i++) { - if(Constants.getMode() == Mode.SIM) { - deltas[i] = pods.get(i).getDeltaSimNoNoise(); - } else { deltas[i] = pods.get(i).getDelta(); - } - } Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); - wheelOnlyHeading = getPoseOdomTrue().exp(twist).getRotation(); + wheelOnlyHeading = getPoseOdom().exp(twist).getRotation(); // update encoders this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions()); + this.odom.update(getSensorYaw(), getSwerveModulePositions()); if(Constants.getMode() == Mode.SIM) { - this.odom.update(getSensorYaw(), getSwerveModulePositionsSimNoNoise()); - } else { - this.odom.update(getSensorYaw(), getSwerveModulePositions()); + simNoNoiseOdom.update(); } - - Logger.getInstance().recordOutput("Drive/Odom", getPose()); - if(Constants.getMode() == Mode.SIM) { - Logger.getInstance().recordOutput("Drive/OdomTrue", getPoseOdomTrue()); - } + Logger.getInstance().recordOutput("Drive/Pose", getPose()); + Logger.getInstance().recordOutput("Drive/Odom", getPoseOdom()); SmartDashboard.putNumber("NavYaw",getPoseYawWrapped().getDegrees()); @@ -589,6 +578,37 @@ public void publishSwervePodPIDErrors(){ SmartDashboard.putNumber("BLThrustError", BLThrustError); } - + private class SimNoNoiseOdom{ + private SwerveDriveOdometry odomNoNoise; + + public SimNoNoiseOdom() { + odomNoNoise =new SwerveDriveOdometry(DrivetrainConstants.DRIVE_KINEMATICS, getSensorYaw(), getSwerveModulePositionsSimNoNoise(), new Pose2d(0.0, 0.0, new Rotation2d())); + } + + public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { + return new SwerveModulePosition[] { + podFR.getPositionSimNoNoise(), + podFL.getPositionSimNoNoise(), + podBL.getPositionSimNoNoise(), + podBR.getPositionSimNoNoise() + }; + + } + public Pose2d getPoseTrue() { + return odomNoNoise.getPoseMeters(); + } + public void update() { + SwerveModulePosition[] deltas = new SwerveModulePosition[4]; + for(int i=0;i< pods.size(); i++) { + deltas[i] = pods.get(i).getDeltaSimNoNoise(); + } + Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); + wheelOnlyHeading = getPose().exp(twist).getRotation(); + odomNoNoise.update(wheelOnlyHeading, getSwerveModulePositionsSimNoNoise()); + Logger.getInstance().recordOutput("Drive/PoseSimNoNoise", getPoseTrue()); + } + + + } } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 948904d..a2c1e64 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java @@ -222,3 +222,4 @@ public void setupShuffleboard() { .getEntry(); } } + diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 42d42d9..e6b9081 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -41,7 +41,7 @@ public SimPhotonVision(PhotonCamera c, Transform3d t) { @Override public void periodic() { - Pose2d currentPose = Drivetrain.getInstance().getPoseOdomTrue(); + Pose2d currentPose = Drivetrain.getInstance().getSimNoNoisePose(); simVision.update(currentPose); } From e17df85d76916ca662dc268429f733f8dd9941f4 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 8 Aug 2023 17:13:21 -0600 Subject: [PATCH 18/24] testing autos --- simgui-window.json | 17 ++++++++++++----- simgui.json | 5 ++++- .../java/team3176/robot/RobotContainer.java | 1 + .../robot/subsystems/drivetrain/Drivetrain.java | 6 ++++++ 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index e032060..f657288 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -5,13 +5,20 @@ "MainWindow": { "GLOBAL": { "fps": "120", - "height": "1016", + "height": "736", "maximized": "0", "style": "0", "userScale": "2", - "width": "1920", - "xpos": "879", - "ypos": "608" + "width": "1440", + "xpos": "309", + "ypos": "310" + } + }, + "Table": { + "0x542B5671,2": { + "Column 0 Width": "169", + "Column 1 Width": "156", + "RefScale": "13" } }, "Window": { @@ -32,7 +39,7 @@ }, "###Other Devices": { "Collapsed": "0", - "Pos": "43,20", + "Pos": "145,13", "Size": "250,695" }, "###System Joysticks": { diff --git a/simgui.json b/simgui.json index 7d01860..e53cf16 100644 --- a/simgui.json +++ b/simgui.json @@ -38,9 +38,12 @@ } }, "NetworkTables": { + "Persistent Values": { + "open": false + }, "transitory": { "SmartDashboard": { - "AutoSelector": { + "Auton Choice": { "open": true }, "open": true diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 489500b..45092b7 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -253,6 +253,7 @@ public Command getAutonomousCommand() { PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); } + choosenAutonomousCommand = new PathPlannerAuto("wall_3nSteal_3").getauto(); return choosenAutonomousCommand; } } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index c3b3b3e..b9dd0f9 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -321,6 +321,9 @@ public void resetPose(Pose2d pose) { podFL.getPosition(), podBL.getPosition(), podBR.getPosition() }, pose); + if(Constants.getMode() == Mode.SIM) { + simNoNoiseOdom.resetSimPose(pose); + } } public void resetPoseToVision() { @@ -594,6 +597,9 @@ public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { }; } + public void resetSimPose(Pose2d p) { + odomNoNoise.resetPosition(p.getRotation(), getSwerveModulePositionsSimNoNoise(), p); + } public Pose2d getPoseTrue() { return odomNoNoise.getPoseMeters(); } From c946c5b0599524ffd2512e39624e2957bd965836 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 8 Aug 2023 19:34:59 -0600 Subject: [PATCH 19/24] fixed auto drive bug --- .../team3176/robot/subsystems/drivetrain/Drivetrain.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 26ea54f..109df4a 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -82,7 +82,7 @@ public enum coordType { double angleAvgRollingWindow; public enum driveMode { - DEFENSE, DRIVE, CUBECHASETELEOP, CUBECHASEAUTON + DEFENSE, DRIVE, CUBECHASETELEOP, CUBECHASEAUTON, AUTO } private SwervePod podFR; @@ -297,6 +297,7 @@ public void resetPoseToVision() { } public void setModuleStates(SwerveModuleState[] states) { + currentDriveMode = driveMode.AUTO; for (int idx = 0; idx < (pods.size()); idx++) { pods.get(idx).setModule(states[idx]); } @@ -500,6 +501,8 @@ public void periodic() { case DRIVE: calculateNSetPodPositions(); break; + case AUTO: + break; default: calculateNSetPodPositions(); break; From 63664ccd8e44b5aec89dc47533797272fc9b51ba Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 9 Aug 2023 12:40:53 -0600 Subject: [PATCH 20/24] making vision work on red and blue --- simgui-window.json | 4 +-- simgui.json | 7 +++++ src/main/java/team3176/robot/Robot.java | 1 + .../java/team3176/robot/RobotContainer.java | 22 ++++++++++++-- .../subsystems/vision/PhotonVisionSystem.java | 29 +++++++++++++++---- .../subsystems/vision/SimPhotonVision.java | 21 ++++++-------- 6 files changed, 62 insertions(+), 22 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index f657288..c3238d5 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1440", - "xpos": "309", - "ypos": "310" + "xpos": "80", + "ypos": "260" } }, "Table": { diff --git a/simgui.json b/simgui.json index e53cf16..bc19610 100644 --- a/simgui.json +++ b/simgui.json @@ -12,6 +12,7 @@ "types": { "/AdvantageKit/RealOutputs/Arm/mech2d": "Mechanism2d", "/FMSInfo": "FMSInfo", + "/LiveWindow/PhotonVisionSystem": "Subsystem", "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", "/LiveWindow/Ungrouped/DigitalInput[1]": "Digital Input", "/LiveWindow/Ungrouped/DigitalInput[2]": "Digital Input", @@ -19,6 +20,12 @@ "/LiveWindow/Ungrouped/DigitalInput[4]": "Digital Input", "/LiveWindow/Ungrouped/PIDController[10]": "PIDController", "/LiveWindow/Ungrouped/PIDController[11]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[12]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[13]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[14]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[15]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[16]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[17]": "PIDController", "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 7d64d2b..a77674a 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/src/main/java/team3176/robot/Robot.java @@ -164,6 +164,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { robotContainer.checkAutonomousSelection(); + robotContainer.checkAllaince(); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 45092b7..c15c43f 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -10,12 +10,14 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardInput; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.InstantCommand; import team3176.robot.commands.drivetrain.*; @@ -58,10 +60,11 @@ public class RobotContainer { private final Drivetrain drivetrain; private final Superstructure superstructure; private final RobotState robotState; - private final PhotonVisionSystem vision; + private PhotonVisionSystem vision; private LoggedDashboardChooser autonChooser = new LoggedDashboardChooser<>("AutoSelector"); private String choosenAutonomousString = ""; private Command choosenAutonomousCommand; + private Alliance currentAlliance = Alliance.Blue; /** @@ -222,8 +225,8 @@ public void clearCanFaults(){ public void printCanFaults(){ pdh.getStickyFaults(); } - public void checkAutonomousSelection() { - if(autonChooser.get() != null && !choosenAutonomousString.equals(autonChooser.get())) { + public void checkAutonomousSelection(Boolean force) { + if(autonChooser.get() != null && (!choosenAutonomousString.equals(autonChooser.get()) || force)) { Long start = System.nanoTime(); choosenAutonomousString = autonChooser.get(); try { @@ -238,6 +241,19 @@ public void checkAutonomousSelection() { System.out.println("Autonomous Selected: [" + choosenAutonomousString + "] generated in " + (totalTime / 1000000.0) + "ms"); } } + public void checkAutonomousSelection() { + checkAutonomousSelection(false); + } + + public void checkAllaince() { + if(DriverStation.getAlliance() != currentAlliance) { + currentAlliance = DriverStation.getAlliance(); + //Updated any things that need to change + System.out.println("changed alliance"); + checkAutonomousSelection(true); + vision.refresh(); + } + } /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java index d9992d4..03f915f 100644 --- a/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java +++ b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java @@ -1,5 +1,6 @@ package team3176.robot.subsystems.vision; +import java.io.File; import java.util.ArrayList; import java.util.Optional; @@ -12,12 +13,15 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import team3176.robot.Constants; import team3176.robot.Constants.Mode; @@ -36,16 +40,21 @@ public class PhotonVisionSystem extends SubsystemBase{ EstimatedRobotPose currentEstimate; private PhotonVisionSystem() { realCam = new PhotonCamera("camera1"); - - if(Constants.getMode() == Mode.SIM) { - simInstance = new SimPhotonVision(realCam,camera2Robot); - } try { field = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + if (DriverStation.getAlliance() == Alliance.Blue) { + field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + } else { + field.setOrigin(OriginPosition.kRedAllianceWallRightSide); + } } catch(Exception e) { System.out.println("woops can't load the field"); } + + if(Constants.getMode() == Mode.SIM) { + simInstance = new SimPhotonVision(realCam,camera2Robot,field); + } estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, camera2Robot); } public static PhotonVisionSystem getInstance() { @@ -54,6 +63,17 @@ public static PhotonVisionSystem getInstance() { } return instance; } + public void refresh() { + if (DriverStation.getAlliance() == Alliance.Blue) { + field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + } else { + field.setOrigin(OriginPosition.kRedAllianceWallRightSide); + } + if(Constants.getMode() == Mode.SIM) { + simInstance.switchAllaince(field); + } + estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, camera2Robot); + } @Override public void periodic() { Pose2d currentPose = Drivetrain.getInstance().getPose(); @@ -74,7 +94,6 @@ public void periodic() { Drivetrain.getInstance().addVisionPose(poseEst.get()); Logger.getInstance().recordOutput("photonvision/multitag", poseEst.get().estimatedPose); } - Logger.getInstance().recordOutput("photonvision/targetposes", targets.toArray(new Pose3d[targets.size()])); Logger.getInstance().recordOutput("photonvision/poseEstimates", estimates.toArray(new Pose3d[estimates.size()])); } diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index e6b9081..8cd6f1e 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import team3176.robot.subsystems.drivetrain.Drivetrain; @@ -20,24 +21,20 @@ public class SimPhotonVision extends SubsystemBase{ Transform3d camera2Robot; VisionSystemSim simVision = new VisionSystemSim("photonvision"); PhotonCameraSim simCam; - AprilTagFieldLayout field; PhotonPoseEstimator estimator; - public SimPhotonVision(PhotonCamera c, Transform3d t) { + public SimPhotonVision(PhotonCamera c, Transform3d t, AprilTagFieldLayout field) { camera2Robot = t; - simCam = new PhotonCameraSim(c, SimCameraProperties.LL2_960_720(),0.05,20); + simCam = new PhotonCameraSim(c, SimCameraProperties.LL2_960_720(),0.07,Units.feetToMeters(30)); simVision.addCamera(simCam, camera2Robot); //simVision.addVisionTargets(new VisionTargetSim(t2pose,TargetModel.kTag16h5,2)); - try { - field = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); - simVision.addVisionTargets(field); - } - catch(Exception e) { - System.out.println("woops can't load the field"); - } - estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, c, camera2Robot); + simVision.addVisionTargets(field); + + } + public void switchAllaince(AprilTagFieldLayout field) { + simVision.removeVisionTargets("apriltags"); + simVision.addVisionTargets(field); } - @Override public void periodic() { From 69f301e52fbac864d0fd8f451dc1edc4bdf30ef9 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 10 Aug 2023 22:30:28 -0600 Subject: [PATCH 21/24] moving true sim odom to seperate class --- simgui-window.json | 4 +- .../subsystems/drivetrain/Drivetrain.java | 40 ++------------- .../subsystems/drivetrain/SimNoNoiseOdom.java | 50 +++++++++++++++++++ 3 files changed, 55 insertions(+), 39 deletions(-) create mode 100644 src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java diff --git a/simgui-window.json b/simgui-window.json index c3238d5..e7ce8e2 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1440", - "xpos": "80", - "ypos": "260" + "xpos": "362", + "ypos": "271" } }, "Table": { diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 729d558..db07f7f 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -141,7 +141,7 @@ private Drivetrain(GyroIO io) { podFL = new SwervePod(1, new SwervePodIOSim()); podBL = new SwervePod(2, new SwervePodIOSim()); podBR = new SwervePod(3, new SwervePodIOSim()); - simNoNoiseOdom = new SimNoNoiseOdom(); + simNoNoiseOdom = new SimNoNoiseOdom(new ArrayList<>( List.of(podFR, podFL, podBL, podBR))); break; default: break; @@ -379,7 +379,7 @@ public Rotation2d getPoseYawWrapped() { * * @return Rotation2d of the yaw */ - private Rotation2d getSensorYaw() { + protected Rotation2d getSensorYaw() { if(Constants.getMode() == Mode.SIM) { if(this.odom == null || this.poseEstimator == null) { return new Rotation2d(); @@ -584,40 +584,6 @@ public void publishSwervePodPIDErrors(){ SmartDashboard.putNumber("BLThrustError", BLThrustError); } - private class SimNoNoiseOdom{ - private SwerveDriveOdometry odomNoNoise; - - public SimNoNoiseOdom() { - odomNoNoise =new SwerveDriveOdometry(DrivetrainConstants.DRIVE_KINEMATICS, getSensorYaw(), getSwerveModulePositionsSimNoNoise(), new Pose2d(0.0, 0.0, new Rotation2d())); - } - - public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { - return new SwerveModulePosition[] { - podFR.getPositionSimNoNoise(), - podFL.getPositionSimNoNoise(), - podBL.getPositionSimNoNoise(), - podBR.getPositionSimNoNoise() - }; - - } - public void resetSimPose(Pose2d p) { - odomNoNoise.resetPosition(p.getRotation(), getSwerveModulePositionsSimNoNoise(), p); - } - public Pose2d getPoseTrue() { - return odomNoNoise.getPoseMeters(); - } - public void update() { - SwerveModulePosition[] deltas = new SwerveModulePosition[4]; - for(int i=0;i< pods.size(); i++) { - deltas[i] = pods.get(i).getDeltaSimNoNoise(); - } - Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); - wheelOnlyHeading = getPose().exp(twist).getRotation(); - odomNoNoise.update(wheelOnlyHeading, getSwerveModulePositionsSimNoNoise()); - Logger.getInstance().recordOutput("Drive/PoseSimNoNoise", getPoseTrue()); - } - - - } + } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java b/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java new file mode 100644 index 0000000..6801165 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java @@ -0,0 +1,50 @@ +package team3176.robot.subsystems.drivetrain; + +import java.util.ArrayList; +import java.util.List; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import team3176.robot.constants.DrivetrainConstants; + +public class SimNoNoiseOdom{ + private SwerveDriveOdometry odomNoNoise; + private ArrayList pods; + private Rotation2d wheelOnlyHeading; + public SimNoNoiseOdom(ArrayList pods) { + this.pods = pods; + wheelOnlyHeading = new Rotation2d(); + odomNoNoise =new SwerveDriveOdometry(DrivetrainConstants.DRIVE_KINEMATICS, Drivetrain.getInstance().getSensorYaw(),getSwerveModulePositionsSimNoNoise(), new Pose2d(0.0, 0.0, new Rotation2d())); + } + public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { + SwerveModulePosition[] positions = new SwerveModulePosition[pods.size()]; + for(int i = 0; i < pods.size(); ++i) { + positions[1] = pods.get(i).getPositionSimNoNoise(); + } + return positions; + + } + public void resetSimPose(Pose2d p) { + odomNoNoise.resetPosition(p.getRotation(), getSwerveModulePositionsSimNoNoise(), p); + } + public Pose2d getPoseTrue() { + return odomNoNoise.getPoseMeters(); + } + public void update() { + SwerveModulePosition[] deltas = new SwerveModulePosition[4]; + for(int i=0;i< pods.size(); i++) { + deltas[i] = pods.get(i).getDeltaSimNoNoise(); + } + Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); + wheelOnlyHeading = odomNoNoise.getPoseMeters().exp(twist).getRotation(); + odomNoNoise.update(wheelOnlyHeading, getSwerveModulePositionsSimNoNoise()); + Logger.getInstance().recordOutput("Drive/PoseSimNoNoise", getPoseTrue()); + } + + + } \ No newline at end of file From 4ff2d218710b8d3a9221b1f29177d1afc273d388 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 10 Aug 2023 23:02:53 -0600 Subject: [PATCH 22/24] fixing runtime bug --- simgui-window.json | 4 ++-- .../robot/subsystems/drivetrain/Drivetrain.java | 4 ++-- .../robot/subsystems/drivetrain/SimNoNoiseOdom.java | 10 ++++++++-- .../robot/subsystems/drivetrain/SwervePodIOSim.java | 2 +- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index e7ce8e2..6f510dd 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1440", - "xpos": "362", - "ypos": "271" + "xpos": "-2", + "ypos": "742" } }, "Table": { diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index db07f7f..3b9b012 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -141,7 +141,7 @@ private Drivetrain(GyroIO io) { podFL = new SwervePod(1, new SwervePodIOSim()); podBL = new SwervePod(2, new SwervePodIOSim()); podBR = new SwervePod(3, new SwervePodIOSim()); - simNoNoiseOdom = new SimNoNoiseOdom(new ArrayList<>( List.of(podFR, podFL, podBL, podBR))); + simNoNoiseOdom = new SimNoNoiseOdom(new ArrayList<>(List.of(podFR, podFL, podBL, podBR))); break; default: break; @@ -190,7 +190,7 @@ private Drivetrain(GyroIO io) { // Prevents more than one instance of drivetrian public static Drivetrain getInstance() { if (instance == null) { - if(Constants.getMode() != Mode.REPLAY) { + if(Constants.getMode() != Mode.SIM) { instance = new Drivetrain(new GyroIONavX()); } else{ diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java b/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java index 6801165..1484f4d 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java @@ -18,13 +18,19 @@ public class SimNoNoiseOdom{ private Rotation2d wheelOnlyHeading; public SimNoNoiseOdom(ArrayList pods) { this.pods = pods; + System.out.println("pods size " + this.pods.size()); wheelOnlyHeading = new Rotation2d(); - odomNoNoise =new SwerveDriveOdometry(DrivetrainConstants.DRIVE_KINEMATICS, Drivetrain.getInstance().getSensorYaw(),getSwerveModulePositionsSimNoNoise(), new Pose2d(0.0, 0.0, new Rotation2d())); + odomNoNoise =new SwerveDriveOdometry(DrivetrainConstants.DRIVE_KINEMATICS, new Rotation2d(), new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, new Pose2d(0.0, 0.0, new Rotation2d())); } public SwerveModulePosition[] getSwerveModulePositionsSimNoNoise() { SwerveModulePosition[] positions = new SwerveModulePosition[pods.size()]; for(int i = 0; i < pods.size(); ++i) { - positions[1] = pods.get(i).getPositionSimNoNoise(); + positions[i] = pods.get(i).getPositionSimNoNoise(); } return positions; diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java index a709671..426928d 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePodIOSim.java @@ -35,7 +35,7 @@ public void updateInputs(SwervePodIOInputs inputs) { } double delta = (driveSim.getAngularVelocityRadPerSec() * Constants.LOOP_PERIODIC_SECS); inputs.drivePositionSimNoNoise += delta; - inputs.drivePositionRad += delta + simNoise.nextGaussian(0.0, 4.0) * Math.pow(delta,2) * 0.1; + inputs.drivePositionRad += delta + simNoise.nextGaussian(0.0, 2.0) * Math.pow(delta,2) * 0.1; inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveCurrentAmpsStator = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; From 8e416c8335db8b8544a410d6a6bee05a74359835 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 10 Aug 2023 23:11:17 -0600 Subject: [PATCH 23/24] updating simulated gyro value to true pose --- simgui-window.json | 4 ++-- .../java/team3176/robot/subsystems/drivetrain/Drivetrain.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 6f510dd..eb2f8ea 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1440", - "xpos": "-2", - "ypos": "742" + "xpos": "266", + "ypos": "928" } }, "Table": { diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 3b9b012..b9bbbf6 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -384,7 +384,7 @@ protected Rotation2d getSensorYaw() { if(this.odom == null || this.poseEstimator == null) { return new Rotation2d(); } - return wheelOnlyHeading; + return simNoNoiseOdom.getPoseTrue().getRotation(); } return inputs.rotation2d; From 90d74b07ebaea793e55d40bdac12e470e3130fc8 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 10 Aug 2023 23:35:37 -0600 Subject: [PATCH 24/24] setting up for multiple cameras --- simgui-window.json | 4 ++-- .../subsystems/vision/PhotonVisionSystem.java | 3 ++- .../robot/subsystems/vision/SimPhotonVision.java | 15 +++++++++------ 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index eb2f8ea..e6eab98 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1440", - "xpos": "266", - "ypos": "928" + "xpos": "233", + "ypos": "830" } }, "Table": { diff --git a/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java index 03f915f..29b3ebb 100644 --- a/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java +++ b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java @@ -2,6 +2,7 @@ import java.io.File; import java.util.ArrayList; +import java.util.List; import java.util.Optional; import org.littletonrobotics.junction.Logger; @@ -53,7 +54,7 @@ private PhotonVisionSystem() { } if(Constants.getMode() == Mode.SIM) { - simInstance = new SimPhotonVision(realCam,camera2Robot,field); + simInstance = new SimPhotonVision(List.of(realCam),List.of(camera2Robot),field); } estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, camera2Robot); } diff --git a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java index 8cd6f1e..c1ba6bf 100644 --- a/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -1,5 +1,7 @@ package team3176.robot.subsystems.vision; +import java.util.List; + import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.simulation.PhotonCameraSim; @@ -18,15 +20,16 @@ public class SimPhotonVision extends SubsystemBase{ // Simulated Vision System. - Transform3d camera2Robot; VisionSystemSim simVision = new VisionSystemSim("photonvision"); - PhotonCameraSim simCam; PhotonPoseEstimator estimator; - public SimPhotonVision(PhotonCamera c, Transform3d t, AprilTagFieldLayout field) { - camera2Robot = t; - simCam = new PhotonCameraSim(c, SimCameraProperties.LL2_960_720(),0.07,Units.feetToMeters(30)); - simVision.addCamera(simCam, camera2Robot); + public SimPhotonVision(List c, List t, AprilTagFieldLayout field) { + for(int i = 0; i < c.size(); i++) { + Transform3d camera2Robot = t.get(i); + PhotonCameraSim simCam = new PhotonCameraSim(c.get(i), SimCameraProperties.LL2_960_720(),0.07,Units.feetToMeters(30)); + simVision.addCamera(simCam, camera2Robot); + } + //simVision.addVisionTargets(new VisionTargetSim(t2pose,TargetModel.kTag16h5,2)); simVision.addVisionTargets(field);