diff --git a/shuffleboard.json b/shuffleboard.json new file mode 100644 index 0000000..b994472 --- /dev/null +++ b/shuffleboard.json @@ -0,0 +1,393 @@ +{ + "tabPane": [ + { + "title": "SmartDashboard", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": {} + } + }, + { + "title": "LiveWindow", + "autoPopulate": true, + "autoPopulatePrefix": "LiveWindow/", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": {} + } + }, + { + "title": "Robot", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 60.0, + "showGrid": true, + "hgap": 10.0, + "vgap": 10.0, + "titleType": 1, + "tiles": { + "0,0": { + "size": [ + 6, + 8 + ], + "content": { + "_type": "Grid Layout", + "_title": "Match", + "Layout/Number of columns": 1, + "Layout/Number of rows": 3, + "Layout/Label position": "HIDDEN", + "_children": { + "0,0": { + "_type": "Basic FMS Info", + "_title": "Basic FMS Info" + }, + "0,1": { + "_type": "Field", + "_title": "Field", + "Game/Game": "A2021_Infinite_Recharge", + "Visuals/Robot Icon Size": 50.0, + "Visuals/Show Outside Circles": false + }, + "0,2": { + "_type": "Grid Layout", + "_title": "Match Time", + "Layout/Number of columns": 3, + "Layout/Number of rows": 1, + "Layout/Label position": "BOTTOM", + "_children": { + "0,0": { + "_type": "Number Bar", + "_title": "Autonomous", + "Range/Min": 0.0, + "Range/Max": 15.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 4, + "Visuals/Show text": true + }, + "1,0": { + "_type": "Number Bar", + "_title": "Teleoperated", + "Range/Min": 0.0, + "Range/Max": 90.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + }, + "2,0": { + "_type": "Number Bar", + "_title": "Climb", + "Range/Min": 0.0, + "Range/Max": 30.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + } + } + } + } + } + }, + "0,8": { + "size": [ + 6, + 5 + ], + "content": { + "_type": "List Layout", + "_title": "Power", + "Layout/Label position": "HIDDEN", + "_children": [ + { + "_type": "Graph", + "_title": "Graph", + "Graph/Visible time": 30.0, + "Graph/X-axis auto scrolling": true, + "Y-axis/Automatic bounds": true, + "Y-axis/Upper bound": 1.0, + "Y-axis/Lower bound": -1.0, + "Y-axis/Unit": "ul" + }, + { + "_type": "PDP", + "_title": "PDP", + "Visuals/Show voltage and current values": true + } + ] + } + }, + "6,0": { + "size": [ + 3, + 4 + ], + "content": { + "_type": "Robot Preferences", + "_title": "Robot Preferences", + "Miscellaneous/Show search box": false + } + }, + "9,0": { + "size": [ + 7, + 8 + ], + "content": { + "_type": "Subsystem Layout", + "_title": "Drive", + "Layout/Label position": "HIDDEN", + "_children": [ + { + "_type": "Swerve Drivebase", + "_title": "", + "Visuals/Show velocity vectors": true + }, + { + "_type": "Grid Layout", + "_title": "Layout", + "Layout/Number of columns": 2, + "Layout/Number of rows": 1, + "Layout/Label position": "HIDDEN", + "_children": { + "1,0": { + "_type": "Grid Layout", + "_title": "Layout", + "Layout/Number of columns": 1, + "Layout/Number of rows": 2, + "Layout/Label position": "BOTTOM", + "_children": { + "0,1": { + "_type": "Split Button Chooser", + "_title": "Speed" + }, + "0,0": { + "_type": "3-Axis Accelerometer", + "_title": "Accelerometer", + "Accelerometer/Range": "k16G", + "Visuals/Show value": true, + "Visuals/Precision": 2, + "Visuals/Show tick marks": true + } + } + }, + "0,0": { + "_type": "Gyro", + "_title": "Gyro", + "Visuals/Major tick spacing": 45.0, + "Visuals/Starting angle": 180.0, + "Visuals/Show tick mark ring": true, + "Visuals/Counter clockwise": false + } + } + } + ] + } + }, + "16,0": { + "size": [ + 7, + 8 + ], + "content": { + "_type": "Camera Stream", + "_title": "Camera", + "Crosshair/Show crosshair": false, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": false, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + }, + "10,8": { + "size": [ + 6, + 5 + ], + "content": { + "_type": "Grid Layout", + "_title": "Turret", + "Layout/Number of columns": 2, + "Layout/Number of rows": 1, + "Layout/Label position": "HIDDEN", + "_children": { + "1,0": { + "_type": "Boolean Box", + "_title": "Shooter Safety", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + "0,0": { + "_type": "List Layout", + "_title": "Layout", + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Number Bar", + "_title": "Speed", + "Range/Min": 0.0, + "Range/Max": 100.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 0, + "Visuals/Show text": true + }, + { + "_type": "Number Bar", + "_title": "Angle", + "Range/Min": 0.0, + "Range/Max": 100.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 0, + "Visuals/Show text": true + }, + { + "_type": "Number Bar", + "_title": "Hood", + "Range/Min": 0.0, + "Range/Max": 100.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 0, + "Visuals/Show text": true + } + ] + } + } + } + }, + "6,8": { + "size": [ + 4, + 5 + ], + "content": { + "_type": "Grid Layout", + "_title": "Cargo", + "Layout/Number of columns": 1, + "Layout/Number of rows": 2, + "Layout/Label position": "HIDDEN", + "_children": { + "0,1": { + "_type": "Grid Layout", + "_title": "Layout", + "Layout/Number of columns": 3, + "Layout/Number of rows": 1, + "Layout/Label position": "BOTTOM", + "_children": { + "0,0": { + "_type": "Number Bar", + "_title": "Intake", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + }, + "1,0": { + "_type": "Number Bar", + "_title": "Serializer", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + }, + "2,0": { + "_type": "Number Bar", + "_title": "Storage", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + } + } + }, + "0,0": { + "_type": "Grid Layout", + "_title": "Layout", + "Layout/Number of columns": 1, + "Layout/Number of rows": 1, + "Layout/Label position": "BOTTOM", + "_children": { + "0,0": { + "_type": "Split Button Chooser", + "_title": "Extender" + } + } + } + } + } + }, + "6,4": { + "size": [ + 3, + 4 + ], + "content": { + "_type": "List Layout", + "_title": "Climber", + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Number Bar", + "_title": "Claws", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + }, + { + "_type": "Number Bar", + "_title": "Arm", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + } + ] + } + }, + "16,8": { + "size": [ + 7, + 5 + ], + "content": { + "_type": "Camera Stream", + "_title": "Limelight", + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": false, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + } + } + } + } + ], + "windowGeometry": { + "x": 0.0, + "y": 25.0, + "width": 1680.0, + "height": 1025.0 + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8d03887..f8fda41 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -345,4 +345,13 @@ public final class Constants { public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV; public static final double DEGREES_PER_PIXEL = 1 / PIXELS_PER_DEGREE; } + + public static final class AutoConstants { + public static final double PATH_MAX_VEL = 3.0; + public static final double PATH_MAX_ACCEL = 3.0; + public static final double LOCK_ON_DURATION = 0.8; + public static final double LOCK_ON_TIME_ALLOWANCE = 1.6; + public static final double STORAGE_TIME_TWO_BALLS = 1.5; + public static final double STORAGE_TIME_ONE_BALL = 0.8; + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 59c67a5..085b6d7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -4,51 +4,36 @@ package frc4388.robot; -import java.io.File; import java.util.logging.Level; import java.util.logging.Logger; -import com.diffplug.common.base.Errors; - import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; -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.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; -import frc4388.utility.RobotTime; -import frc4388.utility.Vector2D; /** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the TimedRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.gradle file in the + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the * project. */ public class Robot extends TimedRobot { private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName()); Command m_autonomousCommand; - private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - - // private double current; // private static DesmosServer desmosServer = new DesmosServer(8000); - public static Alliance alliance; - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. + * This function is run when the robot is first started up and should be used for any initialization + * code. */ @Override public void robotInit() { + LOGGER.fine("robotInit()"); LOGGER.log(Level.ALL, "Logging Test 1/8"); LOGGER.log(Level.SEVERE, "Logging Test 2/8"); LOGGER.log(Level.WARNING, "Logging Test 3/8"); @@ -57,138 +42,88 @@ public class Robot extends TimedRobot { LOGGER.log(Level.FINE, "Logging Test 6/8"); LOGGER.log(Level.FINER, "Logging Test 7/8"); LOGGER.log(Level.FINEST, "Logging Test 8/8"); - // var path = - // PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move - // Forward.path").toFile()); - // LOGGER.finest(path::toString); - LOGGER.fine("robotInit()"); - - - // LOGGER.fine("Sssssssssh."); - // DriverStation.silenceJoystickConnectionWarning(true); - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our - // autonomous chooser on the dashboard. + // Instantiate our RobotContainer. This will perform all our button bindings, and put our autonomous + // chooser on the dashboard. m_robotContainer = new RobotContainer(); - // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod); - SmartDashboard.putData(CommandScheduler.getInstance()); // desmosServer.start(); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); ExtenderIntakeGroup.setDirectionToOut(); } /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. * *

- * This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. + * This runs after the mode specific periodic functions, but before LiveWindow and SmartDashboard + * integrated updating. */ @Override public void robotPeriodic() { - m_robotTime.updateTimes(); - Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); - Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. - Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); - - // System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString()); - // System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString()); - // System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString()); - - // current = - // m_robotContainer.m_robotBoomBoom.getCurrent() + - // m_robotContainer.m_robotClimber.getCurrent(); //+ - // m_robotContainer.m_robotHood.getCurrent() + - // m_robotContainer.m_robotIntake.getCurrent() + - // m_robotContainer.m_robotExtender.getCurrent() + - // m_robotContainer.m_robotSerializer.getCurrent() + - // m_robotContainer.m_robotStorage.getCurrent() + - // m_robotContainer.m_robotSwerveDrive.getCurrent(); - // m_robotContainer.m_robotTurret.getCurrent(); - // SmartDashboard.putNumber("Total Robot Current Draw", current); - // SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); - // SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); - // 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. + // 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(); - - // print odometry data to smart dashboard for debugging (if causing timeout - // errors, you can comment it) } /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. + * This function is called once each time the robot enters Disabled mode. You can use it to reset + * any subsystem information you want to clear when the robot is disabled. */ @Override public void disabledInit() { LOGGER.fine("disabledInit()"); - m_robotTime.endMatchTime(); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override - public void disabledPeriodic() { - m_robotContainer.m_robotVisionOdometry.setLEDs(false); - } + public void disabledPeriodic() {} /** - * This autonomous runs the autonomous command selected by your - * {@link RobotContainer} class. + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { LOGGER.fine("autonomousInit()"); - Robot.alliance = DriverStation.getAlliance(); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } - m_robotTime.startMatchTime(); } /** * This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void teleopInit() { LOGGER.fine("teleopInit()"); - Robot.alliance = DriverStation.getAlliance(); + DriverStation.silenceJoystickConnectionWarning(true); - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. + // This makes sure that the autonomous stops running when teleop starts running. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - m_robotTime.startMatchTime(); - DriverStation.silenceJoystickConnectionWarning(true); } /** * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override public void testInit() { @@ -199,6 +134,5 @@ public class Robot extends TimedRobot { * This function is called periodically during test mode. */ @Override - public void testPeriodic() { - } + public void testPeriodic() {} } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6ddb1e..b482307 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,7 +4,6 @@ package frc4388.robot; -import java.util.ArrayList; import java.util.Objects; import java.util.logging.Logger; @@ -18,28 +17,26 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.XboxController; 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.wpilibj2.command.CommandGroupBase; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc4388.robot.Constants.AutoConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.PathRecorder; -import frc4388.robot.commands.RunCommandForTime; -import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; -import frc4388.robot.commands.ExtenderIntakeCommands.RunExtender; -import frc4388.robot.commands.ShooterCommands.TrackTarget; +import frc4388.robot.commands.shooter.TimedWaitUntilCommand; +import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shuffleboard.CommandSchedule; import frc4388.robot.commands.shuffleboard.ShooterTuner; import frc4388.robot.subsystems.BoomBoom; @@ -55,579 +52,286 @@ import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.VisionOdometry; -import frc4388.utility.Vector2D; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; +//TODO: Try using ConditionalCommand for subsystem default commands. +//TODO: Replace Path Recorder with Auto Chooser. +//TODO: Add POV button pad bindings as an example. +//XXX: Re-enable extender in autonomous. + /** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very little robot logic should - * actually be handled in the {@link Robot} periodic methods (other than the - * scheduler calls). Instead, the structure of the robot (including subsystems, - * commands, and button mappings) should be declared here. + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); - // RobotMap + /* Robot Map */ public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40); public final Climber m_robotClimber = new Climber(m_robotMap.elbow); - public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.frontLeft, m_robotMap.frontRight, m_robotMap.backLeft, m_robotMap.backRight, m_robotMap.gyro); + public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); - public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); - private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad + public final LED m_robotLED = new LED(m_robotMap.LEDController); public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - /* Autonomous */ + /* Dashboard Tools */ private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive); - + private final SendableChooser autoChooser = new SendableChooser(); private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom); private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false); - // Controllers - private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + + /* Controllers */ + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); - - public static boolean softLimits = true; + + private static boolean softLimits = true; // control mode switching - public static enum ControlMode { SHOOTER, CLIMBER }; - public static ControlMode currentControlMode = ControlMode.SHOOTER; + private enum ControlMode { + SHOOTER, CLIMBER + } - // turret mode switching - private enum TurretMode { MANUAL, AUTONOMOUS }; - private TurretMode currentTurretMode = TurretMode.MANUAL; - - // climber mode switching - private enum ClimberMode { MANUAL, AUTONOMOUS }; - private ClimberMode currentClimberMode = ClimberMode.MANUAL; + private ControlMode currentControlMode = ControlMode.SHOOTER; // drive on off mode switching - private enum DriveMode { ON, OFF }; + private enum DriveMode { + ON, OFF + } + private DriveMode currentDriveMode = DriveMode.ON; - // private SendableChooser quickAutoChooser = new SendableChooser<>(); - public SendableChooser autoChooser = new SendableChooser(); - -/** - * SmartDash - * - Limelight cam X - * - Limit switches X - * - Shooter RPM X - * - Distance to target x - * - target locked - * - claws boolean - * - field - */ - -// private SequentialCommandGroup makeTheWeirdGroup() { -// SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), -// new ParallelCommandGroup( -// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), -// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0) -// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. -// return weirdAutoShootingGroup; -// } /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - autoChooser.addOption("DriveOffLineAndShoot", driveOffLineAndShoot); - autoChooser.addOption("OneBallAuto", oneBallAuto); - autoChooser.setDefaultOption("TwoBallAuto", twoBallAuto); - autoChooser.addOption("ThreeBallAuto", threeBallAuto); - // autoChooser.addOption("instantThreeBallSingleShotAutoSequence", instantThreeBallSingleShotAutoSequence); - autoChooser.addOption("instantThreeBallDoubleSingleShotAutoSequence", instantThreeBallDoubleSingleShotAutoSequence); + autoChooser.addOption("threeBallPlus", threeBallPlus); SmartDashboard.putData("AutoChooser", autoChooser); + Preferences.initString("Autonomous", "Three Ball"); + configureButtonBindings(); /* Default Commands */ // Swerve Drive with Input - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> { - if (currentDriveMode == DriveMode.ON) { - m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), - getDriverController().getRight(), true); - } - if (currentDriveMode == DriveMode.OFF) { - m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false); - } - }, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + if (currentDriveMode == DriveMode.ON) { + m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true); + } + if (currentDriveMode == DriveMode.OFF) { + m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false); + } + }, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); // Intake with Triggers - m_robotIntake.setDefaultCommand( - new RunCommand(() -> m_robotIntake.runWithTriggers( - getOperatorController().getLeftTriggerAxis(), - getOperatorController().getRightTriggerAxis()), - m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); + m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand")); - // Serializer Manual - m_robotSerializer.setDefaultCommand( - new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), - m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + // TODO: Comment + m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand")); - // Turret Manual - m_robotTurret.setDefaultCommand( - new RunCommand(() -> { - if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - }, m_robotTurret)); + // Serializer Manual + m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand")); - // Hood Manual - m_robotHood.setDefaultCommand( - new RunCommand(() -> { - if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getLeftY()); } - if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } - }, m_robotHood)); + // Turret Manual + m_robotTurret.setDefaultCommand(new RunCommand(() -> { + if (currentControlMode == ControlMode.SHOOTER) { + m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); + } + if (currentControlMode == ControlMode.CLIMBER) { + m_robotTurret.runTurretWithInput(0); + } + }, m_robotTurret)); - // //Climber Manual - m_robotClimber.setDefaultCommand( - new RunCommand(() -> { - if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } - if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); } - }, m_robotClimber)); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber)); - - m_robotBoomBoom.setDefaultCommand( - new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom) - ); + // Hood Manual + m_robotHood.setDefaultCommand(new RunCommand(() -> { + if (currentControlMode == ControlMode.SHOOTER) { + m_robotHood.runHood(getOperatorController().getLeftY()); + } + if (currentControlMode == ControlMode.CLIMBER) { + m_robotHood.runHood(0); + } + }, m_robotHood)); + // Climber Manual + m_robotClimber.setDefaultCommand(new RunCommand(() -> { + if (currentControlMode == ControlMode.SHOOTER) { + m_robotClimber.setMotors(0.0); + } + if (currentControlMode == ControlMode.CLIMBER) { + m_robotClimber.setMotors(-getOperatorController().getRightY()); + } + }, m_robotClimber)); + + SmartDashboard.putData("Shooter Tuner", m_shooterTuner); SmartDashboard.putData("Shooter Tuner", m_shooterTuner); SmartDashboard.putData("Command Schedule", m_commandSchedule); } /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + * Use this method to define your button->command mappings. Buttons can be created by instantiating + * a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or + * {@link XboxController}), and then passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - //! Driver Buttons + // ! Driver Buttons - // Start > Calibrate Odometry - new JoystickButton(getDriverController(), XboxController.Button.kBack.value) - .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + // Start > Calibrate Odometry + new JoystickButton(getDriverController(), XboxController.Button.kBack.value).whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - // Start > Calibrate Odometry - new JoystickButton(getDriverController(), XboxController.Button.kStart.value) - .whenPressed(m_robotSwerveDrive::resetGyro); + // Start > Calibrate Odometry + new JoystickButton(getDriverController(), XboxController.Button.kStart.value).whenPressed(m_robotSwerveDrive::resetGyro); - // Left Bumper > Shift Down - new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // Left Bumper > Shift Down + new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - // Right Bumper > Shift Up - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + // Right Bumper > Shift Up + new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whenPressed(new InstantCommand(() -> switchControlMode())) - .whenReleased(new InstantCommand(() -> switchControlMode())); + new JoystickButton(getDriverController(), XboxController.Button.kA.value).whenPressed(new InstantCommand(() -> switchControlMode())).whenReleased(new InstantCommand(() -> switchControlMode())); - new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whenPressed(new InstantCommand(() -> switchDriveMode())) - .whenReleased(new InstantCommand(() -> switchDriveMode())); + new JoystickButton(getDriverController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> switchDriveMode())).whenReleased(new InstantCommand(() -> switchDriveMode())); - //! Operator Buttons + // ! Operator Buttons - // Right Bumper > Storage Out - new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // Right Bumper > Storage Out + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - // Left Bumper > Storage In - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // Left Bumper > Storage In + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - // B > Toggle claws - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); + // B > Toggle claws + new JoystickButton(getOperatorController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); - // X > Toggles extender in and out - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + // X > Toggles extender in and out + new JoystickButton(getOperatorController(), XboxController.Button.kX.value).whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - // A > Spit Out Ball - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)) - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); + // A > Spit Out Ball + new JoystickButton(getOperatorController(), XboxController.Button.kA.value).whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)).whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); - // Y > Full aim command + // Y > Full aim command // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); - + // .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, + // m_robotVisionOdometry)); - //! Test Buttons + // ! Test Buttons // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED)); // * aim with turret to target); + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, + // m_robotVisionOdometry, false, false)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + // .whenPressed(new RunCommandForTime(new RunCommand(() -> + // m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), + // m_robotTurret), 1.0)); + new JoystickButton(getOperatorController(), XboxController.Button.kY.value).whileHeld(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom)); // * aim with turret to target); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - - - //! Button Box Buttons - // Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) + // ! Button Box Buttons + // Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, + // climber, and extender) // SmartDashboard.putData("BB LEFT ON", new SequentialCommandGroup( - // new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret), - // new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret), - - // new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood), - // new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood), - - // new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender) + // new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret), + // new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret), + + // new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood), + // new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood), + + // new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender) // )); - + // SmartDashboard.putData("BB LEFT OFF", new SequentialCommandGroup( - // new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret), - // new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret), - - // new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood), - // new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood), + // new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret), + // new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret), - // new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender), + // new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood), + // new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood), - // new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret), - // new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood), - // new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender), - // new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender), - // new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber) + // new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender), + + // new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret), + // new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood), + // new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender), + // new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, + // m_robotExtender), + // new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber) // )); // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) - // .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) - // .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) - - // .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) - // .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) - - // .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - - // .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) - - // .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + // .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + // .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + // .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + // .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) - // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - - // Middle Switch > Climber and Shooter mode switching + // .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), + // m_robotExtender)) + + // .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + // .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + + // .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + // .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + + // .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), + // m_robotExtender)) + + // .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), + // m_robotTurret)) + // .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + // .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) + // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, + // m_robotExtender)) + // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + + // Middle Switch > Climber and Shooter mode switching // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) - // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); - // // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.)) - // // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); - + // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) + // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); + // // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.)) + // // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) - // .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); + // .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) + // .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); - // // Left Button > Extender In - new JoystickButton(getDriverController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // // Left Button > Extender In + new JoystickButton(getDriverController(), XboxController.Button.kX.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - // Left Button > Extender Out - new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // Left Button > Extender Out + new JoystickButton(getDriverController(), XboxController.Button.kY.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } - /** - * Generate autonomous - * @param maxVel max velocity for the path (null to override default value of 5.0) - * @param maxAccel max acceleration for the path (null to override default value of 5.0) - * @param inputs strings (paths) or commands you want to run (in order) - * @return array of commands - */ - public SequentialCommandGroup buildAuto(Double maxVel, Double maxAccel, Object... inputs) { - maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY); - maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION); - - ArrayList commands = new ArrayList(); - // commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - // commands.add(new InstantCommand(() -> m_robotSwerveDrive.m, m_robotSwerveDrive)); - - PIDController xController = SwerveDriveConstants.X_CONTROLLER; - PIDController yController = SwerveDriveConstants.Y_CONTROLLER; - ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - // parse input - for (int i=0; i m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive)); - commands.add(new PPSwerveControllerCommand( - traj, - m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, - xController, - yController, - thetaController, - m_robotSwerveDrive::setModuleStates, - m_robotSwerveDrive)); - } - - if (inputs[i] instanceof Command) { - commands.add((Command) inputs[i]); - } - } - - commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive)); - Command[] ret = new Command[commands.size()]; - ret = commands.toArray(ret); - SequentialCommandGroup seqCG = new SequentialCommandGroup(ret); - return seqCG; + private boolean isLockedOn() { + return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn(); } - // ! ways to not coast - // // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive); - // * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive); - // * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels. - // * 3b. try to only set the drive motors to brake if in auto mode. - // * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive); - - // ? 1.0 input, 1 second: 134 inches - // ? 0.75 input, 1 second: 48 inches - // ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS - - double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate. - - double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 - double offset = 10.0; // * distance (in inches) from ball that we actually want to stop - - // ! ball positions are "unit tested" - Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub. - Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. - Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs. - - private SequentialCommandGroup shoot(double storageRunTime) { - return new SequentialCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true) - ) - ); - } - - private ParallelRaceGroup shoot(double storageRunTime, double timeout) { - return new SequentialCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true) - ) - ).withTimeout(timeout + storageRunTime); - } - - SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true) - )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - - SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.3, true) - )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - - SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new ParallelCommandGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 4.0, true) - )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - - Command resetGyro = new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive); - Command brakeDrive = new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive); - - private Command brakeStorage(double storageRunTime) { - return new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotIntake), storageRunTime, true); - } - - SequentialCommandGroup extendThenAimTurret() { - return new SequentialCommandGroup( - new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), - new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 0.5, true) // TODO: optimize time - ); - } - - ParallelDeadlineGroup idleDrumUntilShootingFirstBall() { - return new ParallelDeadlineGroup( - extendThenAimTurret(), - new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(9000), m_robotBoomBoom) - ); - } - - private ParallelDeadlineGroup intakeWithPath1(double intakeRunTime) { - return new ParallelDeadlineGroup( - new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time - new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer), - buildAuto(3.0, 3.0, "JMove1") // TODO: make faster? - ); - } - - private ParallelDeadlineGroup intakeWithPath2(double intakeRunTime) { - return new ParallelDeadlineGroup( - new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time - new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer), - buildAuto(3.0, 3.0, "JMove2") // TODO: make faster? - ); - } - - ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1(3.0), weirdAutoShootingGroup2); - // ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3); - - ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup( - intakeWithPath2(2.8), - new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom), - new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true) - ); - - // ! DRIVE OFF LINE AND SHOOT (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET) - SequentialCommandGroup driveOffLineAndShoot = new SequentialCommandGroup( - resetGyro, - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond), // * go backwards five feet - brakeDrive, - shoot(1.0), - brakeStorage(0.1) - ); - - // ! ONE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET) - SequentialCommandGroup oneBallAuto = new SequentialCommandGroup( - shoot(1.0), - brakeStorage(0.1) - ); - - // ! TWO BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE) - SequentialCommandGroup twoBallAuto = new SequentialCommandGroup( - idleDrumUntilShootingFirstBall(), - shoot(1.0, 4.0), // TODO: optimize time - brakeStorage(0.1), - intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget - shoot(5.0), // TODO: optimize time - brakeStorage(0.1) - ); - - // ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE) - SequentialCommandGroup threeBallAuto = new SequentialCommandGroup( - idleDrumUntilShootingFirstBall(), - shoot(0.8), // TODO: optimize time - brakeStorage(0.1), - intakeWithPathAndTrackTarget, - // intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget - shoot(0.8), // TODO: optimize time - brakeStorage(0.1), - intakeWithPath2AndIdleShooterAndAimTurret, - shoot(4.0), // TODO: optimize time - brakeStorage(0.1) - ); - - // private final CommandGroupBase instantThreeBallSingleShotAutoSequence = CommandGroupBase.sequence( - // // Preloaded Ball - // new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - // new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"), - // new RunExtender(m_robotExtender).withName("DeployExtender"), - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstBallTarget"), - // new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstBallFeed"), - // new WaitCommand(3.0).withName("FirstBallShootTimer"), - // new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstBallStopFeed"), - // // Second Ball - // new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - // new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"), - // buildAuto(3.0, 3.0, "JMove1").withName("JMove1"), - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("SecondBallTarget"), - // new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("SecondBallFeed"), - // new WaitCommand(3.0).withName("SecondBallShootTimer"), - // new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("SecondBallStopFeed"), - // // Third Ball - // new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - // new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"), - // buildAuto(3.0, 3.0, "JMove2").withName("JMove2"), - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget"), - // new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"), - // new WaitCommand(3.0).withName("ThirdBallShootTimer"), - // new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"), - // new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"), - // new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed") - // ); - - private static final class InstAutoConst { - private static final double PATH_MAX_VEL = 3.0; - private static final double PATH_MAX_ACCEL = 3.0; - private static final double TRACK_TIME_ALLOWANCE = 1.0; - private static final double STORAGE_TIME_TWO_BALLS = 1.5; - private static final double STORAGE_TIME_ONE_BALL = 0.8; - } - private final CommandGroupBase instantThreeBallDoubleSingleShotAutoSequence = CommandGroupBase.sequence( - new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"), - new RunExtender(m_robotExtender).withName("DeployExtender"), - new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"), - new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"), - // Preload and Second Ball - buildAuto(InstAutoConst.PATH_MAX_VEL, InstAutoConst.PATH_MAX_ACCEL, "JMove1").withName("JMove1"), - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstSecondBallTarget").withTimeout(InstAutoConst.TRACK_TIME_ALLOWANCE + 1.0), - new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstSecondBallFeed"), - new WaitCommand(InstAutoConst.STORAGE_TIME_TWO_BALLS).withName("FirstSecondBallShootTimer").deadlineWith(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, false).withName("FirstSecondBallTarget")), - new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstSecondBallStopFeed"), - // Third Ball - new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"), - buildAuto(InstAutoConst.PATH_MAX_VEL, InstAutoConst.PATH_MAX_ACCEL, "JMove2").withName("JMove2"), - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget").withTimeout(InstAutoConst.TRACK_TIME_ALLOWANCE + 1.0), - new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"), - new WaitCommand(InstAutoConst.STORAGE_TIME_ONE_BALL).withName("ThirdBallShootTimer").deadlineWith(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, false).withName("FirstSecondBallTarget")), - new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"), - new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"), - new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed") - ); - /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -637,27 +341,74 @@ public class RobotContainer { return autoChooser.getSelected(); } + private final CommandGroupBase threeBallPlus = CommandGroupBase.sequence( + new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"), + // new RunExtender(m_robotExtender).withName("DeployExtender"), + new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"), + // Get Second Ball + new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"), + makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove1").withName("JMove1"), + // Shoot Preloaded and Second Ball + makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"), + // Get Third Ball + new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"), + makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove2").withName("JMove2"), + // Shoot Third Ball + makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"), + // Stop + new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StopRunningIntake"), + new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"), + new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed") + ); + + private ParallelDeadlineGroup makeTimeoutTrackShotGroup(double lockOnDuration, double lockOnTimeAllowance, double storageRunTime, String name) { + return CommandGroupBase.sequence( + new TimedWaitUntilCommand(this::isLockedOn, lockOnDuration).withTimeout(lockOnTimeAllowance).withName(name + "LockOn"), + new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage).withName(name + "Feed"), + new WaitCommand(storageRunTime).withName(name + "ShootTimer"), + new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName(name + "StopFeed")).deadlineWith(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom).withName(name + "Track") + ); + } + + /** + * Generate autonomous + * @param maxVel max velocity for the path (null to override default value of 5.0) + * @param maxAccel max acceleration for the path (null to override default value of 5.0) + * @param inputs strings (paths) or commands you want to run (in order) + * @return array of commands + */ + private SequentialCommandGroup makePathingGroup(Double maxVel, Double maxAccel, String input) { + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + PathPlannerTrajectory pathTrajectory = PathPlanner.loadPath(input, Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY), Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION)); + PathPlannerState initialState = pathTrajectory.getInitialState(); + Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + + return new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition), m_robotSwerveDrive), + new PPSwerveControllerCommand(pathTrajectory, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), + new InstantCommand(m_robotSwerveDrive::stopModules, m_robotSwerveDrive) + ); + } + public void switchControlMode() { - if (currentControlMode == ControlMode.SHOOTER) { - currentControlMode = ControlMode.CLIMBER; - } else { - currentControlMode = ControlMode.SHOOTER; - } + currentControlMode = currentControlMode == ControlMode.SHOOTER ? ControlMode.CLIMBER : ControlMode.SHOOTER; } public void switchDriveMode() { - if (currentDriveMode == DriveMode.ON) { - currentDriveMode = DriveMode.OFF; - } else { - currentDriveMode = DriveMode.ON; - } + currentDriveMode = currentDriveMode == DriveMode.ON ? DriveMode.OFF : DriveMode.ON; } - public static DeadbandedXboxController getDriverController() { + public DeadbandedXboxController getDriverController() { return m_driverXbox; } - public static DeadbandedXboxController getOperatorController() { + public DeadbandedXboxController getOperatorController() { return m_operatorXbox; } @@ -665,19 +416,6 @@ public class RobotContainer { return m_buttonBox; } - public static void setSoftLimits(boolean set) { - softLimits = set; - } - - /** - * Get odometry. - * - * @return Odometry - */ - public Pose2d getOdometry() { - return m_robotSwerveDrive.getOdometry(); - } - /** * Set odometry to given pose. * diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 2a68991..4985b54 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,7 +4,6 @@ package frc4388.robot; - import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -16,7 +15,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; @@ -30,293 +28,255 @@ import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.shuffleboard.SendableCANSparkMax; /** - * Defines and holds all I/O objects on the Roborio. This is useful for unit - * testing and modularization. + * Defines and holds all I/O objects on the Roborio. This is useful for unit testing and + * modularization. */ public class RobotMap { + // #region Swerve Subsystem + public final WPI_TalonFX frontLeftSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX frontLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX frontRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX frontRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX backLeftSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + public final WPI_TalonFX backLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + public final WPI_TalonFX backRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + public final WPI_TalonFX backRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); + public final CANCoder frontLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder frontRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder backLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); + public final CANCoder backRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public RobotMap() { - // configureLEDMotorControllers(); - configureSwerveMotorControllers(); - configureShooterMotorControllers(); - configureIntakeMotors(); - configureExtenderMotors(); - configureSerializerMotors(); - configureStorageMotors(); - configureClimberMotors(); - } - - /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); -// public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID); - - void configureLEDMotorControllers() {} - - - /* Swerve Subsystem */ - public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + public SwerveModule frontLeft; + public SwerveModule frontRight; + public SwerveModule backLeft; + public SwerveModule backRight; public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); + // #endregion Swerve Subsystem - public SwerveModule leftFront; - public SwerveModule leftBack; - public SwerveModule rightFront; - public SwerveModule rightBack; + // #region Extender Subsystem + public final CANSparkMax extenderMotor = new SendableCANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + // #endregion Extender Subsystem + + // #region Intake Subsystem + public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); + // #endregion Intake Subsystem + + // #region Serializer Subsystem + public final CANSparkMax serializerBelt = new SendableCANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); + // #endregion Serializer Subsystem + + // #region Storage Subsystem + public final CANSparkMax storageMotor = new SendableCANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + // #endregion Storage Subsystem + + // #region Shooter System + // #region Boom Boom Subsystem + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + // #endregion Boom Boom Subsystem + + // #region Turret Subsystem + public final CANSparkMax shooterTurret = new SendableCANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + // #endregion Turret Subsystem + + // #region Hood Subsystem + public final CANSparkMax angleAdjusterMotor = new SendableCANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + // #endregion Hood Subsystem + // #endregion Shooter System + + // #region Climber Subsystem + public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); + // #endregion Climber Subsystem + + // #region Claws Subsystem + public final Servo leftClaw = new Servo(1); + public final Servo rightClaw = new Servo(2); + // #endregion Claws Subsystem + + // #region LED Subsystem + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // #endregion LED Subsystem + + public RobotMap() { + configureSwerveMotorControllers(); + configureExtenderMotors(); + configureIntakeMotors(); + configureSerializerMotors(); + configureStorageMotors(); + configureShooterMotorControllers(); + configureClimberMotors(); + registerDevices(); + } void configureSwerveMotorControllers() { + frontLeftSteerMotor.configFactoryDefault(); + frontLeftDriveMotor.configFactoryDefault(); + frontRightSteerMotor.configFactoryDefault(); + frontRightDriveMotor.configFactoryDefault(); + backLeftSteerMotor.configFactoryDefault(); + backLeftDriveMotor.configFactoryDefault(); + backRightSteerMotor.configFactoryDefault(); + backRightDriveMotor.configFactoryDefault(); + + frontLeftSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontLeftDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + + frontLeftDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontLeftSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + + frontLeftDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontLeftSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + + frontLeftSteerMotor.setNeutralMode(NeutralMode.Brake); + frontRightSteerMotor.setNeutralMode(NeutralMode.Brake); + backLeftSteerMotor.setNeutralMode(NeutralMode.Brake); + backRightSteerMotor.setNeutralMode(NeutralMode.Brake); + + frontLeftDriveMotor.setNeutralMode(NeutralMode.Coast); + frontRightDriveMotor.setNeutralMode(NeutralMode.Coast); + backLeftDriveMotor.setNeutralMode(NeutralMode.Coast); + backRightDriveMotor.setNeutralMode(NeutralMode.Coast); - SendableRegistry.setName(leftFrontSteerMotor, "SwerveDrive", "Left Front Steer Motor"); - SendableRegistry.setName(leftFrontWheelMotor, "SwerveDrive", "Left Front Wheel Motor"); - SendableRegistry.setName(rightFrontSteerMotor, "SwerveDrive", "Right Front Steer Motor"); - SendableRegistry.setName(rightFrontWheelMotor, "SwerveDrive", "Right Front Wheel Motor"); - SendableRegistry.setName(leftBackSteerMotor, "SwerveDrive", "Left Back Steer Motor"); - SendableRegistry.setName(leftBackWheelMotor, "SwerveDrive", "Left Back Wheel Motor"); - SendableRegistry.setName(rightBackSteerMotor, "SwerveDrive", "Right Back Steer Motor"); - SendableRegistry.setName(rightBackWheelMotor, "SwerveDrive", "Right Back Wheel Motor"); - - leftFrontSteerMotor.configFactoryDefault(); - leftFrontWheelMotor.configFactoryDefault(); - rightFrontSteerMotor.configFactoryDefault(); - rightFrontWheelMotor.configFactoryDefault(); - leftBackSteerMotor.configFactoryDefault(); - leftBackWheelMotor.configFactoryDefault(); - rightBackSteerMotor.configFactoryDefault(); - rightBackWheelMotor.configFactoryDefault(); - - leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - NeutralMode mode = NeutralMode.Coast; - leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - leftFrontWheelMotor.setNeutralMode(mode);// Coast - rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - rightFrontWheelMotor.setNeutralMode(mode);// Coast - leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); - leftBackWheelMotor.setNeutralMode(mode);// Coast - rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); - rightBackWheelMotor.setNeutralMode(mode);// Coast - // current limits - - leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); - rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); - leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); - rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); - - leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); - rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); - leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); - rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); - - leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); - rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); - leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); - rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); - - leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); - rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); - leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); - rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); - - leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, - SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); - leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, - SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, - SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); - rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, - SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - + frontLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + frontRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + backLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + backRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + + frontLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + frontRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + backLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + backRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + + frontLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + frontRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + backLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + backRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + + frontLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + frontRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + backLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + backRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + + frontLeft = new SwerveModule(frontLeftDriveMotor, frontLeftSteerMotor, frontLeftEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + frontRight = new SwerveModule(frontRightDriveMotor, frontRightSteerMotor, frontRightEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + backLeft = new SwerveModule(backLeftDriveMotor, backLeftSteerMotor, backLeftEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + backRight = new SwerveModule(backRightDriveMotor, backRightSteerMotor, backRightEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // config cancoder as remote encoder for swerve steer motors - leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); -} + frontLeftSteerMotor.configRemoteFeedbackFilter(frontLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backLeftSteerMotor.configRemoteFeedbackFilter(backLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + frontRightSteerMotor.configRemoteFeedbackFilter(frontRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + backRightSteerMotor.configRemoteFeedbackFilter(backRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + } -/* Climb Subsystem */ -public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO + void configureExtenderMotors() { + extenderMotor.restoreFactoryDefaults(); + extenderMotor.setInverted(true); + extenderMotor.setIdleMode(IdleMode.kBrake); + } -private void configureClimberMotors() { - SendableRegistry.setName(elbow, "Climber", "Elbow"); - SendableRegistry.setName(leftClaw, "Climber", "Left Claw"); - SendableRegistry.setName(rightClaw, "Climber", "Right Claw"); - elbow.configFactoryDefault(); - elbow.setNeutralMode(NeutralMode.Brake); -} -/* Hooks Subsystem */ -public final Servo leftClaw = new Servo(1); -public final Servo rightClaw = new Servo(2); + void configureIntakeMotors() { + intakeMotor.configFactoryDefault(); + intakeMotor.setInverted(false); + intakeMotor.setNeutralMode(NeutralMode.Coast); -// Shooter Config -/* Boom Boom Subsystem */ -public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); -public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); + intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); + } -// turret subsystem -public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + void configureSerializerMotors() { + serializerBelt.restoreFactoryDefaults(); + } -// hood subsystem -public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + void configureStorageMotors() { + storageMotor.restoreFactoryDefaults(); + storageMotor.setIdleMode(IdleMode.kCoast); + } -// Create motor CANSparkMax -void configureShooterMotorControllers() { - - // LEFT FALCON - SendableRegistry.setName(shooterFalconLeft, "BoomBoom", "Left Motor"); + void configureShooterMotorControllers() { + // Boom Boom Subsystem shooterFalconLeft.configFactoryDefault(); shooterFalconLeft.setNeutralMode(NeutralMode.Coast); shooterFalconLeft.setInverted(true); shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - - // RIGHT FALCON - SendableRegistry.setName(shooterFalconRight, "BoomBoom", "Right Motor"); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configFactoryDefault(); shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.setInverted(false); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - // m_shooterFalconRight.configPeakOutputForward(0, - // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) - shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + // shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); //(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.follow(shooterFalconLeft); - - // turret + + // Turret Subsystem shooterTurret.restoreFactoryDefaults(); shooterTurret.setIdleMode(IdleMode.kBrake); shooterTurret.setInverted(true); - - // hood subsystem + + // Hood Subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); } - /* Serializer Subsystem */ - public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); - public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); + void configureClimberMotors() { + elbow.configFactoryDefault(); + elbow.setNeutralMode(NeutralMode.Brake); + } - /* Intake Subsystem */ - public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); - public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + private void registerDevices() { + SendableRegistry.setName(frontLeftSteerMotor, "SwerveDrive", "Left Front Steer Motor"); + SendableRegistry.setName(frontLeftDriveMotor, "SwerveDrive", "Left Front Drive Motor"); + SendableRegistry.setName(frontRightSteerMotor, "SwerveDrive", "Right Front Steer Motor"); + SendableRegistry.setName(frontRightDriveMotor, "SwerveDrive", "Right Front Drive Motor"); + SendableRegistry.setName(backLeftSteerMotor, "SwerveDrive", "Left Back Steer Motor"); + SendableRegistry.setName(backLeftDriveMotor, "SwerveDrive", "Left Back Drive Motor"); + SendableRegistry.setName(backRightSteerMotor, "SwerveDrive", "Right Back Steer Motor"); + SendableRegistry.setName(backRightDriveMotor, "SwerveDrive", "Right Back Drive Motor"); - void configureIntakeMotors() { SendableRegistry.setName(intakeMotor, "Intake", "Intake Motor"); + SendableRegistry.setName((SendableCANSparkMax) extenderMotor, "Intake", "Extender Motor"); + SendableRegistry.setName((SendableCANSparkMax) serializerBelt, "Intake", "Serializer Belt"); + SendableRegistry.setName((SendableCANSparkMax) storageMotor, "Intake", "Storage Motor"); - intakeMotor.configFactoryDefault(); - intakeMotor.setInverted(false); - intakeMotor.setNeutralMode(NeutralMode.Coast); - - intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); - intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); - } + SendableRegistry.setName(shooterFalconLeft, "Shooter", "BoomBoom Left Motor"); + SendableRegistry.setName(shooterFalconRight, "Shooter", "BoomBoom Right Motor"); + SendableRegistry.setName((SendableCANSparkMax) shooterTurret, "Shooter", "Turret"); + SendableRegistry.setName((SendableCANSparkMax) angleAdjusterMotor, "Shooter", "Hood"); - void configureExtenderMotors() { - SendableRegistry.add(new SendableCANSparkMax(extenderMotor), "Intake", "Extender Motor"); - - extenderMotor.restoreFactoryDefaults(); - extenderMotor.setInverted(true); - extenderMotor.setIdleMode(IdleMode.kBrake); - } - - void configureSerializerMotors() { - SendableRegistry.add(new SendableCANSparkMax(serializerBelt), "Intake", "Serializer Belt"); - SendableRegistry.setName(serializerBeam, "Intake", "Serializer Beam"); - - serializerBelt.restoreFactoryDefaults(); - } - - /* Storage Subsystem */ - public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - - void configureStorageMotors() { - SendableRegistry.add(new SendableCANSparkMax(storageMotor), "Intake", "Storage Motor"); - storageMotor.restoreFactoryDefaults(); - storageMotor.setIdleMode(IdleMode.kCoast); + SendableRegistry.setName(elbow, "Climber", "Elbow"); + SendableRegistry.setName(leftClaw, "Climber", "Left Claw"); + SendableRegistry.setName(rightClaw, "Climber", "Right Claw"); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java deleted file mode 100644 index 5fe6a4e..0000000 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ /dev/null @@ -1,118 +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 frc4388.robot.commands.ShooterCommands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.VisionOdometry; - -public class AimToCenter extends CommandBase { - /** Creates a new AimWithOdometry. */ - Turret m_turret; - VisionOdometry m_visionOdometry; - - Supplier supplier; - Pose2d odo; - - // use odometry to find x and y later - double x; - double y; - double m_targetAngle; - - // public static Gains m_aimGains; - - public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier supplier) { - // Use addRequirements() here to declare subsystem dependencies. - m_turret = turret; - m_visionOdometry = visionOdometry; - - this.supplier = supplier; - - addRequirements(m_turret, m_visionOdometry); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - odo = this.supplier.get(); - // ! Yes I realize this stupid, yes it works I promise, coordinate system is funky - x = -odo.getY(); - y = -odo.getX(); - - SmartDashboard.putNumber("trans x", x); - SmartDashboard.putNumber("trans y", y); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - odo = this.supplier.get(); // * update odometry using really cool supplier -aarav - - m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360; - SmartDashboard.putNumber("Target Angle", m_targetAngle); - m_turret.runShooterRotatePID(m_targetAngle); - - // Check if limelight is within range (comment out to disable vision odo) - if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ - // m_visionOdometry.updateOdometryWithVision(); - // m_visionOdometry.setLEDs(true); - } - else{ - // m_visionOdometry.setLEDs(false); - } - } - - public static double angleToCenter(double x, double y, double gyro) { - double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) - // double angle = Math.toDegrees(Math.atan2(y, -x) - gyro); - return (angle - 360); - } - - public static double aaravAngleToCenter(double x, double y, double gyro) { - - double actualGyro = -gyro + 90; - - double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro; - if (-x > 0) { return (180 + exp); } - if (-x < 0) { return (360 + exp); } - - if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated - if (x == 0 && y < 0) { return (90 - actualGyro); } - - System.err.println("Invalid case."); - return 0; - } - - /** - * Checks if in deadzone. - * @param angle Angle to check. - * @return True if in deadzone. - */ - public static boolean isDeadzone(double angle) { - if (angle == Double.NaN) { - return false; - } - return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT)); - } - - // 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/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java deleted file mode 100644 index 3de6da1..0000000 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.ShooterCommands; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Hood; -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.VisionOdometry; - -// 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 Seek extends SequentialCommandGroup { - - /** Seeks. - * Seeking -> Sought - * @author Aarav Shah - * @blame Aarav Shah (thomas did this) - */ - public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds)); - } - - /** Seeks. - * Seeking -> Sought - * @author Aarav Shah - * @blame Aarav Shah (thomas did this) - */ - public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds, double[] fakeOdo) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds)); - } -} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java deleted file mode 100644 index e255e4b..0000000 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ /dev/null @@ -1,232 +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 frc4388.robot.commands.ShooterCommands; - -import java.util.Objects; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants; -import frc4388.robot.Robot; -import frc4388.robot.RobotContainer; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Hood; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.VisionOdometry; -import frc4388.utility.DummySensor; -import frc4388.utility.Gains; - -public class Shoot extends CommandBase { - - // subsystems - private SwerveDrive swerve; - private Turret turret; - private BoomBoom drum; - private Hood hood; - private VisionOdometry visionOdometry; - - private boolean toShoot; - - // given - private double odoX, odoY; - private double distance; - private double gyroAngle; - - // targets - private double targetAngle, targetVel, targetHood; - - // pid - private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS; - private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS; - private double error; - private double prevError; - private double kP, kI, kD; - private double proportional, integral, derivative; - private double output, normOutput; - private double tolerance; - - boolean timerStarted; - long startTime; - private double timeTolerance; - - private boolean isAimedInTolerance; - private int inverted; - private double initialSwerveRotation; - - private boolean endsWithLimelight; - - private double[] fakeOdo; - - /** - * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball - * - * @param swerve Drive Train - * @param drum Shooter Drum - * @param turret Shooter Turret - * @param hood Shooter Hood - * - * @author Aarav Shah - */ - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, boolean toShoot, boolean endsWithLimelight) { - // Use addRequirements() here to declare subsystem dependencies. - this.swerve = swerve; - this.drum = drum; - this.turret = turret; - this.hood = hood; - this.visionOdometry = visionOdometry; - - this.toShoot = toShoot; - this.endsWithLimelight = endsWithLimelight; - - kP = turretGains.kP; - kI = turretGains.kI; - kD = turretGains.kD; - - proportional = 0; - integral = 0; - derivative = 0; - - tolerance = 10.0; - timeTolerance = 500; - isAimedInTolerance = false; - - this.inverted = 1; - - this.fakeOdo = null; - - addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry); - } - - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo, boolean toShoot, boolean endsWithLime) { - this(swerve, drum, turret, hood, visionOdometry, toShoot, endsWithLime); - if (fakeOdo.length != 2) { - throw new IllegalArgumentException(); - } - this.fakeOdo = fakeOdo; - } - - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) { - this(swerve, drum, turret, hood, visionOdometry, false, false); - } - - /** - * Updates error for custom PID. - */ - public void updateError() { - targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - - error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; - - // if (error > 180) { - // error = error - 360; - // this.inverted = -1; - // } else { - // this.inverted = 1; - // } - if (error > 180) { error = error - 360; } - - isAimedInTolerance = (Math.abs(error) <= tolerance); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - timerStarted = false; - startTime = 0; - - this.odoX = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[0]), -this.swerve.getOdometry().getY()); - this.odoY = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[1]), -this.swerve.getOdometry().getX()); - - this.distance = Math.hypot(odoX, odoY); - - this.gyroAngle = this.swerve.getRegGyro().getDegrees(); - this.initialSwerveRotation = gyroAngle; - - // get targets (shooter tables) - this.targetVel = drum.getVelocity(distance); - this.targetHood = drum.getHood(distance); - - this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - - // initial error - updateError(); - prevError = error; - } - - /** - * Run custom PID. - */ - public void runPID() { - prevError = error; - updateError(); - - this.proportional = error; - this.integral = integral + (error * Constants.LOOP_TIME); - this.derivative = (error - prevError) / Constants.LOOP_TIME; - this.output = kP * proportional + kI * integral + kD * derivative; - this.normOutput = (output / 360); // inverted; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - runPID(); - - SmartDashboard.putNumber("Error", this.error); - SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); - SmartDashboard.putNumber("Normalized Output", this.normOutput); - - this.turret.runTurretWithCustomPID(normOutput); - this.swerve.driveWithInput(RobotContainer.getDriverController().getLeft(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative - - if (this.toShoot) { - this.hood.runAngleAdjustPID(this.targetHood); - this.drum.runDrumShooterVelocityPID(this.targetVel); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - - System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase()); - // ? return to initial swerve rotation - // swerve.driveWithInput(0, 0, initialSwerveRotation, true); - - // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); - // this.turret.m_boomBoomRotateMotor.set(0.0); - - // ? should stop the turret and the swerve - ////this.swerve.stopModules(); - this.turret.runTurretWithInput(0.0); - - if (this.toShoot) { - this.hood.runHood(0.0); - this.drum.runDrumShooter(0.0); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (!endsWithLimelight) { - if (isAimedInTolerance && !timerStarted) { - timerStarted = true; - startTime = System.currentTimeMillis(); - } - SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - } else { - return this.visionOdometry.m_camera.getLatestResult().hasTargets(); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java deleted file mode 100644 index b4313a4..0000000 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ /dev/null @@ -1,234 +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 frc4388.robot.commands.ShooterCommands; - -import java.util.ArrayList; -import java.util.HashMap; -import java.util.logging.Logger; -import java.util.stream.Collector; -import java.util.stream.Collectors; - -import org.opencv.core.Point; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.RobotContainer; -import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Hood; -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.Vision; -import frc4388.robot.subsystems.VisionOdometry; -import frc4388.utility.LEDPatterns; -import frc4388.utility.Vector2D; -import frc4388.utility.desmos.DesmosServer; - -public class TrackTarget extends CommandBase { - private static final Logger LOGGER = Logger.getLogger(TrackTarget.class.getSimpleName()); - /** Creates a new TrackTarget. */ - SwerveDrive m_swerve; - Turret m_turret; - VisionOdometry m_visionOdometry; - BoomBoom m_boomBoom; - Hood m_hood; - LED m_leds; - - boolean isAuto; - - static double velocity; - static double hoodPosition; - - ArrayList points = new ArrayList<>(); - - private static boolean targetLocked = false; - public static boolean isTargetNotLocked() { - return !targetLocked; - } - private double velocityTolerance = 300.0; - private double hoodTolerance = 5.0; - - boolean isExecuted = false; - - // timing - boolean isAimed; - - boolean timerStarted; - long startTime; - private double timeTolerance; - - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds, boolean isAuto) { - m_turret = turret; - m_boomBoom = boomBoom; - m_hood = hood; - m_visionOdometry = visionOdometry; - m_leds = leds; - - this.isAuto = isAuto; - this.timeTolerance = 1000; - - addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); - SmartDashboard.putNumber("Distance Adjust", -35); - SmartDashboard.putBoolean("Target Locked", false); - } - - public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds) { - this(turret, boomBoom, hood, visionOdometry, leds, false); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - timerStarted = false; - startTime = 0; - - velocity = 0; - hoodPosition = 0; - m_visionOdometry.setDriverMode(false); - m_visionOdometry.setLEDs(true); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - points = m_visionOdometry.getTargetPoints(); - if (points.isEmpty()) { - LOGGER.severe("Vision Obscured"); - return; - } - // points = getFakePoints(); - //// points = filterPoints(points); - - Point average = VisionOdometry.averagePoint(points); - // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); - - // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || - // Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) - // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); - // else - // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), - // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), - // true); - - double regressedDistance = getDistance(average.y); - - // ! offset trig solution - double trigDesiredOffset = 10; // * inches - double trigAngleOffset = Math.toDegrees(Math.atan(trigDesiredOffset / regressedDistance)); // * degrees - double trigPixelOffset = trigAngleOffset * VisionConstants.PIXELS_PER_DEGREE; // * pixels - - // ! offset csv solution - double csvAngleOffset = m_boomBoom.getOffset(regressedDistance); // * degrees - double csvPixelOffset = csvAngleOffset * VisionConstants.PIXELS_PER_DEGREE; // * pixels - - double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2.1; - - // m_turret.runTurretWithInput(output - trigPixelOffset); - m_turret.runTurretWithInput(output - csvPixelOffset); - - // ! no longer a +30 lol -aarav - double distAdj = SmartDashboard.getNumber("Distance Adjust", -35); - velocity = m_boomBoom.getVelocity(regressedDistance + distAdj); - hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj); - - m_boomBoom.runDrumShooterVelocityPID(velocity); - m_hood.runAngleAdjustPID(hoodPosition); - - double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); - double currentHood = this.m_hood.getEncoderPosition(); - - targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2); - - - SmartDashboard.putNumber("Distance", regressedDistance - 35); - SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); - SmartDashboard.putNumber("Vel Target Track", velocity); - SmartDashboard.putBoolean("Target Locked", targetLocked); - - if (targetLocked){ - m_leds.setPattern(LEDConstants.SHOOTING_PATTERN); - } - else{ - m_leds.setPattern(LEDConstants.DEFAULT_PATTERN); - } - } - - public ArrayList filterPoints(ArrayList points) { - Point average = VisionOdometry.averagePoint(points); - - HashMap pointDistances = new HashMap<>(); - double distanceSum = 0; - - for(Point point : points) { - Vector2D difference = new Vector2D(point); - difference.subtract(new Vector2D(average)); - - double mag = difference.magnitude(); - distanceSum += mag; - - pointDistances.put(point, mag); - } - - final double averageDist = distanceSum / points.size(); - return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 1.3 * averageDist).collect(Collectors.toList()); - } - - public final ArrayList getFakePoints() { - ArrayList fakePoints = new ArrayList<>(); - - for(int i = 0; i < 10; i++) { - Point p = new Point((Math.random() * 20) - 10 + (VisionConstants.LIME_HIXELS/2), (Math.random() * 20) - 10 + (VisionConstants.LIME_VIXELS/2)); - fakePoints.add(p); - } - - return fakePoints; - } - - public final double getDistance(double averageY) { - double y_rot = averageY / VisionConstants.LIME_VIXELS; - y_rot *= Math.toRadians(VisionConstants.V_FOV); - y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; - y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); - - double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); - - double regressedDistance = distanceRegression(distance); - regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; - SmartDashboard.putNumber("Distance from Lime 123", distance); - SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance); - return regressedDistance; - } - - public final double distanceRegression(double distance) { - return (79.6078 * Math.pow(1.01343, distance) - 56.6671); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_visionOdometry.setLEDs(false); - SmartDashboard.putBoolean("Target Locked", false); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (this.isAuto) { - if (targetLocked && !timerStarted) { - timerStarted = true; - startTime = System.currentTimeMillis(); - } - return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/TimedWaitUntilCommand.java b/src/main/java/frc4388/robot/commands/shooter/TimedWaitUntilCommand.java new file mode 100644 index 0000000..fc69bcf --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/TimedWaitUntilCommand.java @@ -0,0 +1,41 @@ +package frc4388.robot.commands.shooter; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class TimedWaitUntilCommand extends CommandBase { + private final BooleanSupplier m_condition; + private final double m_duration; + + protected Timer m_timer = new Timer(); + + public TimedWaitUntilCommand(BooleanSupplier condition, double seconds, Subsystem... requirements) { + m_condition = requireNonNullParam(condition, "condition", "TimedWaitUntilCommand"); + m_duration = seconds; + addRequirements(requirements); + } + + @Override + public void initialize() { + m_timer.reset(); + m_timer.start(); + } + + @Override + public boolean isFinished() { + if (m_condition.getAsBoolean()) + return m_timer.hasElapsed(m_duration); + m_timer.reset(); + return false; + } + + @Override + public void end(boolean interrupted) { + m_timer.stop(); + } +} diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java new file mode 100644 index 0000000..4f6d61a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -0,0 +1,86 @@ +package frc4388.robot.commands.shooter; + +import java.util.ArrayList; +import java.util.logging.Logger; + +import org.opencv.core.Point; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +// TODO: Try putting swerve drive rotation to be perpendicular to the center point to prevent being rolled while shooting. +// FIXME: Regression and distance offset happen separately, and the turret offset uses the regressed distance without the distance offset. +public class TrackTarget extends CommandBase { + private static final Logger LOGGER = Logger.getLogger(TrackTarget.class.getSimpleName()); + private static final double TURRET_OFFSET_DEGREES = 10; + + private final VisionOdometry m_visionOdometry; + private final Turret m_turret; + private final Hood m_hood; + private final BoomBoom m_boomBoom; + + public TrackTarget(VisionOdometry visionOdometry, Turret turret, Hood hood, BoomBoom boomBoom) { + this.m_visionOdometry = visionOdometry; + this.m_turret = turret; + this.m_hood = hood; + this.m_boomBoom = boomBoom; + } + + @Override + public void initialize() { + m_visionOdometry.setDriverMode(false); + m_visionOdometry.setLEDs(true); + SmartDashboard.putBoolean("TrackTarget/Running", true); + } + + @Override + public void execute() { + ArrayList points = m_visionOdometry.getTargetPoints(); + if (!points.isEmpty()) { + Point pointAverage = VisionOdometry.averagePoint(points); + + double distanceTarget = getDistance(pointAverage.y); + double turretOffset = Math.toDegrees(Math.atan(TURRET_OFFSET_DEGREES / distanceTarget)) * VisionConstants.PIXELS_PER_DEGREE; + distanceTarget -= SmartDashboard.getNumber("TrackTarget/Target Distance Offset", -35); + SmartDashboard.putNumber("TrackTarget/Target Distance (Regressed and Offset)", distanceTarget); + + double turretOutput = 2.1 * ((pointAverage.x + 40) - VisionConstants.LIME_HIXELS / 2.0) / VisionConstants.LIME_HIXELS; + double hoodTarget = m_boomBoom.getHood(distanceTarget); + double velocityTarget = m_boomBoom.getVelocity(distanceTarget); + SmartDashboard.putNumber("TrackTarget/Target Hood", hoodTarget); + SmartDashboard.putNumber("TrackTarget/Target Velocity", velocityTarget); + + m_turret.runTurretWithInput(turretOutput - turretOffset); + m_hood.runAngleAdjustPID(hoodTarget); + m_boomBoom.runDrumShooterVelocityPID(velocityTarget); + } else { + LOGGER.severe("No vision target points."); + SmartDashboard.putString("TrackTarget/Target Distance (Regressed and Offset)", "obscured"); + SmartDashboard.putString("TrackTarget/Target Hood", "obscured"); + SmartDashboard.putString("TrackTarget/Target Velocity", "obscured"); + } + } + @Override + public void end(boolean interrupted) { + SmartDashboard.putBoolean("TrackTarget/Running", false); + } + private static double getDistance(double averageY) { + double yRot = averageY / VisionConstants.LIME_VIXELS; + yRot *= Math.toRadians(VisionConstants.V_FOV); + yRot -= Math.toRadians(VisionConstants.V_FOV) / 2; + yRot += Math.toRadians(VisionConstants.LIME_ANGLE); + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(yRot); + + double regressedDistance = regression(distance, 79.6078, 1.01343, -56.6671); + regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; + return regressedDistance; + } + private static double regression(double x, double a, double b, double c) { + return a * Math.pow(b, x) + c; + } +} diff --git a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java index 0348541..2f02e68 100644 --- a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java +++ b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java @@ -13,12 +13,16 @@ import java.util.logging.Logger; import com.diffplug.common.base.Errors; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -52,7 +56,15 @@ public class ShooterTuner extends CommandBase { var tab = Shuffleboard.getTab("Shooter Tuner"); if (tab.getComponents().isEmpty()) { var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5); - manual.add("Manual Shooter Data", m_shotEditor); + //XXX: + var sbi = new SendableBuilderImpl(); + sbi.setTable(NetworkTableInstance.getDefault().getTable("ShooterTuner/Manual Shooter Data")); + sbi.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d); + sbi.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d); + sbi.addDoubleProperty("Turret Offset", () -> tableOverrideEntry.turretOffset, d -> tableOverrideEntry.turretOffset = d); + sbi.addDoubleProperty("Distance", () -> tableOverrideEntry.distance, d -> tableOverrideEntry.distance = d); + sbi.addBooleanProperty("Measure Distance", () -> measureDistance, b -> measureDistance = b); + // manual.add("Manual Shooter Data", m_shotEditor); manual.add("Manual Data Appender", m_shotCsvAppender); var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5); csv.add("Shooter Table", m_tableEditor); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index c2f0a41..a86ec46 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -24,6 +24,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; import com.diffplug.common.base.StringPrinter; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; @@ -37,8 +38,7 @@ import frc4388.utility.NumericData; public class BoomBoom extends SubsystemBase { private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName()); public WPI_TalonFX m_shooterFalconLeft; - public WPI_TalonFX m_shooterFalconRight; - public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; + public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; double speed2; @@ -217,4 +217,11 @@ public class BoomBoom extends SubsystemBase { public double getCurrent(){ return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent(); } + + public boolean isLockedOn() { + double currentDrum = Math.min(m_shooterFalconLeft.getSelectedSensorVelocity(), m_shooterFalconRight.getSelectedSensorVelocity()); + double targetDrum = Math.max(m_shooterFalconLeft.get(), m_shooterFalconRight.get()); + return Math.abs(currentDrum - targetDrum) > VELOCITY_TOLERANCE; + } + private static final double VELOCITY_TOLERANCE = 300.0; } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 2a471a6..57153ae 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -143,4 +143,11 @@ public class Hood extends SubsystemBase { public double getCurrent(){ return m_angleAdjusterMotor.getOutputCurrent(); } + + public boolean isLockedOn() { + double currentHood = getEncoderPosition(); + double targetHood = m_angleAdjusterMotor.get(); + return Math.abs(currentHood - targetHood) > HOOD_TOLERANCE; + } + private static final double HOOD_TOLERANCE = 5.0; } diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index 99702fc..0d11a64 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -1,22 +1,16 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SerializerConstants; -import edu.wpi.first.wpilibj.DigitalInput; import com.revrobotics.CANSparkMax; public class Serializer extends SubsystemBase{ private CANSparkMax m_serializerBelt; - // private CANSparkMax m_serializerShooterBelt; - private DigitalInput m_serializerBeam; private boolean serializerState; - public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { + public Serializer(CANSparkMax serializerBelt) { m_serializerBelt = serializerBelt; - m_serializerBeam = serializerBeam; m_serializerBelt.set(0); - // m_serializerShooterBelt.set(0); } /** @@ -26,23 +20,6 @@ public class Serializer extends SubsystemBase{ public void setSerializer(double input){ m_serializerBelt.set(input); } - /** - * Gets The State Of The Beam - * @return The State Of The Beam - */ - public boolean getBeam() { - return m_serializerBeam.get(); - } - - /** - * Sets The Serializer State With The Beam - * @param state Your State Of The Button - * @param beambroken The State of the Beam Senser - */ - public void setSerializerStateWithBeam() { - if (m_serializerBeam.get()) setSerializer(0.0); - else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED); - } /** * Gets the Serializer State @@ -51,6 +28,7 @@ public class Serializer extends SubsystemBase{ public boolean getSerializerState() { return serializerState; } + public double getCurrent(){ return m_serializerBelt.getOutputCurrent(); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8a2c3e3..33ffc47 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -25,10 +25,10 @@ import frc4388.utility.Gains; public class SwerveDrive extends SubsystemBase { - private SwerveModule m_leftFront; - private SwerveModule m_leftBack; - private SwerveModule m_rightFront; - private SwerveModule m_rightBack; + private SwerveModule m_frontLeft; + private SwerveModule m_frontRight; + private SwerveModule m_backLeft; + private SwerveModule m_backRight; double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; @@ -56,16 +56,18 @@ public class SwerveDrive extends SubsystemBase { public final Field2d m_field = new Field2d(); - public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, - WPI_Pigeon2 gyro) { + public SwerveDrive(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft, SwerveModule backRight, WPI_Pigeon2 gyro) { + //XXX: + setName("Swerve Drive"); + addChild("Gyro", m_gyro); - m_leftFront = leftFront; - m_leftBack = leftBack; - m_rightFront = rightFront; - m_rightBack = rightBack; + m_frontLeft = frontLeft; + m_frontRight = frontRight; + m_backLeft = backLeft; + m_backRight = backRight; m_gyro = gyro; - modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; + modules = new SwerveModule[] {m_frontLeft, m_frontRight, m_backLeft, m_backRight}; // m_poseEstimator = new SwerveDrivePoseEstimator( // getRegGyro(),//m_gyro.getRotation2d(), @@ -295,10 +297,10 @@ public class SwerveDrive extends SubsystemBase { } public double getCurrent(){ - return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent(); + return m_frontLeft.getCurrent() + m_frontRight.getCurrent() + m_backRight.getCurrent() + m_backLeft.getCurrent(); } public double getVoltage(){ - return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage(); + return m_frontLeft.getVoltage() + m_frontRight.getVoltage() + m_backRight.getVoltage() + m_backLeft.getVoltage(); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index c6a09fe..6369591 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; import frc4388.utility.Gains; @@ -253,5 +252,10 @@ public class Turret extends SubsystemBase { public double getCurrent(){ return m_boomBoomRotateMotor.getOutputCurrent(); } - + public boolean isLockedOn() { + double currentTurret = getBoomBoomAngleDegrees(); + double targetTurret = m_boomBoomRotateMotor.get(); + return Math.abs(currentTurret - targetTurret) > TURRET_TOLERANCE; + } + private static final double TURRET_TOLERANCE = 0.2; } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index c5b6d7d..54b1874 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -20,6 +20,7 @@ import org.photonvision.targeting.TargetCorner; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -67,6 +68,12 @@ public class VisionOdometry extends SubsystemBase { System.out.println("Result: " + result.hasTargets() + ", latency: " + latency); ArrayList points = new ArrayList<>(); + //XXX: REMOVE + if (RobotBase.isSimulation()) { + points = new ArrayList<>(); + points.add(new Point(320, 240)); + return points; + } if(!result.hasTargets()) return points; @@ -84,7 +91,6 @@ public class VisionOdometry extends SubsystemBase { points.add(new Point(corner.x, VisionConstants.LIME_VIXELS - corner.y)); } } - return points; } diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java index 5da9a74..1caf512 100644 --- a/src/main/java/frc4388/utility/AnsiLogging.java +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -8,7 +8,6 @@ import java.io.OutputStream; import java.io.PrintStream; import java.io.PrintWriter; import java.io.StringWriter; -import java.nio.charset.StandardCharsets; import java.time.ZoneId; import java.time.ZonedDateTime; import java.util.Map; @@ -27,12 +26,11 @@ import com.diffplug.common.base.Errors; import org.fusesource.jansi.Ansi.Attribute; import org.fusesource.jansi.Ansi.Color; - -import edu.wpi.first.hal.HAL; - import org.fusesource.jansi.AnsiConsole; import org.fusesource.jansi.AnsiPrintStream; +import edu.wpi.first.hal.HAL; + public class AnsiLogging { public static final Level LEVEL = Level.ALL; private static final AnsiPrintStream ANSI_CONSOLE_STREAM = AnsiConsole.err(); @@ -62,6 +60,7 @@ public class AnsiLogging { DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage)); // Store the handler for HAL to use when sending errors to DriverStation. halLoggerHandler = new LoggingAnsiConsoleHandler(new HalOutputStream()); + // Logger.getLogger(name) } catch (IOException exception) { exception.printStackTrace(AnsiConsole.sysErr()); } diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java deleted file mode 100644 index 5bda50f..0000000 --- a/src/main/java/frc4388/utility/RobotTime.java +++ /dev/null @@ -1,76 +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 frc4388.utility; - -/** - *

Keeps track of Robot times like time passed, delta time, etc - *

All times are in milliseconds - */ -public class RobotTime { - private long m_currTime = System.currentTimeMillis(); - public long m_deltaTime = 0; - - private long m_startRobotTime = m_currTime; - public long m_robotTime = 0; - public long m_lastRobotTime = 0; - - private long m_startMatchTime = 0; - public long m_matchTime = 0; - public long m_lastMatchTime = 0; - - public long m_frameNumber = 0; - - /** - * Private constructor prevents other classes from instantiating - */ - private RobotTime(){} - - private static RobotTime instance = null; - - /** - * Gets the instance of Robot Time. If there is no instance running one will be created. - * @return instance of Robot Time - */ - public static RobotTime getInstance() { - if (instance == null) { - instance = new RobotTime(); - } - return instance; - } - - /** - * Call this once per periodic loop. - */ - public void updateTimes() { - m_lastRobotTime = m_robotTime; - m_lastMatchTime = m_matchTime; - - m_currTime = System.currentTimeMillis(); - m_robotTime = m_currTime - m_startRobotTime; - m_deltaTime = m_robotTime - m_lastRobotTime; - m_frameNumber++; - - if (m_startMatchTime != 0) { - m_matchTime = m_currTime - m_startMatchTime; - } - } - - /** - * Call this in both the auto and periodic inits - */ - public void startMatchTime() { - if (m_startMatchTime == 0) { - m_startMatchTime = m_currTime; - } - } - - /** - * Call this in disabled init - */ - public void endMatchTime() { - m_startMatchTime = 0; - m_matchTime = 0; - } -} diff --git a/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java b/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java index f3983d2..97453c9 100644 --- a/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java +++ b/src/main/java/frc4388/utility/shuffleboard/SendableCANSparkMax.java @@ -5,19 +5,17 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; -public class SendableCANSparkMax implements Sendable { - private final CANSparkMax m_canSparkMax; - - public SendableCANSparkMax(CANSparkMax canSparkMax) { - m_canSparkMax = canSparkMax; +public class SendableCANSparkMax extends CANSparkMax implements Sendable { + public SendableCANSparkMax(int deviceId, MotorType type) { + super(deviceId, type); } @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Motor Controller"); builder.setActuator(true); - builder.setSafeState(m_canSparkMax::stopMotor); - builder.addDoubleProperty("Value", m_canSparkMax::get, m_canSparkMax::set); + builder.setSafeState(this::stopMotor); + builder.addDoubleProperty("Value", this::get, this::set); } } diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java deleted file mode 100644 index 525080f..0000000 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ /dev/null @@ -1,118 +0,0 @@ -package frc4388.commands; - -import org.junit.Test; - -import frc4388.robot.commands.ShooterCommands.AimToCenter; - -import org.junit.Assert; - -public class AimToCenterTest { - - private static final double DELTA = 1e-15; - - /** - * Unit tests the isDeadzone function in AimToCenter.java - * @author Ryan Manley - * @link www.hoohle.com - */ - public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { - boolean output; - - //20 deg - output = AimToCenter.isDeadzone(20.); - Assert.assertFalse(output); - - //-10 deg - output = AimToCenter.isDeadzone(-10.); - Assert.assertTrue(output); - - //-1 deg - output = AimToCenter.isDeadzone(-1.); - Assert.assertTrue(output); - - //341 deg - output = AimToCenter.isDeadzone(341.); - Assert.assertTrue(output); - - //340 deg - output = AimToCenter.isDeadzone(340.); - Assert.assertTrue(output); - - //0 deg - output = AimToCenter.isDeadzone(0.); - Assert.assertFalse(output); - - //200 deg - output = AimToCenter.isDeadzone(200.); - Assert.assertTrue(output); - - //2000000 deg - output = AimToCenter.isDeadzone(2000000.); - Assert.assertTrue(output); - - //NaN deg - // output = AimToCenter.isDeadzone(Double.NaN); - // Assert.assertFalse(output); - } - - //@Test - public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() { - double actual; - double expected; - - //(5,5) Gyro = 0 deg - actual = AimToCenter.angleToCenter(5., 5., 0.); - expected = 180. + 45.; - Assert.assertEquals(expected, actual, DELTA); - - //(-5,5) Gyro = 0 deg - actual = AimToCenter.angleToCenter(-5.0, 5., 0.); - expected = 180. + 90. + 45.; - Assert.assertEquals(expected, actual, DELTA); - - //(-5,-5) Gyro = 0 deg - actual = AimToCenter.angleToCenter(-5.0, -5., 0.); - expected = 45.; - Assert.assertEquals(expected, actual, DELTA); - - //(5,-5) Gyro = 0 deg - actual = AimToCenter.angleToCenter(5., -5., 0.); - expected = 90. + 45.; - Assert.assertEquals(expected, actual, DELTA); - - //(0,0) Gyro = 0 deg - actual = AimToCenter.angleToCenter(0., 0., 0.); - Assert.assertNotNull(actual); - - //(5,5) Gyro = 180 deg - actual = AimToCenter.angleToCenter(5., 5., 180.); - expected = 45.; - Assert.assertEquals(expected, actual, DELTA); - - //(100,100) Gyro = 90 deg - actual = AimToCenter.angleToCenter(100., 100., 90.); - expected = 90. + 45.; - Assert.assertEquals(expected, actual, DELTA); - - //(null,5) Gyro = 0 deg - actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.); - expected = Double.NaN; - Assert.assertEquals(expected, actual, DELTA); - - //(null,null) Gyro = 0 deg - actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.); - expected = Double.NaN; - Assert.assertEquals(expected, actual, DELTA); - - //(null,null) Gyro = null deg - actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN); - expected = Double.NaN; - Assert.assertEquals(expected, actual, DELTA); - - //(5,5) Gyro = -20 deg - actual = AimToCenter.angleToCenter(5., -5., -45.); - expected = 180.; - Assert.assertEquals(expected, actual, DELTA); - - } -} diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java deleted file mode 100644 index cc96d41..0000000 --- a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java +++ /dev/null @@ -1,101 +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 frc4388.utility; - -import static org.junit.Assert.*; - -import org.junit.*; - -/** - * Add your docs here. - */ -public class RobotTimeUtilityTest { - RobotTime robotTime = RobotTime.getInstance(); - - @Test - public void testUpdateTimes() { - // Arrange - long lastTime; - robotTime.m_deltaTime = 0; - robotTime.m_robotTime = 0; - robotTime.m_lastRobotTime = 0; - robotTime.m_frameNumber = 0; - robotTime.endMatchTime(); - robotTime.m_lastMatchTime = 0; - - // Assert - assertEquals(0, robotTime.m_deltaTime); - assertEquals(0, robotTime.m_robotTime); - assertEquals(0, robotTime.m_lastRobotTime); - assertEquals(0, robotTime.m_frameNumber); - lastTime = robotTime.m_robotTime; - - // Act - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(true, robotTime.m_deltaTime > 0); - assertEquals(true, robotTime.m_robotTime > 0); - assertEquals(lastTime, robotTime.m_lastRobotTime); - assertEquals(1, robotTime.m_frameNumber); - lastTime = robotTime.m_robotTime; - - // Act - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(true, robotTime.m_deltaTime > 0); - assertEquals(true, robotTime.m_robotTime > 0); - assertEquals(lastTime, robotTime.m_lastRobotTime); - assertEquals(2, robotTime.m_frameNumber); - } - - @Test - public void testMatchTime() { - // Arrange - long lastTime; - - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(0, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - robotTime.startMatchTime(); - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(true, robotTime.m_matchTime > 0); - assertEquals(lastTime, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - wait(1); - robotTime.updateTimes(); - robotTime.endMatchTime(); - - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(lastTime, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(lastTime, robotTime.m_lastMatchTime); - } - - private void wait(int millis) { - try { - Thread.sleep(millis); - } catch (Exception e) {} - } -}