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-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..e6eab98 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -5,13 +5,20 @@ "MainWindow": { "GLOBAL": { "fps": "120", - "height": "720", + "height": "736", "maximized": "0", "style": "0", "userScale": "2", - "width": "1280", - "xpos": "537", - "ypos": "110" + "width": "1440", + "xpos": "233", + "ypos": "830" + } + }, + "Table": { + "0x542B5671,2": { + "Column 0 Width": "169", + "Column 1 Width": "156", + "RefScale": "13" } }, "Window": { @@ -22,22 +29,22 @@ }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,465", + "Pos": "43,465", "Size": "796,155" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "250,277", - "Size": "750,185" + "Pos": "327,29", + "Size": "475,435" }, "###Other Devices": { "Collapsed": "0", - "Pos": "1025,20", + "Pos": "145,13", "Size": "250,695" }, "###System Joysticks": { "Collapsed": "0", - "Pos": "5,350", + "Pos": "20,319", "Size": "192,218" }, "###Timing": { @@ -47,7 +54,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 5543b54..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", @@ -29,9 +36,40 @@ "/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" + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/VisionSystemSim-photonvision/Sim Field": "Field2d", + "/SmartDashboard/photonvision Sim Field": "Field2d" + } + }, + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "transitory": { + "SmartDashboard": { + "Auton Choice": { + "open": true + }, + "open": true + }, + "northstar": { + "calibration": { + "open": true + }, + "open": true + }, + "photonvision": { + "camera1": { + "open": true + }, + "open": true, + "photonvision": { + "open": true + } + } } } } diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 9ecc806..a77674a 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); + }); } @@ -131,7 +163,8 @@ public void disabledInit() { @Override public void disabledPeriodic() { - //nan + 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 069839f..c15c43f 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,21 +6,24 @@ import java.io.File; +import org.littletonrobotics.junction.Logger; +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.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.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; 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.*; 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; @@ -30,8 +33,8 @@ import team3176.robot.subsystems.superstructure.IntakeCone; import team3176.robot.subsystems.superstructure.Superstructure; -//import team3176.robot.subsystems.vision.VisionDual; -import team3176.robot.subsystems.vision.VisionCubeChase; +import team3176.robot.subsystems.vision.PhotonVisionSystem; +import team3176.robot.subsystems.vision.SimPhotonVision; /** * This class is where the bulk of the robot should be declared. Since @@ -55,9 +58,14 @@ 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 SendableChooser autonChooser; + private final RobotState robotState; + private PhotonVisionSystem vision; + private LoggedDashboardChooser autonChooser = new LoggedDashboardChooser<>("AutoSelector"); + private String choosenAutonomousString = ""; + private Command choosenAutonomousCommand; + private Alliance currentAlliance = Alliance.Blue; + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -70,16 +78,17 @@ public RobotContainer() { drivetrain = Drivetrain.getInstance(); intakeCube = IntakeCube.getInstance(); intakeCone = IntakeCone.getInstance(); + robotState = RobotState.getInstance(); + vision = PhotonVisionSystem.getInstance(); pdh = new PowerDistribution(Hardwaremap.PDH_CID, ModuleType.kRev); - vision = VisionCubeChase.getInstance(); superstructure = Superstructure.getInstance(); - drivetrain.setDefaultCommand(new SwerveDrive( - controller::getForward, - controller::getStrafe, - controller::getSpin)); - arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); - autonChooser = new SendableChooser<>(); + drivetrain.setDefaultCommand(drivetrain.swerveDrivePercent( + () -> controller.getForward() * 0.7, + () -> controller.getStrafe() * 0.7, + () -> controller.getSpin() * 3)); + + //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()) { @@ -87,9 +96,8 @@ public RobotContainer() { autonChooser.addOption(s, s); } } - - SmartDashboard.putData("Auton Choice", autonChooser); + SmartDashboard.putData("Auton Choice", autonChooser.getSendableChooser()); configureBindings(); } @@ -99,33 +107,31 @@ 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(10).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); - //m_Controller.getTransStick_Button10() - // .onFalse(new InstantCommand(() -> m_Drivetrain.setDriveMode(driveMode.DRIVE), m_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(drivetrain.swerveDefenseCommand()).withName("swerveDefense")); + //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()); //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.swerveDrivePercent( + () -> controller.getForward() * 1.0, + () -> controller.getStrafe() * 1.0, + () -> controller.getSpin() * 7)); controller.rotStick.button(2).whileTrue(new SpinLockDrive( controller::getForward, @@ -133,7 +139,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(drivetrain.swerveDefenseCommand()).withName("setBrakeMode")); controller.rotStick.button(4).whileTrue(superstructure.intakeCubeHumanPlayer()); controller.rotStick.button(4).onFalse(superstructure.prepareCarry()); @@ -142,41 +148,41 @@ 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()); - 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.y().onFalse(claw.idleCommand()); - controller.operator.rightBumper().and(controller.operator.leftBumper().negate()).onTrue(new SetColorWantState(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()).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())); @@ -219,7 +225,35 @@ public void clearCanFaults(){ public void printCanFaults(){ pdh.getStickyFaults(); } + public void checkAutonomousSelection(Boolean force) { + if(autonChooser.get() != null && (!choosenAutonomousString.equals(autonChooser.get()) || force)) { + Long start = System.nanoTime(); + choosenAutonomousString = autonChooser.get(); + 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"); + } + } + 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. * @@ -227,10 +261,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"; - - PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); - return ppSwerveAuto.getauto(); + + 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(); + } + choosenAutonomousCommand = new PathPlannerAuto("wall_3nSteal_3").getauto(); + return choosenAutonomousCommand; } } 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/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/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/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/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/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/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/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/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/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/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/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 5bf8de0..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; } @@ -372,7 +370,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,13 +402,17 @@ public static RobotState getInstance() { return instance; } + public Command setColorWantedState(int state) { + return this.runOnce(() -> this.setColorWantState(state)).withName("setColorWantedState"); + } + @Override public void periodic() { io.updateInputs(inputs); 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) { @@ -420,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) // { @@ -444,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 656ad51..b9bbbf6 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java @@ -8,20 +8,22 @@ 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.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,11 +37,13 @@ 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.List; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; public class Drivetrain extends SubsystemBase { private static Drivetrain instance; @@ -84,7 +88,7 @@ public enum coordType { double angleAvgRollingWindow; public enum driveMode { - DEFENSE, DRIVE, VISION, CUBECHASETELEOP, CUBECHASEAUTON + DEFENSE, DRIVE, CUBECHASETELEOP, CUBECHASEAUTON, AUTO } private SwervePod podFR; @@ -103,6 +107,7 @@ public enum driveMode { private GyroIOInputs inputs; Field2d field; Pose3d visionPose3d; + SimNoNoiseOdom simNoNoiseOdom; // private final DrivetrainIOInputs inputs = new DrivetrainIOInputs(); @@ -136,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(new ArrayList<>(List.of(podFR, podFL, podBL, podBR))); break; default: break; @@ -184,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{ @@ -223,7 +229,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 @@ -233,53 +253,24 @@ 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); - 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); - - } 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)); + 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())); } + Logger.getInstance().recordOutput("SwerveStates/real", realStates); } @@ -294,6 +285,29 @@ public driveMode getCurrentDriveMode() { public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } + 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; + 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(); @@ -307,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() { @@ -314,6 +331,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]); } @@ -361,12 +379,12 @@ 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(); } - return wheelOnlyHeading; + return simNoNoiseOdom.getPoseTrue().getRotation(); } return inputs.rotation2d; @@ -421,6 +439,7 @@ public SwerveModulePosition[] getSwerveModulePositions() { podBR.getPosition() }; } + public void setCoastMode() { for (int idx = 0; idx < (pods.size()); idx++) { @@ -459,117 +478,81 @@ 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)); + } + 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 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(); + lastPose = poseEstimator.getEstimatedPosition(); SwerveModulePosition[] deltas = new SwerveModulePosition[4]; + for(int i=0;i< pods.size(); i++) { - deltas[i] = pods.get(i).getDelta(); + deltas[i] = pods.get(i).getDelta(); } Twist2d twist = DrivetrainConstants.DRIVE_KINEMATICS.toTwist2d(deltas); - wheelOnlyHeading = getPose().exp(twist).getRotation(); + wheelOnlyHeading = getPoseOdom().exp(twist).getRotation(); // update encoders this.poseEstimator.update(getSensorYaw(), getSwerveModulePositions()); this.odom.update(getSensorYaw(), getSwerveModulePositions()); - Logger.getInstance().recordOutput("Drive/Odom", getPose()); + if(Constants.getMode() == Mode.SIM) { + simNoNoiseOdom.update(); + } + + + Logger.getInstance().recordOutput("Drive/Pose", getPose()); + Logger.getInstance().recordOutput("Drive/Odom", getPoseOdom()); + 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; + case AUTO: + 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); } 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..1484f4d --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/drivetrain/SimNoNoiseOdom.java @@ -0,0 +1,56 @@ +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; + System.out.println("pods size " + this.pods.size()); + wheelOnlyHeading = 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[i] = 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 diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java b/src/main/java/team3176/robot/subsystems/drivetrain/SwervePod.java index 3c9cd98..a2c1e64 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; @@ -12,19 +14,20 @@ 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; +import team3176.robot.Constants.Mode; 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; @@ -48,7 +51,9 @@ public class SwervePod { 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 deltaSimNoNoise = 0.0; private LoggedTunableNumber velMax = new LoggedTunableNumber("az_vel"); private LoggedTunableNumber velAcc = new LoggedTunableNumber("az_acc"); @@ -78,6 +83,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,59 +91,60 @@ 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 - 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(azimuthEncoderAbsPosition)).getRadians())); - //Logger.getInstance().recordOutput("Drive/Module" + Integer.toString(this.id) + "", id); + 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) + "/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); - - 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; } /* * 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)); + double m = Units.feetToMeters((DrivetrainConstants.WHEEL_DIAMETER_INCHES/12.0 * Math.PI) * inputs.drivePositionRad / (2*Math.PI)); + 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; @@ -171,6 +178,25 @@ public double getThrustEncoderVelocity() { return inputs.driveVelocityRadPerSec; } + @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; + 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); + 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]) @@ -196,3 +222,4 @@ public void setupShuffleboard() { .getEntry(); } } + 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..426928d 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, 2.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())}; diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java index 5346969..fa62017 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java @@ -135,8 +135,8 @@ public void setAngleSetpoint(double setpointAngle) { * Commands */ public Command armSetPosition(double angleInDegrees) { - SmartDashboard.putNumber("armSetPosition",angleInDegrees); - return this.run(() -> setPIDPosition(angleInDegrees)); + Logger.getInstance().recordOutput("Arm/armSetPosition",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..33d4df2 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java @@ -1,42 +1,24 @@ 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.Hardwaremap; +import team3176.robot.Constants; +import team3176.robot.Constants.Mode; 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; 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); - } - 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 @@ -44,35 +26,35 @@ public void setClawMotor(double percent, int amps) { 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) { @@ -80,29 +62,19 @@ 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() { 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; } @@ -110,7 +82,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,28 +93,31 @@ 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(new DelayedBoolean(0.5,() -> !getIsLinebreakTwo())::get).withName("intakeCone"); } public Command intakeCube() { - return this.intakeGamePiece(GamePiece.CUBE).until(this::getLinebreakOne); + 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(); } - }); + }).withName("determineGamePiece"); + } + public Command idleCommand() { + return this.runOnce(this::idle); } @@ -156,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); } @@ -177,14 +151,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..304a379 --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/superstructure/ClawIOSpark.java @@ -0,0 +1,46 @@ +// 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 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; + /** 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); + } + + + /** 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/IntakeCone.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java index 121b3de..42be3e2 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java @@ -103,9 +103,9 @@ 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()); + .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..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(); @@ -131,7 +134,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(() ->{ @@ -148,7 +153,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..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();} @@ -34,40 +32,24 @@ public enum GamePiece {CUBE, CONE, NONE} public Command groundCube() { - return new IntakeGroundCube().andThen(this.prepareCarry()); - } - - 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()); + return new IntakeGroundCube().andThen(this.prepareCarry()).withName("groundCube"); } public Command clawIntakeCube() { - return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)); + 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) .andThen(claw.scoreGamePiece()) - .andThen(this.prepareCarry())); + .andThen(this.prepareCarry())).withName("scoreGamePieceAuto"); } public Command scoreFirstGamePieceAuto() { return claw.determineGamePiece() @@ -75,7 +57,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 +65,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 +79,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)); + 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)) - .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + return new ParallelCommandGroup(claw.intakeCone(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeConeHumanPlayer"); } public Command preparePoop() { 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..29b3ebb --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/PhotonVisionSystem.java @@ -0,0 +1,107 @@ +package team3176.robot.subsystems.vision; + +import java.io.File; +import java.util.ArrayList; +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.targeting.PhotonTrackedTarget; + +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; +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"); + 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(List.of(realCam),List.of(camera2Robot),field); + } + estimator = new PhotonPoseEstimator(field, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, realCam, camera2Robot); + } + public static PhotonVisionSystem getInstance() { + if (instance == null) { + instance = new PhotonVisionSystem(); + } + 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(); + 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 new file mode 100644 index 0000000..c1ba6bf --- /dev/null +++ b/src/main/java/team3176/robot/subsystems/vision/SimPhotonVision.java @@ -0,0 +1,48 @@ +package team3176.robot.subsystems.vision; + +import java.util.List; + +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +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.Transform3d; +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. + + VisionSystemSim simVision = new VisionSystemSim("photonvision"); + PhotonPoseEstimator estimator; + + 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); + + } + public void switchAllaince(AprilTagFieldLayout field) { + simVision.removeVisionTargets("apriltags"); + simVision.addVisionTargets(field); + } + @Override + public void periodic() { + + Pose2d currentPose = Drivetrain.getInstance().getSimNoNoisePose(); + simVision.update(currentPose); + } + +} 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 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; + } +} 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" + } + ] + }