diff --git a/.github/workflows/compile.yml b/.github/workflows/compile.yml index 5824d6d..81ad063 100644 --- a/.github/workflows/compile.yml +++ b/.github/workflows/compile.yml @@ -20,6 +20,18 @@ jobs: steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v3 + # Download distance sensor library + - name: Download distance sensor library + run: wget -O distsen.zip https://github.com/REVrobotics/2m-Distance-Sensor/releases/download/v2022.0.4/2m-Distance-Sensor-SDK-2022.0.4.zip + # Extract the distance sensor + - name: Extract distance sensor library + run: unzip distsen.zip + # Create /root/wpilib/2023/ + - name: Create /root/wpilib/2023/ directory + run: mkdir -p /root/wpilib/2023/ + # Copy maven to /root/wpilib/2023/ + - name: Copy maven to /root/wpilib/2023/ + run: cp -R 2m-Distance-Sensor-SDK-2022.0.4/maven /root/wpilib/2023/ # Declares the repository safe and not under dubious ownership. - name: Add repository to git safe directories diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index dd26add..6df10e6 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -1,7 +1,8 @@ name: Format with Google Style on: - push: + schedule: + - cron: "0 9 * * *" # 9 am UTC is 3 am CST, which is our timezone permissions: contents: write diff --git a/.vscode/settings.json b/.vscode/settings.json index f320b56..f579099 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,8 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "editor.detectIndentation": false, + "editor.tabSize": 2, + "editor.insertSpaces": true, } diff --git a/src/main/java/frc/robot/ArbitraryValues.java b/src/main/java/frc/robot/ArbitraryValues.java new file mode 100644 index 0000000..2f1f63c --- /dev/null +++ b/src/main/java/frc/robot/ArbitraryValues.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** Add your docs here. */ +public class ArbitraryValues { + + /** + * arbitrary value for unknown doubles; intended for values that we don't know due to unbuilt bot; + * DONT USE FOR ACTUAL BOT + * + * @return 0.0 if the robot base isn't real; will throw exception if base is real + */ + public static double arbitraryDouble() { + if (RobotBase.isReal()) { + throw new IllegalStateException("unsafe value for use on bot"); + } + return 0.0; + } + + /** + * arbitrary value for unknown integer; intended for value that we don't know due to unbuilt bot; + * DON'T USE FOR ACTUAL BOT + * + * @return 0 if the robot base isn't real; will throw exception if base is real + */ + public static int arbitraryInteger() { + if (RobotBase.isReal()) { + throw new IllegalStateException("unsafe value for use on bot"); + } + return 0; + } + + /** + * arbitrary value for unknown boolean; intended for value that we don't know due to unbuilt bot; + * DON'T USE FOR ACTUAL BOT + * + * @return false if the robot base isn't real; will throw exception if base is real + */ + public static boolean arbitraryBoolean() { + if (RobotBase.isReal()) { + throw new IllegalStateException("unsafe value for use on bot"); + } + return false; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a7072a8..e30e617 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,7 @@ package frc.robot; +import frc.robot.positioning.Pose; import edu.wpi.first.math.util.Units; /** @@ -18,6 +19,16 @@ public final class Constants { public static class OperatorConstants { public static final int CONTROLLER_NUMBER = 0; + public static final int JOYSTICK_NUMBER = 1; + } + + public static class RobotDimensions { + /** The pose of the robot's center */ + public static final Pose CENTER_POSE = new Pose(0, 0, 8.5); + /** The front/back length of the robot's frame in inches */ + public static final double FRAME_LENGTH = 29.0; + /** The left/right width of the robot's frame in inches */ + public static final double FRAME_WIDTH = 29.0; } /** General constants for the drivetrain. Primarily used by {@link frc.robot.subsystems.Drive}. */ @@ -96,10 +107,178 @@ public static class ContollerButtons { /** General constants for the drivetrain. Primarily used by {@link frc.robot.subsystems.Drive}. */ public static class DriveConstants { /* CAN IDs for the drivetrain motor controllers */ - public static final int ID_TALON_FRONT_LEFT = 0; + public static final int ID_TALON_FRONT_LEFT = 3; public static final int ID_TALON_FRONT_RIGHT = 1; - public static final int ID_TALON_REAR_LEFT = 2; - public static final int ID_TALON_REAR_RIGHT = 3; + public static final int ID_TALON_REAR_LEFT = 4; + public static final int ID_TALON_REAR_RIGHT = 2; + } + + /* TODO: reorganize this class after Cow Town, make separate classes for each individual + * sub-subsystem + */ + public static class Arm { + /** The pose of the arm's base in robot oriented coordinates */ + public static final Pose BASE_POSE = new Pose(0, 0, 8.5); + /** The distance between the floor and the arm base in inches */ + public static final double BASE_DISTANCE_TO_FLOOR = 8.5; + /** Length in inches from the base of the arm to the shoulder joint */ + public static final double BASE_TO_SHOULDER_LENGTH = 19; + /** + * How high above the arm base should we keep the end effector to stop the arm from stabbing the + * robot? This only matters inside the frame perimeter. Distance in inches. + */ + public static final double KEEP_OUT_HEIGHT = 6; + + public static final int ID_TALON_ARM_SHOULDER = 6; + public static final int ID_TALON_ARM_WINCH = 2; /* this is actually a Talon, not a Talon SRX */ + public static final int ID_TALON_ARM_CLAW = 0; /* This is actually a Spark */ + public static final int ID_TALON_ARM_BASE = 5; + + public static final int CHANNEL_ANALOG_PRESSURE_SENSOR = 0; + + public static final double SETPOINT_PRESSURE_CONE = 5.7; + public static final double SETPOINT_PRESSURE_CUBE = 5.5; + + public static final boolean INVERTED_TALON_SENSOR_ARM_SHOULDER = true; + public static final boolean INVERTED_TALON_SENSOR_ARM_BASE = true; + + public static final boolean INVERTED_TALON_ARM_SHOULDER = false; + public static final boolean INVERTED_TALON_ARM_BASE = false; + + public static final boolean AUTO_ZERO_REVERSE_LIMIT_SHOULDER = true; + public static final boolean AUTO_ZERO_REVERSE_LIMIT_BASE = true; + + public static final double MIN_LENGTH_ARM_EXTENDER = 30.5; + + public static final double EXTRA_LENGTH_ARM_EXTENDER = 26; + + public static final double DONE_THRESHOLD_ARM_EXTENSION = 1.0; + /* DONE_THRESHOLD_ARM_CLAW is not tuned yet - update it when implementing pressure sensor */ + public static final double DONE_THRESHOLD_ARM_CLAW = 0.1; + + public static final double CONE_PRESSURE_THRESHOLD = 4.0; + + public static final double SPEED_WINCH_ARM_EXTENSION = 1.0; + + public static final double SPEED_ARM_CLAW = 1; + + public static final double SPEED_ARM_SHOULDER_HOMING = 0.5; + public static final double SPEED_ARM_BASE_HOMING = 0.5; + + public static class ShoulderPID { + public static final double kP = 1; + public static final double kI = 0.002; + public static final double kD = 100; + public static final double kFF = 0.0; + public static final double kIZone = 800; + public static final double ACCEPTABLE_ERROR = 50; + } + + public static class BasePID { + public static final double kP = 1; + public static final double kI = 0.001; + public static final double kD = 75; + public static final double kFF = 0.0; + public static final double kIZone = 150; + public static final double ACCEPTABLE_ERROR = 300; + } + + public static class Poses { + public static final Pose RESET = new Pose(0, 0, 44); + public static final Pose SCORE_LOWER = new Pose(0, 23, -6); + public static final Pose SCORE_MID_CONE = new Pose(0, 38, 32); + public static final Pose SCORE_MID_CUBE = new Pose(0, 38, 18); + public static final Pose SCORE_HIGH_CONE = new Pose(0, 55, 44); + public static final Pose SCORE_HIGH_CUBE = new Pose(0, 55, 28); + public static final Pose PICKUP = new Pose(-1, 32, -8); + public static final Pose HUMAN_SHELF = new Pose(0, 27, 32); + public static final Pose CARRYING = new Pose(0, 0, 44); + } + } + + public static class I2C { + public static int DISTANCE_SENSOR_MULTIPLEXER_PORT = 0; + public static int COLOR_SENSOR_MULTIPLEXER_PORT = 1; + } + + public static class Limits { + /** Maximum horizontal extension over the frame in inches */ + public static final double MAX_FRAME_EXTENSION = 48.0; + /** Maximum extended robot height in inches */ + public static final double MAX_EXTENDED_HEIGHT = 78.0; + /** Minimum distance from overextending we want to keep in inches */ + public static final double OVEREXTENSION_DANGER_DISTANCE = 4.0; + } + + /** Holds constants for things not related to the robot */ + public static class Game { + public static class Field { + public static class Grid { + public static class ConeNode { + public static class Top { + /** Height of the node in feet */ + public static final double HEIGHT = 3.8333; + /** Horizontal distance from the bumper to the node in feet */ + public static final double DISTANCE = 3.3125; + /** Height of the reflective tape off the ground in feet */ + public static final double TAPE_HEIGHT = 3.4896; + /** The distance from the top of the tape to the top of the node */ + public static final double ABOVE_TAPE = 0.0156; + } + + public static class Middle { + /** Height of the node in feet */ + public static final double HEIGHT = 2.8333; + /** Horizontal distance from the bumper to the node in feet */ + public static final double DISTANCE = 1.8958; + /** Height of the reflective tape off the ground in feet */ + public static final double TAPE_HEIGHT = 1.8438; + /** The distance from the top of the tape to the top of the node */ + public static final double ABOVE_TAPE = 0.6667; + } + + public static class CubeNode { + public static class Top { + /** Height of the node in feet */ + public static final double HEIGHT = 2.9583; + /** Horizontal distance from the bumper to the middle of the node in feet */ + public static final double DISTANCE = 3.3125; + } + + public static class Middle { + /** Height of the node in feet */ + public static final double HEIGHT = 1.9583; + /** Horizontal distance from the bumper to the middle of the node in feet */ + public static final double DISTANCE = 1.8958; + } + } + } + } + + public static class Objects { + public static class Cone { + /** Height in inches */ + public static final double HEIGHT = 12.8125; + /** Width of the square base in inches */ + public static final double WIDTH = 8.375; + /** Diameter of the bottom of the cone */ + public static final double BOTTOM_DIAMETER = 6.625; + /** Diameter of top bottom of the cone */ + public static final double TOP_DIAMETER = 1.75; + /** Average diameter of the cone's grabbable area */ + public static final double AVG_DIAMETER = 4.1875; + /** The pressure reading required to securely hold the cone */ + public static final double PRESSURE_TO_HOLD = -1; + } + + public static class Cube { + /** Side length of a properly inflated cube in inches */ + public static final double LENGTH = 9.5; + /** The pressure reading required to securely hold the cube */ + public static final double PRESSURE_TO_HOLD = -1; + } + } + } } /** @@ -121,5 +300,15 @@ public static class BalanceConstants { /* Angle threshholds for when the bot is considered balanced and not */ public static final double OFF_ANGLE_THRESHOLD = 10; public static final double ON_ANGLE_THRESHOLD = 5; + public static final double SPEED_MULTIPLIER = 0.4; + } + + public static class Plunger { + public static final int ID_PWMMOTOR_PLUNGER = 1; + public static final double SPEED_PLUNGER_DEPLOY = 0.2; + } + + public static class Camera { + public static final int PORT_CAMERA_AIM = 0; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..8b6a4f5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,3 +1,4 @@ + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -7,6 +8,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.positioning.ArmKinematics; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -14,8 +16,12 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ +@SuppressWarnings("unused") public class Robot extends TimedRobot { private Command m_autonomousCommand; + private Command m_homingRoutine; + + private Command displayAimVideo; private RobotContainer m_robotContainer; @@ -28,6 +34,10 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + displayAimVideo = m_robotContainer.getDisplayAimVideoCommand(); + + displayAimVideo.initialize(); } /** @@ -66,7 +76,23 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + + // Opens the claw if the arm is close to being overextended + checkOverexteneded(); + } + + private boolean isAlmostOverextended() { + return ArmKinematics.isAlmostOverextended(m_robotContainer.containerGetArmPose()); + } + + private void checkOverexteneded() { + if (isAlmostOverextended()) { + m_robotContainer.getRetractCommand().schedule(); + } else if (m_robotContainer.getRetractCommand().isScheduled()) { + m_robotContainer.getRetractCommand().end(true); + } + } @Override public void teleopInit() { @@ -76,12 +102,17 @@ public void teleopInit() { // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + // Opens the + m_robotContainer.getResetCommand().schedule(); } } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + + checkOverexteneded(); + } @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index edd8c90..d51d61d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,14 @@ package frc.robot; +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.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; @@ -16,7 +23,31 @@ import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.Limelight; import frc.robot.subsystems.TaskScheduler; +import frc.robot.commands.CloseClawCone; +import frc.robot.commands.CloseClawCube; +import frc.robot.commands.DeployPlunger; +import frc.robot.commands.DriveFieldOriented; +import frc.robot.commands.DriveForwards; +import frc.robot.commands.DriveReverse; +import frc.robot.commands.DriveRobotOriented; +import frc.robot.commands.ExtendArm; +import frc.robot.commands.ManualArm; +import frc.robot.commands.RetractArm; +import frc.robot.commands.StartLeavingCommunity; +import frc.robot.commands.StopLeavingCommunity; +import frc.robot.commands.moveToPose.MoveToPose; +import frc.robot.positioning.Pose; +import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.I2CManager; +import frc.robot.subsystems.Plunger; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.ArmClaw; +import frc.robot.subsystems.arm.ArmExtender; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; +@SuppressWarnings("unused") /** * 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} @@ -24,6 +55,38 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + // The robot's subsystems and commands are defined here... + private final Drive driveSubsystem = new Drive(); + /* BIG CHUNGUS ARM CODE */ + private final I2CManager I2CManager = new I2CManager(); + private final ArmBase armBase = new ArmBase(); + private final ArmShoulder armShoulder = new ArmShoulder(); + private final ArmExtender armExtender = new ArmExtender(I2CManager); + private final ArmClaw armClaw = new ArmClaw(); + private final Camera aimCamera = new Camera(Constants.Camera.PORT_CAMERA_AIM); + private final CompilationArm compilationArm = + new CompilationArm(armBase, armClaw, armExtender, armShoulder); + private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); + private RetractArm retractArmCommand = new RetractArm(armExtender); + private final Plunger plunger = new Plunger(); + private final DeployPlunger deployPlunger = new DeployPlunger(plunger); + private final StartLeavingCommunity startLeavingCommunity = new StartLeavingCommunity(driveSubsystem); + private final StopLeavingCommunity stopLeavingCommunity = new StopLeavingCommunity(driveSubsystem); + private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); + private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); + private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera); + + private final Balance balance = new Balance(driveSubsystem); + private final DriveRobotOriented driveRO = new DriveRobotOriented(driveSubsystem); + private final DriveFieldOriented driveFO = new DriveFieldOriented(driveSubsystem); + + private final Command openClaw = armClaw.manualOpenClawCommand(); + private final Command closeClaw = armClaw.manualCloseClawCommand(); + private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); + private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); + + private final ExtendArm extend = new ExtendArm(armExtender); + private final RetractArm retract = new RetractArm(armExtender); /* SUBSYSTEMS */ private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); private final Limelight limelight = new Limelight(); @@ -39,15 +102,30 @@ public class RobotContainer { private final ExampleAutoTask exampleAutoTask = new ExampleAutoTask(exampleCommand); // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController controller = + public static CommandXboxController controller = new CommandXboxController(OperatorConstants.CONTROLLER_NUMBER); + public static CommandJoystick joystick = new CommandJoystick(OperatorConstants.JOYSTICK_NUMBER); + + private final SendableChooser autoChooser; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // Configure the trigger bindings configureBindings(); - // Verify auto tasks. If each task isnt verified the autotask will throw a exception. - verifyAutoTasks(); + + autoChooser = new SendableChooser<>(); + autoChooser.setDefaultOption("Fling cube + mobility bonus", + Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); + autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); + + SmartDashboard.putData("Choose auto", autoChooser); + + compilationArm.setDefaultCommand(manualArm); + driveSubsystem.setDefaultCommand(driveRO); + } + + public Pose containerGetArmPose() { + + return compilationArm.getArmPose(); } /** @@ -60,7 +138,96 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - controller.povUp().onTrue(balance); + + // controller.y().whileTrue(deployPlunger); + + controller.rightBumper().whileTrue(balance); + + controller.y().toggleOnTrue(driveFO); + + controller.b().whileTrue(deployPlunger); + + joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase, + armShoulder, armExtender, compilationArm)); + + joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); + + joystick.povUp().whileTrue(extend); + joystick.povDown().whileTrue(retract); + + joystick.button(11).whileTrue(openClaw); + /* TODO: get better buttons with Gabriel */ + /* Or just get color sensor working */ + joystick.trigger().whileTrue(closeClaw); + //joystick.button(2).whileTrue(closeClawCube); + + /*joystick + .button(2) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.RESET, armShoulder, armBase, armExtender, compilationArm)); + joystick + .button(3) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.PICKUP, armShoulder, armBase, armExtender, compilationArm)); + joystick + .button(11) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.SCORE_LOWER, + armShoulder, + armBase, + armExtender, + compilationArm)); + joystick + .button(7) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.SCORE_HIGH_CONE, + armShoulder, + armBase, + armExtender, + compilationArm)); + joystick + .button(9) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.SCORE_MID_CONE, + armShoulder, + armBase, + armExtender, + compilationArm)); + joystick + .button(8) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.SCORE_HIGH_CUBE, + armShoulder, + armBase, + armExtender, + compilationArm)); + joystick + .button(10) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.SCORE_MID_CUBE, + armShoulder, + armBase, + armExtender, + compilationArm)); + joystick + .button(12) + .onTrue( + new MoveToPose( + Constants.Arm.Poses.HUMAN_SHELF, + armShoulder, + armBase, + armExtender, + compilationArm)); + joystick + .button(1) + .onTrue(armClaw.manualCloseClawCommand());*/ } private void verifyAutoTasks() { @@ -74,6 +241,72 @@ private void verifyAutoTasks() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + return autoChooser.getSelected(); + } + + public Command getResetCommand() { + return MoveToPose.retractingMoveToPose( + Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm) + .alongWith(armClaw.openClawCommand()); + } + + public RetractArm getRetractCommand() { + return retractArmCommand; + } + + public Command getDisplayAimVideoCommand() { + return displayAimVideo; + } + + private static double scaleAxis(double a) { + return Math.signum(a) * Math.pow(a, 2); + } + + public static double getControllerLeftXAxis() { + return controller.getLeftX(); + } + + public static double getScaledControllerLeftXAxis() { + return scaleAxis(getControllerLeftXAxis()); + } + + public static double getControllerLeftYAxis() { + return controller.getLeftY() * -1.0; + } + + public static double getScaledControllerLeftYAxis() { + return scaleAxis(getControllerLeftYAxis()); + } + + public static double getControllerRightXAxis() { + return controller.getRightX(); + } + + public static double getScaledControllerRightXAxis() { + return scaleAxis(getControllerRightXAxis()); + } + + public static double getControllerRightYAxis() { + return controller.getRightY() * -1.0; + } + + public static double getScaledControllerRightYAxis() { + return scaleAxis(getControllerRightYAxis()); + } + + public static double getJoystickXAxis() { + return joystick.getX()* -1.0; + } + + public static double getScaledJoystickXAxis() { + return scaleAxis(getJoystickXAxis()); + } + + public static double getJoystickYAxis() { + return joystick.getY() * -1.0; + } + + public static double getScaledJoystickYAxis() { + return scaleAxis(getJoystickYAxis()); } } diff --git a/src/main/java/frc/robot/TCA9548A.java b/src/main/java/frc/robot/TCA9548A.java new file mode 100644 index 0000000..20a22c9 --- /dev/null +++ b/src/main/java/frc/robot/TCA9548A.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.I2C.Port; + +/** Add your docs here. */ +public class TCA9548A { + int portNum; + I2C multiplexer; + + public TCA9548A(int portNum) { + if (portNum > 7 || portNum < 0) return; + this.portNum = 0x70 + portNum; + multiplexer = new I2C(Port.kMXP, 0x70 + portNum); + } + + public TCA9548A() { + this(0); /* Default, this is the port this year's bot uses */ + } + + public void setBus(int busNumber) { + if (busNumber >= 8 || busNumber < 0) return; + multiplexer.write(portNum, 1 << busNumber); + } +} diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 3ebb407..5b2cbdd 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -6,12 +6,31 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.Constants; +import frc.robot.commands.homingRoutine.HomeBase; +import frc.robot.commands.homingRoutine.HomeShoulder; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.ArmClaw; +import frc.robot.subsystems.arm.ArmExtender; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; public final class Autos { - /** Example static factory for an autonomous command. */ - public static CommandBase exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); + public static CommandBase homingRoutine( + ArmShoulder armShoulderSubsystem, + ArmBase armBaseSubsystem, + ArmExtender armExtenderSubsystem, + ArmClaw armClawSubsystem, + CompilationArm compilationArmSubsystem) { + return Commands.sequence( + armExtenderSubsystem.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), + new HomeShoulder(armShoulderSubsystem, compilationArmSubsystem), + new HomeBase(armBaseSubsystem, compilationArmSubsystem), + armClawSubsystem.openClawCommand()).withTimeout(5.5); + } + + public static CommandBase homingNoOpenClaw(ArmShoulder armShoulder, ArmBase armBase, ArmExtender armExtender, CompilationArm compilationArm) { + return Commands.sequence(armExtender.setCommand(Constants.Arm.MIN_LENGTH_ARM_EXTENDER), new HomeShoulder(armShoulder, compilationArm), new HomeBase(armBase, compilationArm)); } private Autos() { diff --git a/src/main/java/frc/robot/commands/Balance.java b/src/main/java/frc/robot/commands/Balance.java index d910776..c34b0e0 100644 --- a/src/main/java/frc/robot/commands/Balance.java +++ b/src/main/java/frc/robot/commands/Balance.java @@ -5,16 +5,17 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; import frc.robot.Constants.BalanceConstants; import frc.robot.subsystems.Drive; public class Balance extends CommandBase { - /** Creates a new BalanceAuto. */ - boolean BalanceXMode; + private boolean BalanceXMode; + private boolean BalanceYMode; - boolean BalanceYMode; private final Drive drive; + /** Creates a new BalanceAuto */ public Balance(Drive drive) { this.drive = drive; addRequirements(drive); @@ -40,10 +41,10 @@ public void execute() { } if (!BalanceYMode - && (Math.abs(pitchAngleDegrees) >= Math.abs(BalanceConstants.OFF_ANGLE_THRESHOLD))) { + && (Math.abs(rollAngleDegrees) >= Math.abs(BalanceConstants.OFF_ANGLE_THRESHOLD))) { BalanceYMode = true; } else if (BalanceYMode - && (Math.abs(pitchAngleDegrees) <= Math.abs(BalanceConstants.ON_ANGLE_THRESHOLD))) { + && (Math.abs(rollAngleDegrees) <= Math.abs(BalanceConstants.ON_ANGLE_THRESHOLD))) { BalanceYMode = false; } if (BalanceXMode) { @@ -54,7 +55,7 @@ public void execute() { double rollAngleRadians = rollAngleDegrees * (Math.PI / 180.0); yAxisRate = Math.sin(rollAngleRadians) * -1; } - drive.moveMecanumRobot(yAxisRate, xAxisRate, 0); + drive.moveMecanumRobot((-xAxisRate * Constants.BalanceConstants.SPEED_MULTIPLIER), (-yAxisRate * Constants.BalanceConstants.SPEED_MULTIPLIER), 0); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/CloseClawCone.java b/src/main/java/frc/robot/commands/CloseClawCone.java new file mode 100644 index 0000000..303d1af --- /dev/null +++ b/src/main/java/frc/robot/commands/CloseClawCone.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmClaw; + +public class CloseClawCone extends CommandBase { + private ArmClaw armClaw; + + /** Creates a new CloseClawCone. */ + public CloseClawCone(ArmClaw armClaw) { + // Use addRequirements() here to declare subsystem dependencies. + this.armClaw = armClaw; + + addRequirements(armClaw); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armClaw.set(Constants.Arm.SETPOINT_PRESSURE_CONE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armClaw.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return armClaw.isAtSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/CloseClawCube.java b/src/main/java/frc/robot/commands/CloseClawCube.java new file mode 100644 index 0000000..479697b --- /dev/null +++ b/src/main/java/frc/robot/commands/CloseClawCube.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmClaw; + +public class CloseClawCube extends CommandBase { + private ArmClaw armClaw; + + /** Creates a new CloseClawCube. */ + public CloseClawCube(ArmClaw armClaw) { + // Use addRequirements() here to declare subsystem dependencies. + this.armClaw = armClaw; + + addRequirements(armClaw); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armClaw.set(Constants.Arm.SETPOINT_PRESSURE_CUBE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armClaw.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return armClaw.isAtSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/DeployPlunger.java b/src/main/java/frc/robot/commands/DeployPlunger.java new file mode 100644 index 0000000..7be933a --- /dev/null +++ b/src/main/java/frc/robot/commands/DeployPlunger.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Plunger; + +/** Command to deploy Plunger */ +public class DeployPlunger extends CommandBase { + private Plunger plunger; + /** + * Creates instance of DeployPlunger + * + * @param plunger takes plunger subsystem for dependency injection + */ + public DeployPlunger(Plunger plunger) { + this.plunger = plunger; + addRequirements(plunger); + } + + /** Called when the command starts/button is presse - starts the motor */ + @Override + public void initialize() { + plunger.deployPlunger(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + /** called when the command end/button is realeased - stops the motor from spinning */ + @Override + public void end(boolean interrupted) { + plunger.stop(); + } + + /** + * Returns true when the command should end. Will never return true due to because it's designed + * to be used with whenHeld + */ + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveFieldOriented.java b/src/main/java/frc/robot/commands/DriveFieldOriented.java new file mode 100644 index 0000000..e218851 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveFieldOriented.java @@ -0,0 +1,44 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Drive; + +public class DriveFieldOriented extends CommandBase { + + /* Variables */ + private final Drive drivetrain; + private double mecanumX; + private double mecanumY; + private double mecanumZ; + + public DriveFieldOriented(Drive driveSubsystem) { + drivetrain = driveSubsystem; + addRequirements(driveSubsystem); + } + + @Override + public void initialize() { + SmartDashboard.putString("Drive Perspective", "Field"); + } + + @Override + public void execute() { + /* Gets the left and right axes of the robot and uses that to move */ + mecanumX = RobotContainer.getScaledControllerLeftXAxis(); + mecanumY = RobotContainer.getScaledControllerLeftYAxis(); + mecanumZ = RobotContainer.getScaledControllerRightXAxis(); + drivetrain.moveMecanumField(mecanumY, mecanumX, mecanumZ); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveForwards.java b/src/main/java/frc/robot/commands/DriveForwards.java new file mode 100644 index 0000000..04ae803 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveForwards.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drive; + +/** Leaves the community for the */ +public class DriveForwards extends CommandBase { + + private final Drive drivetrain; + + /** Creates a new LeaveCommunity. */ + public DriveForwards(Drive driveSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.drivetrain = driveSubsystem; + addRequirements(driveSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drivetrain.moveSimple(0.5, 0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveReverse.java b/src/main/java/frc/robot/commands/DriveReverse.java new file mode 100644 index 0000000..ec2929c --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveReverse.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drive; + +/** Leaves the community for the */ +public class DriveReverse extends CommandBase { + + private final Drive drivetrain; + + /** Creates a new LeaveCommunity. */ + public DriveReverse(Drive driveSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.drivetrain = driveSubsystem; + addRequirements(driveSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drivetrain.moveSimple(-0.5, -0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveRobotOriented.java b/src/main/java/frc/robot/commands/DriveRobotOriented.java new file mode 100644 index 0000000..0f8f85d --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveRobotOriented.java @@ -0,0 +1,45 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Drive; + +public class DriveRobotOriented extends CommandBase { + + /* Variables */ + private final Drive drivetrain; + private double mecanumX; + private double mecanumY; + private double mecanumZ; + + public DriveRobotOriented(Drive driveSubsystem) { + drivetrain = driveSubsystem; + addRequirements(driveSubsystem); + } + + @Override + public void initialize() { + SmartDashboard.putString("Drive Perspective", "Robot"); + } + + @Override + public void execute() { + /* Gets the left and right axes of the robot and uses that to move */ + mecanumX = RobotContainer.getScaledControllerLeftXAxis(); + mecanumY = RobotContainer.getScaledControllerLeftYAxis(); + mecanumZ = RobotContainer.getScaledControllerRightXAxis(); + /*mecanumZ = RobotContainer.getScaledRightXAxis();*/ + drivetrain.moveMecanumRobot(mecanumY, mecanumX, mecanumZ); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ExtendArm.java b/src/main/java/frc/robot/commands/ExtendArm.java new file mode 100644 index 0000000..53b795d --- /dev/null +++ b/src/main/java/frc/robot/commands/ExtendArm.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.arm.ArmExtender; + +public class ExtendArm extends CommandBase { + private ArmExtender armExtender; + /** Creates a new RetractArm. */ + public ExtendArm(ArmExtender armExtender) { + // Use addRequirements() here to declare subsystem dependencies. + this.armExtender = armExtender; + addRequirements(armExtender); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.armExtender.extend(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.armExtender.setArmSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ManualArm.java b/src/main/java/frc/robot/commands/ManualArm.java new file mode 100644 index 0000000..ce5dc0f --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualArm.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; + +public class ManualArm extends CommandBase { + /** Creates a new ManualArm. */ + private final ArmBase armBaseMove; + + private final ArmShoulder armShoulderMove; + + private double armBaseRotation; + private double armShoulderRotation; + + public ManualArm( + ArmBase armBaseSubsystem, ArmShoulder armShoulderSubsystem, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.armBaseMove = armBaseSubsystem; + this.armShoulderMove = armShoulderSubsystem; + + addRequirements(armBaseSubsystem, armShoulderSubsystem, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + armBaseRotation = RobotContainer.getScaledJoystickXAxis(); + armShoulderRotation = RobotContainer.getScaledJoystickYAxis(); + + armBaseMove.manualMoveArmBase(armBaseRotation); + armShoulderMove.manualMoveArmShoulder(armShoulderRotation); + } + + // 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/frc/robot/commands/MoveBaseDegrees.java b/src/main/java/frc/robot/commands/MoveBaseDegrees.java new file mode 100644 index 0000000..2c21078 --- /dev/null +++ b/src/main/java/frc/robot/commands/MoveBaseDegrees.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.CompilationArm; + +public class MoveBaseDegrees extends CommandBase { + private int degrees; + private ArmBase armBase; + /** Creates a new MoveBaseDegrees. */ + public MoveBaseDegrees(int degrees, ArmBase armBase, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.degrees = degrees; + this.armBase = armBase; + + addRequirements(armBase, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armBase.moveBase(degrees); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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 armBase.isBaseAtSetpoint(); + if (armBase.isBaseAtSetpoint()) { + System.out.println("Base is supposedly at setpoint"); + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/MoveShoulderDegrees.java b/src/main/java/frc/robot/commands/MoveShoulderDegrees.java new file mode 100644 index 0000000..25fa760 --- /dev/null +++ b/src/main/java/frc/robot/commands/MoveShoulderDegrees.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; + +public class MoveShoulderDegrees extends CommandBase { + private int degrees; + private ArmShoulder armShoulder; + /** Creates a new MoveBaseDegrees. */ + public MoveShoulderDegrees(int degrees, ArmShoulder armShoulder, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.degrees = degrees; + this.armShoulder = armShoulder; + + addRequirements(armShoulder, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armShoulder.moveShoulder(degrees); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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 armShoulder.isBaseAtSetpoint(); + if (armShoulder.isShoulderAtSetpoint()) { + System.out.println("Shoulder is supposedly at setpoint"); + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/RetractArm.java b/src/main/java/frc/robot/commands/RetractArm.java new file mode 100644 index 0000000..093b883 --- /dev/null +++ b/src/main/java/frc/robot/commands/RetractArm.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.arm.ArmExtender; + +public class RetractArm extends CommandBase { + private ArmExtender armExtender; + /** Creates a new RetractArm. */ + public RetractArm(ArmExtender armExtender) { + // Use addRequirements() here to declare subsystem dependencies. + this.armExtender = armExtender; + addRequirements(armExtender); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.armExtender.retract(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.armExtender.setArmSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/StartLeavingCommunity.java b/src/main/java/frc/robot/commands/StartLeavingCommunity.java new file mode 100644 index 0000000..3024bbd --- /dev/null +++ b/src/main/java/frc/robot/commands/StartLeavingCommunity.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drive; + +/** Leaves the community for the */ +public class StartLeavingCommunity extends CommandBase { + + private final Drive drivetrain; + + /** Creates a new LeaveCommunity. */ + public StartLeavingCommunity(Drive driveSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.drivetrain = driveSubsystem; + addRequirements(driveSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drivetrain.moveMecanumRobot(0.5, 0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/StopLeavingCommunity.java b/src/main/java/frc/robot/commands/StopLeavingCommunity.java new file mode 100644 index 0000000..b2887c7 --- /dev/null +++ b/src/main/java/frc/robot/commands/StopLeavingCommunity.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drive; + +/** Leaves the community for the */ +public class StopLeavingCommunity extends CommandBase { + + private final Drive drivetrain; + + /** Creates a new LeaveCommunity. */ + public StopLeavingCommunity(Drive driveSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.drivetrain = driveSubsystem; + addRequirements(driveSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drivetrain.stop(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java b/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java new file mode 100644 index 0000000..c6139c9 --- /dev/null +++ b/src/main/java/frc/robot/commands/homingRoutine/HomeBase.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.homingRoutine; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.CompilationArm; + +public class HomeBase extends CommandBase { + private final ArmBase armBase; + /** Creates a new HomeShoulder. */ + public HomeBase(ArmBase armShoulder, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.armBase = armShoulder; + + addRequirements(armShoulder, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armBase.manualMoveArmBase(-1 * Constants.Arm.SPEED_ARM_BASE_HOMING); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armBase.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (armBase.baseLimitSwitch()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java b/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java new file mode 100644 index 0000000..1dd48a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/homingRoutine/HomeShoulder.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.homingRoutine; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; + +public class HomeShoulder extends CommandBase { + private final ArmShoulder armShoulder; + /** Creates a new HomeShoulder. */ + public HomeShoulder(ArmShoulder armShoulder, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.armShoulder = armShoulder; + + addRequirements(armShoulder, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armShoulder.manualMoveArmShoulder(-1 * Constants.Arm.SPEED_ARM_SHOULDER_HOMING); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armShoulder.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (armShoulder.shoulderLimitSwitch()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java new file mode 100644 index 0000000..adfd1b1 --- /dev/null +++ b/src/main/java/frc/robot/commands/moveToPose/MoveBaseToPose.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.moveToPose; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.positioning.ArmKinematics; +import frc.robot.positioning.Pose; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.CompilationArm; + +public class MoveBaseToPose extends CommandBase { + private ArmBase armBase; + private Pose pose; + + /** Creates a new moveBaseToPose. */ + public MoveBaseToPose(Pose pose, ArmBase armBase, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.armBase = armBase; + this.pose = pose; + + addRequirements(armBase, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armBase.moveBase((int) (0.5 + ArmKinematics.getBaseRotation(pose))); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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 armBase.isBaseAtSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java new file mode 100644 index 0000000..8ab0cea --- /dev/null +++ b/src/main/java/frc/robot/commands/moveToPose/MoveExtenderToPose.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.moveToPose; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.positioning.ArmKinematics; +import frc.robot.positioning.Pose; +import frc.robot.subsystems.arm.ArmExtender; +import frc.robot.subsystems.arm.CompilationArm; + +public class MoveExtenderToPose extends CommandBase { + private ArmExtender armExtender; + private Pose pose; + + /** Creates a new moveBaseToPose. */ + public MoveExtenderToPose(Pose pose, ArmExtender armExtender, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.armExtender = armExtender; + this.pose = pose; + + addRequirements(armExtender, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println("Arm extension:" + ArmKinematics.getArmExtension(pose)); + armExtender.set(ArmKinematics.getArmExtension(pose)); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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 armExtender.isExtenderAtSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java new file mode 100644 index 0000000..288a495 --- /dev/null +++ b/src/main/java/frc/robot/commands/moveToPose/MoveShoulderToPose.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.moveToPose; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.positioning.ArmKinematics; +import frc.robot.positioning.Pose; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; + +public class MoveShoulderToPose extends CommandBase { + private ArmShoulder armShoulder; + private Pose pose; + + /** Creates a new moveBaseToPose. */ + public MoveShoulderToPose(Pose pose, ArmShoulder armShoulder, CompilationArm compilationArm) { + // Use addRequirements() here to declare subsystem dependencies. + this.armShoulder = armShoulder; + this.pose = pose; + + addRequirements(armShoulder, compilationArm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println(ArmKinematics.getShoulderRotation(pose)); + armShoulder.moveShoulder((int) (0.5 + ArmKinematics.getShoulderRotation(pose))); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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 armShoulder.isShoulderAtSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/moveToPose/MoveToPose.java b/src/main/java/frc/robot/commands/moveToPose/MoveToPose.java new file mode 100644 index 0000000..35cd243 --- /dev/null +++ b/src/main/java/frc/robot/commands/moveToPose/MoveToPose.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.moveToPose; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.positioning.Pose; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.ArmExtender; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; + +/** Add your docs here. */ +public final class MoveToPose { + public MoveToPose() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + public static Command extendingMoveToPose( + Pose pose, + ArmBase armBase, + ArmShoulder armShoulder, + ArmExtender armExtender, + CompilationArm compilationArm) { + return Commands.sequence( + new MoveBaseToPose(pose, armBase, compilationArm), + new MoveShoulderToPose(pose, armShoulder, compilationArm), + new MoveExtenderToPose(pose, armExtender, compilationArm)); + } + + public static Command retractingMoveToPose( + Pose pose, + ArmBase armBase, + ArmShoulder armShoulder, + ArmExtender armExtender, + CompilationArm compilationArm) { + return Commands.sequence( + new MoveExtenderToPose(pose, armExtender, compilationArm), + new MoveShoulderToPose(pose, armShoulder, compilationArm), + new MoveBaseToPose(pose, armBase, compilationArm)); + } +} diff --git a/src/main/java/frc/robot/positioning/ArmKinematics.java b/src/main/java/frc/robot/positioning/ArmKinematics.java new file mode 100644 index 0000000..69dc48b --- /dev/null +++ b/src/main/java/frc/robot/positioning/ArmKinematics.java @@ -0,0 +1,363 @@ +package frc.robot.positioning; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.numbers.N3; +import frc.robot.Constants; +import frc.robot.Constants.Arm; +import frc.robot.Constants.RobotDimensions; +import java.util.Optional; + +/** + * Kinematics class contains forward and inverse kinematics functions for the 2023 robot arm. All + * positions are measured relative to the base of the arm. Don't reuse, the math won't work for any + * other arm. + * + *

If you want to implement this for a different design, these links describe how. + * https://motion.cs.illinois.edu/RoboticSystems/Kinematics.html + * https://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html + */ +public final class ArmKinematics { + /** + * Returns whether or not the arm is in danger of overextending. + * + * @param baseRotation counter clockwise rotation of the arm base zeroed on the Y axis in degrees + * @param shoulderRotation counter clockwise rotation of the shoulder about the X axis zeroed on + * the Z axis in degrees + * @param armExtension distance in inches from the shoulder joint to the end of the grabber arm + */ + public static boolean isAlmostOverextended(final Pose armPose) { + final double horizontalExt = getFrameExtension(armPose); + final double verticalExt = getExtendedRobotHeight(armPose); + + final double dangerZone = Constants.Limits.OVEREXTENSION_DANGER_DISTANCE; + final double horizontal = horizontalExt + dangerZone; + final double vertical = verticalExt + dangerZone; + + final double horizontalMax = Constants.Limits.MAX_FRAME_EXTENSION; + final double verticalMax = Constants.Limits.MAX_EXTENDED_HEIGHT; + + return horizontal > horizontalMax || vertical > verticalMax; + } + + /** + * Returns whether or not the arm is overextended. If this is the case, the robot MUST immediately + * retract the arm to avoid penalties. + * + * @param armPose the pose of the end effector + */ + public static boolean isOverextended(final Pose armPose) { + + final double horizontal = getFrameExtension(armPose); + final double vertical = getExtendedRobotHeight(armPose); + + final double horizontalMax = Constants.Limits.MAX_FRAME_EXTENSION; + final double verticalMax = Constants.Limits.MAX_EXTENDED_HEIGHT; + + return horizontal > horizontalMax || vertical > verticalMax; + } + + /** + * Returns whether the robot is stabbing itself. Keep in mind, this isn't 100% accurate, there + * could be a last minute fix sticking up. How will you handle that? That sounds like a you + * problem, not a me problem. Why are you hitting yourself? Why are you hitting yourself? + * + * @param armPose the end effector pose being checked + */ + public static boolean isStabbingSelf(final Pose armPose) { + final Optional frameIntersection = getFramePlaneInstersection(armPose); + + if (frameIntersection.isPresent()) { + // Outside the frame + return frameIntersection.get().getZ() < Constants.Arm.KEEP_OUT_HEIGHT; + } else { + // Inside the frame + return armPose.getZ() < Constants.Arm.KEEP_OUT_HEIGHT; + } + } + + /** + * @param x right/left offset from the arm base + * @param y front/back offset from the arm base + * @param z up/down offset from the arm base + * @return arm base rotation + */ + public static double getArmExtension(final double x, final double y, final double z) { + final double h = Arm.BASE_TO_SHOULDER_LENGTH; + + return Math.sqrt(x * x + y * y + z * z + h * h - 2 * h * z); + } + + /** + * Gets the arm extension to achieve a given end effector pose based at the bottom of the arm. + * This doesn't take pose orientation into account. + * + * @param target the desired pose + */ + public static double getArmExtension(final Pose target) { + return getArmExtension(target.getX(), target.getY(), target.getZ()); + } + + /** + * @param x right/left offset from the arm base + * @param y front/back offset from the arm base + * @param z up/down offset from the arm base + * @return arm base rotation + */ + public static double getBaseRotation(final double x, final double y, final double z) { + return Math.atan2(x, y) * 360 / (2 * Math.PI); + } + + /** + * Gets the arm base rotation to achieve a given end effector pose based at the bottom of the arm. + * This doesn't take pose orientation into account. + * + * @param target the desired pose + */ + public static double getBaseRotation(final Pose target) { + return getBaseRotation(target.getX(), target.getY(), target.getZ()); + } + + /** + * Calculates the height of the robot in inches for the purpose of not overextending + * + * @param armPose the end effector pose being checked + */ + public static double getExtendedRobotHeight(final Pose armPose) { + final double height = armPose.getWorldOriented(Constants.Arm.BASE_POSE).getZ(); + return height + Constants.Arm.BASE_DISTANCE_TO_FLOOR; + } + + /** + * Calculates the height of the robot in inches for the purpose of not overextending. This math + * only works with vaguely rectangular robots. + * + * @param armPose the end effector pose being checked + */ + public static double getFrameExtension(final Pose armPose) { + final Pose robotPose = armPose.getWorldOriented(Arm.BASE_POSE); + final Pose centerPose = robotPose.getWorldOriented(RobotDimensions.CENTER_POSE); + final Optional frameIntersection = getFramePlaneInstersection(armPose); + + if (frameIntersection.isPresent()) { + // Outside the frame + final double xDiff = centerPose.getX() - frameIntersection.get().getX(); + final double yDiff = centerPose.getY() - frameIntersection.get().getY(); + + return Math.sqrt(xDiff * xDiff + yDiff * yDiff); + } else { + // Inside the frame + return 0.0; + } + } + + /** + * Finds the point at which the arm intersects the plane of the frame + * + * @param armPose the pose of the end effector + */ + public static Optional getFramePlaneInstersection(final Pose armPose) { + // Get end effector pose with respect to the robot center + final Pose robotPose = armPose.getWorldOriented(Arm.BASE_POSE); + final Pose centerPose = robotPose.getWorldOriented(RobotDimensions.CENTER_POSE); + + final double x = centerPose.getX(); + final double y = centerPose.getY(); + final double z = centerPose.getZ(); + final double xFrame = RobotDimensions.FRAME_LENGTH / 2; + final double yFrame = RobotDimensions.FRAME_WIDTH / 2; + + // Skip the math if the frame isn't extended + if (x < xFrame && x > -xFrame && y < yFrame && y > -yFrame) { + return Optional.empty(); + } + + // Figure out which intersection we care about + boolean isLeft = false; + boolean isFront = false; + boolean isRight = false; + boolean isBack = false; + final boolean isForward = centerPose.getX() > 0; + + if (isForward) { + if (y / x > yFrame / xFrame) { + isLeft = true; + } else if (y / x > -yFrame / xFrame) { + isFront = true; + } else { + isRight = true; + } + } else { + if (y / x > yFrame / xFrame) { + isRight = true; + } else if (y / x > -yFrame / xFrame) { + isBack = true; + } else { + isLeft = true; + } + } + + // Calculate frame plane intersection + double xIntercept = 0.0; + double yIntercept = 0.0; + + if (isLeft) { + yIntercept = yFrame; + xIntercept = yFrame * x / y; + } + + if (isFront) { + xIntercept = xFrame; + yIntercept = xFrame * y / x; + } + + if (isRight) { + yIntercept = -yFrame; + xIntercept = yFrame * x / y; + } + + if (isBack) { + xIntercept = -xFrame; + yIntercept = xFrame * y / x; + } + + final double zIntercept = xFrame * z / x; + + return Optional.of(new Pose(xIntercept, yIntercept, zIntercept)); + } + + /** + * Gets the orientation of the end effector. Keep in mind that Rotation3d uses radians instead of + * degrees. + * + * @param baseRotation counter clockwise rotation of the arm base zeroed on the Y axis in degrees + * @param shoulderRotation counter clockwise rotation of the shoulder about the X axis zeroed on + * the Z axis in degrees + * @param armExtension distance in inches from the shoulder joint to the end of the grabber arm + */ + public static Rotation3d getOrientation( + final double baseRotation, final double shoulderRotation, final double armExtension) { + // Since this is a simple enough arm design and the roll will always be 0, + // we can just treat the arm extension as an angle/axis orientation. + final double sinBase = Math.sin(Math.toRadians(baseRotation)); + final double cosBase = Math.cos(Math.toRadians(baseRotation)); + final double sinShoulder = Math.sin(Math.toRadians(shoulderRotation)); + final double cosShoulder = Math.cos(Math.toRadians(shoulderRotation)); + + final double x = armExtension * sinBase * sinShoulder; + final double y = -1 * armExtension * cosBase * sinShoulder; + final double shoulderBasedZ = armExtension * cosShoulder; + + final Vector vector = VecBuilder.fill(x, y, shoulderBasedZ); + return new Rotation3d(vector, 0.0); + } + + /** + * Gets the full pose of the end effector + * + * @param baseRotation counter clockwise rotation of the arm base zeroed on the Y axis in degrees + * @param shoulderRotation counter clockwise rotation of the shoulder about the X axis zeroed on + * the Z axis in degrees + * @param armExtension distance in inches from the shoulder joint to the end of the grabber arm + */ + public static Pose getPose( + final double baseRotation, final double shoulderRotation, final double armExtension) { + final double sinBase = Math.sin(Math.toRadians(baseRotation)); + final double cosBase = Math.cos(Math.toRadians(baseRotation)); + final double sinShoulder = Math.sin(Math.toRadians(shoulderRotation)); + final double cosShoulder = Math.cos(Math.toRadians(shoulderRotation)); + + final double x = armExtension * sinBase * sinShoulder; + final double y = -1 * armExtension * cosBase * sinShoulder; + final double shoulderBasedZ = armExtension * cosShoulder; + final double z = armExtension * cosShoulder + Arm.BASE_TO_SHOULDER_LENGTH; + + final Vector vector = VecBuilder.fill(x, y, shoulderBasedZ); + final Rotation3d orientation = new Rotation3d(vector, 0.0); + + return new Pose(x, y, z, orientation); + } + + /** + * @param x right/left offset from the arm base + * @param y front/back offset from the arm base + * @param z up/down offset from the arm base + * @return arm base rotation + */ + public static double getShoulderRotation(final double x, final double y, final double z) { + final double distance = Math.sqrt(x * x + y * y); + final double heightDiff = z - Arm.BASE_TO_SHOULDER_LENGTH; + + return Math.atan2(distance, heightDiff) * 360 / (2 * Math.PI); + } + + /** + * Gets the shoulder rotation to achieve a given end effector pose based at the bottom of the arm. + * This doesn't take pose orientation into account. + * + * @param target the desired pose + */ + public static double getShoulderRotation(final Pose target) { + return getShoulderRotation(target.getX(), target.getY(), target.getZ()); + } + + /** + * @param baseRotation counter clockwise rotation of the arm base zeroed on the Y axis in degrees + * @param shoulderRotation counter clockwise rotation of the shoulder about the X axis zeroed on + * the Z axis in degrees + * @param armExtension distance in inches from the shoulder joint to the end of the grabber arm + * @return X position relative to the arm base. + */ + public static double getX( + final double baseRotation, final double shoulderRotation, final double armExtension) { + final double sinBase = Math.sin(Math.toRadians(baseRotation)); + final double sinShoulder = Math.sin(Math.toRadians(shoulderRotation)); + + return armExtension * sinBase * sinShoulder; + } + + /** + * @param baseRotation counter clockwise rotation of the arm base zeroed on the Y axis in degrees + * @param shoulderRotation counter clockwise rotation of the shoulder about the X axis zeroed on + * the Z axis in degrees + * @param armExtension distance in inches from the shoulder joint to the end of the grabber arm + * @return Y position relative to the arm base. + */ + public static double getY( + final double baseRotation, final double shoulderRotation, final double armExtension) { + final double cosBase = Math.cos(Math.toRadians(baseRotation)); + final double sinShoulder = Math.sin(Math.toRadians(shoulderRotation)); + + return -1 * armExtension * cosBase * sinShoulder; + } + + /** + * @param baseRotation counter clockwise rotation of the arm base zeroed on the Y axis in degrees + * @param shoulderRotation counter clockwise rotation of the shoulder about the X axis zeroed on + * the Z axis in degrees + * @param armExtension distance in inches from the shoulder joint to the end of the grabber arm + * @return Z(up/down) position relative to the arm base. + */ + public static double getZ( + final double baseRotation, final double shoulderRotation, final double armExtension) { + final double cosShoulder = Math.cos(Math.toRadians(shoulderRotation)); + return armExtension * cosShoulder + Arm.BASE_TO_SHOULDER_LENGTH; + } + + public static double getReferenceAngle(double number) { + boolean done = false; + while (!done) { + if (number < 0) { + number += 360; + } + if (number > 360) { + number -= 360; + } + if (number >= 0 && number <= 360) { + done = true; + } + } + return number; + } +} diff --git a/src/main/java/frc/robot/positioning/Pose.java b/src/main/java/frc/robot/positioning/Pose.java index 4332856..52259e9 100644 --- a/src/main/java/frc/robot/positioning/Pose.java +++ b/src/main/java/frc/robot/positioning/Pose.java @@ -35,6 +35,18 @@ public Pose3d getPose3d() { return new Pose3d(this.getTranslation(), this.getRotation()); } + /** If this pose is in inches, get a pose in feet. Do not use if this pose is in inches. */ + public Pose getFeetFromInches() { + final Translation3d newPosition = this.getTranslation().times(1 / 12); + return new Pose(new Pose3d(newPosition, getRotation())); + } + + /** If this pose is in feet, get a pose in inches. Do not use if this pose is in feet. */ + public Pose getInchesFromFeet() { + final Translation3d newPosition = this.getTranslation().times(12); + return new Pose(new Pose3d(newPosition, getRotation())); + } + /** * Returns this pose, but in world based coordinates This is very useful if you have a pose where * (0,0,0) is on the robot. If you know where that (0,0,0) point is oriented in the world, then diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java new file mode 100644 index 0000000..413a017 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Camera extends SubsystemBase { + private int port; + private UsbCamera camera; + /** Creates a new Camera. */ + public Camera(int port) { + this.port = port; + } + + public void startCamera() { + camera = CameraServer.startAutomaticCapture(this.port); + + camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index cd77210..79b9328 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -154,6 +154,13 @@ public void moveTank(double left, double right) { drivetrain.tankDrive(left, right); } + public void moveSimple(double leftSpeed, double rightSpeed) { + frontLeft.set(ControlMode.PercentOutput, leftSpeed); + rearLeft.set(ControlMode.PercentOutput, leftSpeed); + frontRight.set(ControlMode.PercentOutput, rightSpeed); + rearRight.set(ControlMode.PercentOutput, rightSpeed); + } + /** * Stop. * diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 3057963..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public CommandBase exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/I2CManager.java b/src/main/java/frc/robot/subsystems/I2CManager.java new file mode 100644 index 0000000..0ce5561 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/I2CManager.java @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.Rev2mDistanceSensor; +import com.revrobotics.Rev2mDistanceSensor.RangeProfile; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.TCA9548A; +// import edu.wpi.first.wpilibj.Notifier; +// import com.revrobotics.ColorSensorV3; + +public class I2CManager extends SubsystemBase { + private TCA9548A multiplexer; + private Rev2mDistanceSensor dist; /* This is a color sensor, but we only use it for distance */ + // private ColorSensorV3 color; + private boolean currentRangeValid; + private double currentRange; + private Color currentColor; + // private Notifier thread; + + /** Creates a new I2CManager. */ + public I2CManager() { + multiplexer = new TCA9548A(); + multiplexer.setBus(Constants.I2C.DISTANCE_SENSOR_MULTIPLEXER_PORT); + dist = new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kMXP); + dist.setAutomaticMode(true); + dist.setRangeProfile(RangeProfile.kLongRange); + /*multiplexer.setBus(Constants.I2C.COLOR_SENSOR_MULTIPLEXER_PORT); + color = new ColorSensorV3(I2C.Port.kMXP);*/ + /* TODO: color/distance sensors MAY need additional config */ + + /* We run stuff that needs to be done periodically in a thread instead of this.periodic() + * because I2C calls take a hot minute and can cause loop overruns + */ + /*thread = new Notifier(() -> { + multiplexer.setBus(Constants.I2C.DISTANCE_SENSOR_MULTIPLEXER_PORT); + currentRangeValid = dist.isRangeValid(); + currentRange = dist.getRange(); + multiplexer.setBus(Constants.I2C.COLOR_SENSOR_MULTIPLEXER_PORT); + currentColor = color.getColor(); + }); + + thread.startPeriodic(0.02);*/ + /* Period @ 20ms, the default loop time for command-based */ + } + + public boolean isCurrentRangeValid() { + return this.currentRangeValid; + } + + public double getCurrentRange() { + return this.currentRange; + } + + public Color getCurrentColor() { + return this.currentColor; + } + + @Override + public void periodic() { + /* This subsystem does stuff periodically, but it does it in a thread because I2C calls can + * take a while and will cause loop overruns (see the constructor of this class) + */ + /*multiplexer.setBus(Constants.I2C.DISTANCE_SENSOR_MULTIPLEXER_PORT);*/ + currentRangeValid = dist.isRangeValid(); + currentRange = dist.getRange(); + /*multiplexer.setBus(Constants.I2C.COLOR_SENSOR_MULTIPLEXER_PORT); + currentColor = color.getColor();*/ + } +} diff --git a/src/main/java/frc/robot/subsystems/Plunger.java b/src/main/java/frc/robot/subsystems/Plunger.java new file mode 100644 index 0000000..b3ccb48 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Plunger.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Plunger extends SubsystemBase { + private Spark plunger; + /** Creates a new Plunger. */ + public Plunger() { + this.plunger = new Spark(Constants.Plunger.ID_PWMMOTOR_PLUNGER); + } + + public void deployPlunger() { + plunger.set(Constants.Plunger.SPEED_PLUNGER_DEPLOY); + } + + public void stop() { + plunger.set(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmBase.java b/src/main/java/frc/robot/subsystems/arm/ArmBase.java new file mode 100644 index 0000000..68f35c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmBase.java @@ -0,0 +1,116 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.SensorCollection; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.positioning.ArmKinematics; + +/** + * Subsystem that represents the base of the arm, which (if mechanical implements it) will be able + * to rotate to a given setpoint. + */ +public class ArmBase extends SubsystemBase { + + private WPI_TalonSRX armBaseMotor; + private SensorCollection sensorCollection; + + /** Creates a new ArmBase. Should be called once from {@link frc.robot.RobotContainer}. */ + public ArmBase() { + armBaseMotor = configTalon(Constants.Arm.ID_TALON_ARM_BASE); + sensorCollection = armBaseMotor.getSensorCollection(); + } + + /* TODO: Consider moving this method to a static class. It's not super useful here or in the other arm classes. */ + /** + * Configures the Talon SRX for this class with values supplied in {@link + * frc.robot.Constants.Arm}. + * + * @param id The CAN ID of the Talon SRX + * @return The newly constructed Talon SRX, configured and ready for PID + */ + private WPI_TalonSRX configTalon(int id) { + /* Comment the stuff in this method that's commented out back in + * when we have PID tuned and PID values are set in constants. + */ + WPI_TalonSRX talon = new WPI_TalonSRX(id); + // talon.configFactoryDefault(); + talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + talon.configClearPositionOnLimitR(Constants.Arm.AUTO_ZERO_REVERSE_LIMIT_BASE, 0); + talon.setSensorPhase(Constants.Arm.INVERTED_TALON_SENSOR_ARM_BASE); + talon.setInverted(Constants.Arm.INVERTED_TALON_ARM_BASE); + talon.config_kP(0, Constants.Arm.BasePID.kP); + talon.config_kI(0, Constants.Arm.BasePID.kI); + talon.config_kD(0, Constants.Arm.BasePID.kD); + talon.config_kF(0, Constants.Arm.BasePID.kFF); + talon.config_IntegralZone(0, Constants.Arm.BasePID.kIZone); + talon.configAllowableClosedloopError(0, Constants.Arm.BasePID.ACCEPTABLE_ERROR); + + return talon; + } + /** Checks if either base limit switch is closed. */ + public boolean baseLimitSwitch() { + return sensorCollection.isFwdLimitSwitchClosed() || sensorCollection.isRevLimitSwitchClosed(); + } + + public void manualMoveArmBase(double x) { + armBaseMotor.set(ControlMode.PercentOutput, x); + } + /** Returns the angle of the base CCW from forward in degrees */ + public double getAngle() { + /* TODO: account for difference between limit switch position and front of bot */ + return ((armBaseMotor.getSelectedSensorPosition() / 8192) * 360) / 3; + } + + /** + * Moves the base to a given setpoint. + * + * @param degrees The setpoint to move to in degrees. + */ + public void moveBase(double degrees) { + /* Takes the degree param and converts it to encoder ticks + * so the talon knows what we're talking about + */ + /* TODO: account for difference between limit switch position and front of bot + * doing so will require some funky coding + */ + degrees = ArmKinematics.getReferenceAngle(degrees); + degrees = (degrees + 60) % 360; + System.out.println("Final degrees: " + degrees); + degrees = degrees * 3; + degrees = (degrees / 360) * 8192; + System.out.println("Final encoder ticks: " + degrees); + armBaseMotor.set(ControlMode.Position, degrees); + } + + public Command moveBaseCommand(int degrees) { + return this.runOnce(() -> this.moveBase(degrees)); + } + + /** Checks if the base motor is at the setpoint. */ + public boolean isBaseAtSetpoint() { + /* For some unknown reason, WPI_TalonSRX.getClosedLoopError() will return the error from the + * last setpoint briefly after a new setpoint is set, so we have to calculate error manually + */ + return Math.abs(armBaseMotor.getSelectedSensorPosition() - armBaseMotor.getClosedLoopTarget()) + <= Constants.Arm.BasePID.ACCEPTABLE_ERROR; + } + + /** Stops the base from moving. */ + public void stop() { + armBaseMotor.stopMotor(); + } + + /** Subsystem periodic; called every scheduler run. Not used in this subsystem. */ + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmClaw.java b/src/main/java/frc/robot/subsystems/arm/ArmClaw.java new file mode 100644 index 0000000..9bbdd56 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmClaw.java @@ -0,0 +1,108 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +/** + * Subsystem that represents the claw on the arm. Can open the claw or close it to a given pressure. + */ +public class ArmClaw extends SubsystemBase { + + private Talon clawMotor; + private Double setpoint; + + private AnalogInput pressure; + + private boolean isAtSetpoint; + + /** Creates a new ArmClaw. Should be called once from {@link frc.robot.RobotContainer}. */ + public ArmClaw() { + clawMotor = new Talon(Constants.Arm.ID_TALON_ARM_CLAW); + pressure = new AnalogInput(Constants.Arm.CHANNEL_ANALOG_PRESSURE_SENSOR); + /* We set the setpoint to null when we want to disable the code that automagically moves the claw to the setpoint */ + setpoint = null; + } + + /** Opens the claw. */ + public void openClaw() { + /* We set the setpoint to null when we want to disable the code that automagically moves the claw to the setpoint */ + setpoint = null; + clawMotor.set(Constants.Arm.SPEED_ARM_CLAW); + } + + public void manualCloseClaw() { + setpoint = null; + clawMotor.set(-1 * Constants.Arm.SPEED_ARM_CLAW); + } + + public void stop() { + clawMotor.set(0); + } + + /** + * Command factory to open the claw + * + * @return a command that opens the claw + */ + public Command openClawCommand() { + return this.runEnd(() -> this.openClaw(), () -> this.stop()); + } + + public Command manualOpenClawCommand() { + return this.runEnd(() -> this.openClaw(), () -> this.stop()); + } + + public Command manualCloseClawCommand() { + return this.runEnd(() -> this.manualCloseClaw(), () -> this.stop()); + } + + /** + * Sets the pressure-based setpoint for the claw. + * + * @param setpoint How much pressure we want the claw to apply to whatever it's clamping onto. + * Units are in volts of resistance, least resistance is 5v, most is 0. Least resistence = + * most pressure. + */ + public void set(Double setpoint) { + this.setpoint = setpoint; + } + + public boolean isAtSetpoint() { + /* HOPEFULLY THIS WORKS */ + return isAtSetpoint; + } + + /** + * Subsystem periodic; called once per scheduler run. + * + *

Moves the claw to the current setpoint. + */ + @Override + public void periodic() { + /* Allows us to have a way to make this code not run, so we can do things like open the claw. */ + if (setpoint != null) { + /* Adds a tolerance so we don't vibrate back and forth constantly and destroy the entire mechanism */ + if (Math.abs(setpoint - pressure.getVoltage()) >= Constants.Arm.DONE_THRESHOLD_ARM_CLAW) { + if (pressure.getVoltage() > setpoint) { + clawMotor.set(-1 * Constants.Arm.SPEED_ARM_CLAW); + } else { + clawMotor.set(Constants.Arm.SPEED_ARM_CLAW); + } + } else { + clawMotor.set(0); + isAtSetpoint = true; + } + } + + SmartDashboard.putNumber("Pressure Reading", pressure.getVoltage()); + SmartDashboard.putBoolean("At cone pressure", pressure.getVoltage() >= Constants.Arm.CONE_PRESSURE_THRESHOLD); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtender.java b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java new file mode 100644 index 0000000..573d640 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java @@ -0,0 +1,101 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.I2CManager; + +/** This subsystem represents the climber in a box that extends the arm on the robot. */ +public class ArmExtender extends SubsystemBase { + + private Talon winch; + private boolean extenderAtSetpoint; + + private I2CManager I2CManager; + + private Double setpoint; + + /** Creates a new ArmExtender. Should be called once from {@link frc.robot.RobotContainer}. */ + public ArmExtender(I2CManager I2CManager) { + winch = new Talon(Constants.Arm.ID_TALON_ARM_WINCH); + setpoint = null; + this.I2CManager = I2CManager; + extenderAtSetpoint = false; + } + + public void extend() { + setpoint = null; + winch.set(1); + } + + public void retract() { + winch.set(-1); + } + + public Command extendCommand() { + return this.runOnce(() -> this.extend()); + } + + public Command retractCommand() { + return this.runOnce(() -> this.retract()); + } + + /** + * If the length sensor has a valid reading, this method will return the length of the arm from + * the shoulder to the claw. + * + * @return The length of the arm from the shoulder to the claw in inches. + */ + public double getLength() { + return I2CManager.getCurrentRange() + Constants.Arm.EXTRA_LENGTH_ARM_EXTENDER; + } + + /** + * Sets the setpoint for the arm extension. + * + * @param setpoint How far we want the arm to extend in inches from the shoulder to the claw. + */ + public void set(double setpoint) { + setpoint -= Constants.Arm.EXTRA_LENGTH_ARM_EXTENDER; + this.setpoint = setpoint; + } + + public Command setCommand(double setpoint) { + return this.runOnce(() -> this.set(setpoint)); + } + + public boolean isExtenderAtSetpoint() { + return extenderAtSetpoint; + } + + /** + * Directs arm towards setpoint. Since this is the periodic method, this is called every time the + * scheduler runs. + */ + @Override + public void periodic() { + if (setpoint != null && I2CManager.isCurrentRangeValid()) { + /* Adds a tolerance so we don't vibrate back and forth constantly and destroy the entire mechanism */ + if (Math.abs(setpoint - getLength()) >= Constants.Arm.DONE_THRESHOLD_ARM_EXTENSION) { + if (getLength() > setpoint) { + winch.set(-1 * Constants.Arm.SPEED_WINCH_ARM_EXTENSION); + } else { + winch.set(Constants.Arm.SPEED_WINCH_ARM_EXTENSION); + } + } else { + winch.stopMotor(); + extenderAtSetpoint = true; + } + } + } + + public void setArmSpeed(double speed) { + setpoint = null; + winch.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java b/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java new file mode 100644 index 0000000..bf00754 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmShoulder.java @@ -0,0 +1,113 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.SensorCollection; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.positioning.ArmKinematics; + +/** + * Subsystem that represents the "shoulder" of the arm; that is, the motor which will rotate the arm + * extender. Can rotate to a given setpoint. + */ +public class ArmShoulder extends SubsystemBase { + + private WPI_TalonSRX armShoulderMotor; + private SensorCollection sensorCollection; + + /** Creates a new ArmShoulder. Should be called once from {@link frc.robot.RobotContainer}. */ + public ArmShoulder() { + armShoulderMotor = configTalon(Constants.Arm.ID_TALON_ARM_SHOULDER); + sensorCollection = armShoulderMotor.getSensorCollection(); + } + + /* TODO: Consider moving this method to a static class. It's not super useful here or in the other arm classes. */ + /** + * Configures the Talon SRX for this class with values supplied in {@link + * frc.robot.Constants.Arm}. + * + * @param id The CAN ID of the Talon SRX + * @return The newly constructed Talon SRX, configured and ready for PID + */ + private WPI_TalonSRX configTalon(int id) { + /* Comment the stuff in this method that's commented out back in + * when we have PID tuned and PID values are set in constants. + */ + WPI_TalonSRX talon = new WPI_TalonSRX(id); + // talon.configFactoryDefault(); + talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + talon.configClearPositionOnLimitR(Constants.Arm.AUTO_ZERO_REVERSE_LIMIT_SHOULDER, 0); + talon.setSensorPhase(Constants.Arm.INVERTED_TALON_SENSOR_ARM_SHOULDER); + talon.setInverted(Constants.Arm.INVERTED_TALON_ARM_SHOULDER); + talon.config_kP(0, Constants.Arm.ShoulderPID.kP); + talon.config_kI(0, Constants.Arm.ShoulderPID.kI); + talon.config_kD(0, Constants.Arm.ShoulderPID.kD); + talon.config_kF(0, Constants.Arm.ShoulderPID.kFF); + talon.config_IntegralZone(0, Constants.Arm.ShoulderPID.kIZone); + talon.configAllowableClosedloopError(0, Constants.Arm.ShoulderPID.ACCEPTABLE_ERROR); + + return talon; + } + + public void manualMoveArmShoulder(double x) { + armShoulderMotor.set(ControlMode.PercentOutput, x); + } + + /** Returns the angle of the shoulder in degrees */ + public double getAngle() { + return (armShoulderMotor.getSelectedSensorPosition() / 8192) * 360; + } + + /** + * Moves the shoulder to a given setpoint. + * + * @param degrees The setpoint to move to in degrees. + */ + public void moveShoulder(double degrees) { + /* Takes the degree param and converts it to encoder ticks + * so the talon knows what we're talking about + */ + degrees = ArmKinematics.getReferenceAngle(degrees); + if (degrees <= 10 && degrees >= 0) { + DriverStation.reportWarning( + "Number of degrees passed too small, defaulting to 10 degrees", true); + } + degrees -= 10; + degrees = (degrees / 360) * 8192; + armShoulderMotor.set(ControlMode.Position, degrees); + } + + public boolean isShoulderAtSetpoint() { + return Math.abs( + armShoulderMotor.getSelectedSensorPosition() - armShoulderMotor.getClosedLoopTarget()) + <= Constants.Arm.ShoulderPID.ACCEPTABLE_ERROR; + } + + /** + * Checks if the reverse shoulder limit switch is closed + * + * @return True if the switch is closed + */ + public boolean shoulderLimitSwitch() { + return sensorCollection.isRevLimitSwitchClosed(); + } + + /** Stops the shoulder from moving. */ + public void stop() { + armShoulderMotor.stopMotor(); + } + + /** Subsystem periodic; called every scheduler run. Not used in this subsystem. */ + @Override + public void periodic() { + // This method will be called once per scheduler run + + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/CompilationArm.java b/src/main/java/frc/robot/subsystems/arm/CompilationArm.java new file mode 100644 index 0000000..95dc01c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/CompilationArm.java @@ -0,0 +1,102 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.positioning.ArmKinematics; +import frc.robot.positioning.Pose; + +@SuppressWarnings("unused") +public class CompilationArm extends SubsystemBase { + private ArmBase armBase; + private ArmClaw armClaw; + private ArmExtender armExtender; + private Pose armPose; + private ArmShoulder armShoulder; + + /** Creates a new CompilationArm. */ + public CompilationArm( + ArmBase armBase, ArmClaw armClaw, ArmExtender armExtender, ArmShoulder armShoulder) { + this.armBase = armBase; + this.armClaw = armClaw; + this.armExtender = armExtender; + this.armShoulder = armShoulder; + + updateArmPose(); + } + + /** Returns whether or not the arm is in danger of overextending. */ + public boolean isAlmostOverextended() { + return ArmKinematics.isAlmostOverextended(getArmPose()); + } + + /** + * Returns whether or not the arm is overextended. If this is the case, the robot MUST immediately + * retract the arm to avoid penalties. + */ + public boolean isOverextended() { + return ArmKinematics.isOverextended(getArmPose()); + } + + /** + * Returns whether the robot is stabbing itself. Keep in mind, this only tests whether or not the + * end effector is stabbing the frame. If anything is built up above KEEP_OUT_HEIGHT, this will + * happily run into it. + */ + public boolean isStabbingSelf() { + return ArmKinematics.isStabbingSelf(getArmPose()); + } + + public boolean isArmAtSetpoint() { + return armShoulder.isShoulderAtSetpoint() + && armExtender.isExtenderAtSetpoint() + && armBase.isBaseAtSetpoint(); + } + + /** + * getter method of the position of the arm + * + * @return Current posiiton of the arm + */ + public Pose getArmPose() { + return armPose; + } + + /** + * Moves arm to a given pose + * + * @param pose the position that the arm is to go to + * @deprecated This method has the potential to decapitate the RSL/Limelight tower. Use the + * commands in {@link frc.robot.commands.moveToPose} instead. + */ + @Deprecated + public void moveToPose(Pose pose) { + armBase.moveBase((int) (0.5 + ArmKinematics.getBaseRotation(pose))); + armShoulder.moveShoulder((int) (0.5 + ArmKinematics.getShoulderRotation(pose))); + armExtender.set(ArmKinematics.getArmExtension(pose)); + } + + /** + * Command factory that returns command that moves the arm to a given pose + * + * @param pose the position that the arm is to go to + * @return command that moves arm to a pose + */ + @Override + public void periodic() { + // This method will be called once per scheduler run + // Make sure arm update runs before you use arm pose values + updateArmPose(); + } + + /** Updates the arm pose based on sensor values */ + private void updateArmPose() { + final double baseRotation = armBase.getAngle(); + final double shoulderRotation = armShoulder.getAngle(); + final double armExtension = armExtender.getLength(); + + this.armPose = ArmKinematics.getPose(baseRotation, shoulderRotation, armExtension); + } +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/subsystems/yourMom.java similarity index 60% rename from src/main/java/frc/robot/commands/ExampleCommand.java rename to src/main/java/frc/robot/subsystems/yourMom.java index 5150ec7..ca9fbbd 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/subsystems/yourMom.java @@ -2,27 +2,29 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands; +package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.PrintCommand; -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; +public class yourMom extends CommandBase { + /** Creates a new yourMom. */ + public yourMom() { + // Use addRequirements() here to declare subsystem dependencies. + } + /** prints "Your Mom" */ + public void printYourMom() { + System.out.print("Your Mom"); + } /** - * Creates a new ExampleCommand. + * command to print "Your Mom" * - * @param subsystem The subsystem used by this command. + * @return command that prints "Your Mom" */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); + public PrintCommand commandPrintYourMom() { + return new PrintCommand("Your Mom"); } - // Called when the command is initially scheduled. @Override public void initialize() {} diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 0000000..ecf0636 --- /dev/null +++ b/vendordeps/REV2mDistanceSensor.json @@ -0,0 +1,55 @@ +{ + "fileName": "REV2mDistanceSensor.json", + "name": "REV2mDistanceSensor", + "version": "0.4.0", + "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", + "mavenUrls": [ + "https://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "https://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-java", + "version": "0.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-cpp", + "version": "0.4.0", + "libName": "libDistanceSensor", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "libName": "libDistanceSensorDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..c2ddf7d --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2023.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2023.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2023.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file