diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 00000000..c73712d6 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1,68 @@ +{ + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "NT3@10.1.92.206:59486": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "NT3@10.1.92.206:59984": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "NT3@10.1.92.206:61719": { + "Publishers": { + "open": true + }, + "open": true + }, + "NT3@10.1.92.206:65507": { + "Publishers": { + "open": true + }, + "open": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "192" + }, + "Server": { + "open": true + }, + "outlineviewer@5": { + "open": true + }, + "retained": { + "elevator": { + "open": true + } + }, + "shuffleboard@2": { + "Subscribers": { + "open": true + } + }, + "transitory": { + "Shuffleboard": { + "Auton": { + "Field": { + "open": true + }, + "open": true + } + } + } +} diff --git a/.gradle 8.30.16 PM/7.5.1/checksums/checksums.lock b/.gradle 8.30.16 PM/7.5.1/checksums/checksums.lock new file mode 100644 index 00000000..b9e64952 Binary files /dev/null and b/.gradle 8.30.16 PM/7.5.1/checksums/checksums.lock differ diff --git a/.gradle 8.30.16 PM/7.5.1/dependencies-accessors/dependencies-accessors.lock b/.gradle 8.30.16 PM/7.5.1/dependencies-accessors/dependencies-accessors.lock new file mode 100644 index 00000000..0dc23c9c Binary files /dev/null and b/.gradle 8.30.16 PM/7.5.1/dependencies-accessors/dependencies-accessors.lock differ diff --git a/.gradle 8.30.16 PM/7.5.1/dependencies-accessors/gc.properties b/.gradle 8.30.16 PM/7.5.1/dependencies-accessors/gc.properties new file mode 100644 index 00000000..e69de29b diff --git a/.gradle 8.30.16 PM/7.5.1/fileChanges/last-build.bin b/.gradle 8.30.16 PM/7.5.1/fileChanges/last-build.bin new file mode 100644 index 00000000..f76dd238 Binary files /dev/null and b/.gradle 8.30.16 PM/7.5.1/fileChanges/last-build.bin differ diff --git a/.gradle 8.30.16 PM/7.5.1/fileHashes/fileHashes.lock b/.gradle 8.30.16 PM/7.5.1/fileHashes/fileHashes.lock new file mode 100644 index 00000000..1aee5289 Binary files /dev/null and b/.gradle 8.30.16 PM/7.5.1/fileHashes/fileHashes.lock differ diff --git a/.gradle 8.30.16 PM/7.5.1/gc.properties b/.gradle 8.30.16 PM/7.5.1/gc.properties new file mode 100644 index 00000000..e69de29b diff --git a/.gradle 8.30.16 PM/buildOutputCleanup/buildOutputCleanup.lock b/.gradle 8.30.16 PM/buildOutputCleanup/buildOutputCleanup.lock new file mode 100644 index 00000000..4cf5a0dd Binary files /dev/null and b/.gradle 8.30.16 PM/buildOutputCleanup/buildOutputCleanup.lock differ diff --git a/.gradle 8.30.16 PM/buildOutputCleanup/cache.properties b/.gradle 8.30.16 PM/buildOutputCleanup/cache.properties new file mode 100644 index 00000000..fd80a7b0 --- /dev/null +++ b/.gradle 8.30.16 PM/buildOutputCleanup/cache.properties @@ -0,0 +1,2 @@ +#Tue Jan 16 20:29:04 PST 2024 +gradle.version=7.5.1 diff --git a/.gradle 8.30.16 PM/vcs-1/gc.properties b/.gradle 8.30.16 PM/vcs-1/gc.properties new file mode 100644 index 00000000..e69de29b diff --git a/build.gradle b/build.gradle index 5b29be23..c07a40e3 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } java { diff --git a/simgui.json b/simgui.json index 9cd23685..7ea1ef9f 100644 --- a/simgui.json +++ b/simgui.json @@ -15,7 +15,8 @@ }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/Shuffleboard/Auton/Field": "Field2d" } }, "NetworkTables": { diff --git a/src/main/deploy/choreo/2MStraightLine.traj b/src/main/deploy/choreo/2MStraightLine.traj new file mode 100644 index 00000000..9ed6657f --- /dev/null +++ b/src/main/deploy/choreo/2MStraightLine.traj @@ -0,0 +1,58 @@ +{ + "samples": [ + { + "x": 0, + "y": 0, + "heading": 0, + "angularVelocity": -7.55803462394642e-23, + "velocityX": 4.80718084308354, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 0.399999999995696, + "y": 0, + "heading": -7.497604164638229e-24, + "angularVelocity": -9.193245718395418e-23, + "velocityX": 4.904634275090461, + "velocityY": 0, + "timestamp": 0.08155552026115727 + }, + { + "x": 0.7999999999949925, + "y": 0, + "heading": -5.539197561099716e-24, + "angularVelocity": 2.4013165429964444e-23, + "velocityX": 4.904634275134157, + "velocityY": 0, + "timestamp": 0.16311104052231454 + }, + { + "x": 1.1999999999967819, + "y": 0, + "heading": -3.709865909310565e-24, + "angularVelocity": 2.2430502175244085e-23, + "velocityX": 4.904634275164754, + "velocityY": 0, + "timestamp": 0.24466656078347182 + }, + { + "x": 1.599999999998375, + "y": 0, + "heading": -1.853623907916435e-24, + "angularVelocity": 2.276046566478452e-23, + "velocityX": 4.9046342751623415, + "velocityY": 0, + "timestamp": 0.3262220810446291 + }, + { + "x": 2, + "y": 0, + "heading": 7.346839692639297e-40, + "angularVelocity": 2.2728363693887657e-23, + "velocityX": 4.904634275162733, + "velocityY": 0, + "timestamp": 0.40777760130578633 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2X1MCurve.traj b/src/main/deploy/choreo/2X1MCurve.traj new file mode 100644 index 00000000..ffdda751 --- /dev/null +++ b/src/main/deploy/choreo/2X1MCurve.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 0, + "y": 0, + "heading": 1.050069725977771e-40, + "angularVelocity": 1.5398262290553578e-16, + "velocityX": 2.794710467944005, + "velocityY": 3.5303599919188637, + "timestamp": 0 + }, + { + "x": 0.23173749164767796, + "y": 0.29282094581619517, + "heading": 1.3440021243249235e-17, + "angularVelocity": 1.7652368051626978e-16, + "velocityX": 3.0436823126628485, + "velocityY": 3.8459634962062363, + "timestamp": 0.07613721401873688 + }, + { + "x": 0.49239298101355006, + "y": 0.5602252989065818, + "heading": -3.323421049691745e-18, + "angularVelocity": -2.2017409632498635e-16, + "velocityX": 3.423496547973215, + "velocityY": 3.5121373501399438, + "timestamp": 0.15227442803747376 + }, + { + "x": 0.7919875221555976, + "y": 0.7831366780255028, + "heading": 5.996654462713521e-18, + "angularVelocity": 1.224115648587833e-16, + "velocityX": 3.934929127683013, + "velocityY": 2.927758547388317, + "timestamp": 0.22841164205621065 + }, + { + "x": 1.1317406170413713, + "y": 0.9381018452824517, + "heading": 3.317621643457253e-17, + "angularVelocity": 3.5698130436912057e-16, + "velocityX": 4.462378867750975, + "velocityY": 2.0353406576828874, + "timestamp": 0.3045488560749475 + }, + { + "x": 1.5, + "y": 1, + "heading": -3.674138992979092e-38, + "angularVelocity": -4.357424534191873e-16, + "velocityX": 4.836785633740269, + "velocityY": 0.8129816084561599, + "timestamp": 0.3806860700936844 + }, + { + "x": 1.8687850971430546, + "y": 0.9472063385941725, + "heading": 2.0043532320076904e-17, + "angularVelocity": 2.6387750181910064e-16, + "velocityX": 4.855136739459322, + "velocityY": -0.6950401387793096, + "timestamp": 0.45664378600020644 + }, + { + "x": 2.210735226102118, + "y": 0.799355927973932, + "heading": -2.8324657515278217e-18, + "angularVelocity": -3.0116753508593537e-16, + "velocityX": 4.501848493976605, + "velocityY": -1.9464831038464674, + "timestamp": 0.5326015019067285 + }, + { + "x": 2.5122510777106877, + "y": 0.5805458963497601, + "heading": 1.603040244881554e-17, + "angularVelocity": 2.4833379961114195e-16, + "velocityX": 3.9695223587330672, + "velocityY": -2.8806820875516777, + "timestamp": 0.6085592178132506 + }, + { + "x": 2.7724292850840286, + "y": 0.31390580296723725, + "heading": 2.4391259489329175e-17, + "angularVelocity": 1.1007251786346634e-16, + "velocityX": 3.425303200180388, + "velocityY": -3.510375347660468, + "timestamp": 0.6845169337197727 + }, + { + "x": 3, + "y": 0.018946131691336632, + "heading": 0, + "angularVelocity": -3.2111628420777353e-16, + "velocityX": 2.996018405734816, + "velocityY": -3.8832088058149257, + "timestamp": 0.7604746496262949 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 82fb65db..3cf12d98 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,6 +16,24 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public static class ElevatorConstants { + public static final int EXTENSION_ID = 0; + public static final int EXTENSION_FOLLOW_ID = 0; + + public static final float EXTENSION_LIMIT_METERS = 0; + + public static final int ZERO_LIMIT_ID = 0; + + public static final int GROUND_POSITION = 0; + public static final int SPEAKER_POSITION = 0; + public static final int AMP_POSITION = 0; + public static final int CHUTE_POSITION = 0; + public static final int TRAP_POSITION = 0; + public static final int START_POSITION = 0; + + public static final double POSITIONCONVERSIONFACTOR = 0; + public static final double VELOCITYCONVERSIONFACTOR = 0; + } public static class OperatorConstants { public static final int kDriverControllerPort = 0; } @@ -39,6 +57,7 @@ public static class SwerveConstants { public static final int BR_DRIVE = 6; public static final int BR_STEER = 7; + public static final double BR_OFFSET = 2.43 + Math.PI * 3.0 / 4.0; private static double MODULE_DIST = Units.inchesToMeters(27.25 / 2.0); public static final Translation2d FL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST); @@ -46,4 +65,42 @@ public static class SwerveConstants { public static final Translation2d BL_POS = new Translation2d(-MODULE_DIST, -MODULE_DIST); public static final Translation2d BR_POS = new Translation2d(MODULE_DIST, -MODULE_DIST); } + + public static class RollerandPivotConstants { + public static final int lastmotorID = 0; + public static final int topmotorID = 2; + public static final int bottommotorID = 3; + public static final int motor1ID = 1; + + public static final int sensorID = 0; + public static final int extendedlimitswitchID = 1; + public static final int retractedlimitswitchID = 2; + //public static final int intakeencoderID = 3; + + public static final double rollersclockwise = 1; + public static final double rollerscounterclockwise = -1; + public static final double sensorreached = 1; + public static final double pivotclockwise = 1; + public static final double pivotcounterclockwise = -1; + public static final double pastsensortime = 0.5; + } + + + + + public static class ClimbConstants { + public static final int LEFT_WINCH_MOTOR_ID = 21; + public static final int LEFT_ZERO_LIMIT_ID = 11; + + public static final int RIGHT_WINCH_MOTOR_ID = 22; + public static final int RIGHT_ZERO_LIMIT_ID = 12; + + public static final double WINCH_REDUCTION = 9.49; + public static final double AXLE_PERIMETER_METERS = 6 * Units.inchesToMeters(.289) ; + + public static final double EXTENSION_LIMIT_METERS = Units.inchesToMeters(39); + public static final double EXTENSION_TOLERANCE_METERS = 0.01; + + public static final double MAX_WINCH_POWER = 0.6; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc6f3647..481cdc2b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,105 +4,215 @@ package frc.robot; +import frc.robot.Constants.OperatorConstants; +import frc.robot.subsystems.shooter.ShooterFeederSubsystem; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; +import frc.robot.subsystems.intake.IntakePivotSubsystem; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.controllers.BaseDriveController; +import frc.robot.controllers.DualJoystickDriveController; +import frc.robot.controllers.XboxDriveController; +import frc.robot.commands.climb.ClimbLowerCommand; +import frc.robot.commands.climb.ClimbRaiseCommand; +import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.swerve.BaseSwerveSubsystem; import frc.robot.subsystems.swerve.SingleModuleSwerveSubsystem; import frc.robot.subsystems.swerve.SwerveModule; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.TestSingleModuleSwerveSubsystem; +import java.util.function.BooleanSupplier; +import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.XboxController; 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.RunCommand; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import static frc.robot.Constants.SwerveConstants.*; public class RobotContainer { - private final BaseSwerveSubsystem baseSwerveSubsystem; + // The robot's subsystems and commands are defined here... + private final IntakeRollersSubsystem intakeSubsystem = new IntakeRollersSubsystem(); + private final BaseDriveController driveController = new DualJoystickDriveController(); + private final BaseSwerveSubsystem baseSwerveSubsystem; + + private final IntakePivotSubsystem intakePivotSubsystem; + private final ShooterFlywheelSubsystem shooterSubsystem; + private final ShooterFeederSubsystem feederSubsystem; + private final ShooterPivotSubsystem shooterPivotSubsystem; + + private final ClimbSubsystem climbSubsystem; + + + private final XboxController mechController = new XboxController(2); + private final JoystickButton aButton = new JoystickButton(mechController, XboxController.Button.kA.value); + private final JoystickButton bButton = new JoystickButton(mechController, XboxController.Button.kB.value); + private final JoystickButton leftBumper = new JoystickButton(mechController, XboxController.Button.kLeftBumper.value); + private final JoystickButton rightBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value); + private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); + private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value); + //private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); + + ChoreoTrajectory traj; + // private final SwerveModule module; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + //construct Test + // module = new SwerveModule(6, 7, 0); + // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); + baseSwerveSubsystem = new SwerveSubsystem(); + intakePivotSubsystem = new IntakePivotSubsystem(); + feederSubsystem = new ShooterFeederSubsystem(); + + shooterPivotSubsystem = new ShooterPivotSubsystem(false); + shooterSubsystem = new ShooterFlywheelSubsystem(); + + climbSubsystem = new ClimbSubsystem(); - private final XboxController controller = new XboxController(0); - // private final SwerveModule module; - - private final JoystickButton - LBumper = new JoystickButton(controller, XboxController.Button.kLeftBumper.value), - RBumper = new JoystickButton(controller, XboxController.Button.kRightBumper.value), - AButton = new JoystickButton(controller, XboxController.Button.kA.value); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - //construct Test - // module = new SwerveModule(0, 1, 0); - // baseSwerveSubsystem = new SingleModuleSwerveSubsystem(module); - baseSwerveSubsystem = new SwerveSubsystem(); + traj = Choreo.getTrajectory("Curve"); + // Configure the trigger bindings - configureBindings(); + configureBindings(); } - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ - private void configureBindings() { - if(baseSwerveSubsystem instanceof SwerveSubsystem){ - final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; - swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - swerveSubsystem.setDrivePowers(controller.getLeftX(), -controller.getLeftY(), -controller.getRightX());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); - } - , swerveSubsystem)); + private void configureBindings() { - AButton.onTrue(new InstantCommand(() -> { - swerveSubsystem.resetDriverHeading(); - } - )); - - } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){ - final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; - LBumper.onTrue(new InstantCommand(() -> { - testSwerveSubsystem.decrementTest(); - System.out.println(testSwerveSubsystem.getTest()); - } - )); - - RBumper.onTrue(new InstantCommand(() -> { - testSwerveSubsystem.incrementTest(); - System.out.println(testSwerveSubsystem.getTest()); - } - )); - - AButton.onTrue(new InstantCommand(() -> { - testSwerveSubsystem.toggletoRun(); - System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running"); - })); - - } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ - final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; - - System.out.println("1"); - - swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - swerveSubsystem.setDrivePowers(controller.getLeftX(), -controller.getLeftY());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); - } - , swerveSubsystem)); - - AButton.onTrue(new InstantCommand(() -> { - swerveSubsystem.toggletoRun(); - })); - + /** + * Right trigger - run pivot forward at speed pulled + * Left trigger - run pivot backward at speed pulled + * A button - run flywheels while button held + * B button - run feed wheels while button held + */ + + // aButton.onTrue(new LoadNoteCommand(feederSubsystem)); + + // bButton.onTrue(new ShootNoteCommand(feederSubsystem)); + + // leftBumper.onTrue(new AutoAimCommand(pivotSubsystem)); + + // rightBumper.onTrue(new VerticalCommand(pivotSubsystem)); + + // xButton.onTrue(new ReadyShooterCommand(shooterSubsystem)); + + // yButton.onTrue(new StopShooterCommands(shooterSubsystem)); + + intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> { + intakePivotSubsystem.setPivotSpeed(-mechController.getLeftTriggerAxis()); + })); + + shooterSubsystem.setDefaultCommand(new InstantCommand(() -> { + intakePivotSubsystem.setPivotSpeed(mechController.getRightTriggerAxis()); + })); + + bButton.onTrue(new InstantCommand(() -> { + feederSubsystem.setFeederMotorSpeed(.7); + })); + + bButton.onFalse(new InstantCommand(() -> { + feederSubsystem.setFeederMotorSpeed(0); + })); + + xButton.onTrue(new InstantCommand(() -> { + feederSubsystem.setFeederMotorSpeed(-.7); + })); + + xButton.onFalse(new InstantCommand(() -> { + feederSubsystem.setFeederMotorSpeed(0); + })); + + shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { + shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); + // pivotSubsystem.printCurrentAngle(); + }, shooterPivotSubsystem)); + + if(baseSwerveSubsystem instanceof SwerveSubsystem){ + final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; + + swerveSubsystem.setDefaultCommand(new RunCommand(() -> { + // swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); + // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); + } + , swerveSubsystem)); + + driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { + swerveSubsystem.resetDriverHeading(); + } + )); + + } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){ + final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; + // LBumper.onTrue(new InstantCommand(() -> { + // testSwerveSubsystem.decrementTest(); + // System.out.println(testSwerveSubsystem.getTest()); + // } + // )); + + // RBumper.onTrue(new InstantCommand(() -> { + // testSwerveSubsystem.incrementTest(); + // System.out.println(testSwerveSubsystem.getTest()); + // } + // )); + + // AButton.onTrue(new InstantCommand(() -> { + // testSwerveSubsystem.toggletoRun(); + // System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running"); + // })); + + } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ + final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; + + // System.out.println("1"); + + // swerveSubsystem.setDefaultCommand(new RunCommand(() -> { + // swerveSubsystem.setDrivePowers(controller.getLeftX(), -controller.getLeftY());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); + // } + // , swerveSubsystem)); + + // AButton.onTrue(new InstantCommand(() -> { + // swerveSubsystem.toggletoRun(); + // })); + + } + + } - } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ public Command getAutonomousCommand() { - return new InstantCommand(); + if(baseSwerveSubsystem instanceof SwerveSubsystem){ + + final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; + PIDController thetacontroller = new PIDController(1, 0, 0); //TODO: tune + thetacontroller.enableContinuousInput(-Math.PI, Math.PI); + + BooleanSupplier isBlue = () -> true; //DriverStation.getAlliance() == new Optional ; + + Command swerveCommand = Choreo.choreoSwerveCommand( + traj, + swerveSubsystem::getRobotPosition, + new PIDController(1, 0, 0), //X TODO: tune + new PIDController(1, 0, 0), //Y TODO: tune + thetacontroller, + ((ChassisSpeeds speeds) -> swerveSubsystem.setDrivePowers( + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond + )), + isBlue, + swerveSubsystem + ); + + return Commands.sequence( + Commands.runOnce(() -> swerveSubsystem.resetPose(traj.getInitialPose())), + swerveCommand + ); + } + else return null; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java b/src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java new file mode 100644 index 00000000..57845eae --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.climb; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climb.ClimbSubsystem; + +public class ClimbLowerCommand extends Command { + private ClimbSubsystem climbSubsystem; + + public ClimbLowerCommand(ClimbSubsystem climbSubsystem){ + this.climbSubsystem = climbSubsystem; + this.addRequirements(climbSubsystem); + } + + @Override + public void initialize() { + System.out.println("LOWERING CLIMB..."); + climbSubsystem.goToExtension(0); + } + + @Override + public boolean isFinished() { + return climbSubsystem.isAtExtension(); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + System.out.println(interrupted ? "CLIMB LOWERING INTERRUPTED!" : "CLIMB LOWERED!"); + } + +} diff --git a/src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java b/src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java new file mode 100644 index 00000000..eea65a93 --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java @@ -0,0 +1,34 @@ +package frc.robot.commands.climb; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climb.ClimbSubsystem; +import static frc.robot.Constants.ClimbConstants.*; + +public class ClimbRaiseCommand extends Command { + private ClimbSubsystem climbSubsystem; + + public ClimbRaiseCommand(ClimbSubsystem climbSubsystem){ + this.climbSubsystem = climbSubsystem; + this.addRequirements(climbSubsystem); + } + + @Override + public void initialize() { + System.out.println("RAISING CLIMB..."); + climbSubsystem.goToExtension(EXTENSION_LIMIT_METERS); + } + + @Override + public boolean isFinished() { + return climbSubsystem.isAtExtension(); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + System.out.println(interrupted ? "CLIMB RAISING INTERRUPTED!" : "CLIMB RAISED!"); + } + +} diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java new file mode 100644 index 00000000..5b2ad74b --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java @@ -0,0 +1,26 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class ElevatorSetAutoCommand extends Command{ + private ElevatorSubsystem elevatorSubsystem; + public ElevatorSetAutoCommand(ElevatorSubsystem elevatorSubsystem){ + this.addRequirements(elevatorSubsystem); + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public void end(boolean interrupted){ + return; + } + + @Override + public void execute(){ + this.elevatorSubsystem.setAuto(); + } + + public boolean isFinished(){ + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java new file mode 100644 index 00000000..b5ea4819 --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java @@ -0,0 +1,26 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class ElevatorSetManualCommand extends Command{ + private ElevatorSubsystem elevatorSubsystem; + public ElevatorSetManualCommand(ElevatorSubsystem elevatorSubsystem){ + this.addRequirements(elevatorSubsystem); + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public void end(boolean interrupted){ + return; + } + + @Override + public void execute(){ + this.elevatorSubsystem.setManual(); + } + + public boolean isFinished(){ + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java new file mode 100644 index 00000000..b762881b --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class ElevatorToAMPCommand extends Command{ + private ElevatorSubsystem elevatorSubsystem; + public ElevatorToAMPCommand(ElevatorSubsystem elevatorSubsystem){ + this.addRequirements(elevatorSubsystem); + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public void end(boolean interrupted){ + return; + } + + @Override + public void execute(){ + this.elevatorSubsystem.setTargetState(ElevatorState.AMP); + } + + @Override + public boolean isFinished(){ + if(this.elevatorSubsystem.getState()==ElevatorState.AMP){ + return true; + } + else return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java new file mode 100644 index 00000000..d6349e1b --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class ElevatorToChuteCommand extends Command{ + private ElevatorSubsystem elevatorSubsystem; + public ElevatorToChuteCommand(ElevatorSubsystem elevatorSubsystem){ + this.addRequirements(elevatorSubsystem); + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public void end(boolean interrupted){ + return; + } + + @Override + public void execute(){ + this.elevatorSubsystem.setTargetState(ElevatorState.CHUTE); + } + + @Override + public boolean isFinished(){ + if(this.elevatorSubsystem.getState()==ElevatorState.CHUTE){ + return true; + } + else return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java new file mode 100644 index 00000000..c2a47e17 --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class ElevatorToGroundCommand extends Command{ + private ElevatorSubsystem elevatorSubsystem; + public ElevatorToGroundCommand(ElevatorSubsystem elevatorSubsystem){ + this.addRequirements(elevatorSubsystem); + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public void end(boolean interrupted){ + return; + } + + @Override + public void execute(){ + this.elevatorSubsystem.setTargetState(ElevatorState.GROUND); + } + + @Override + public boolean isFinished(){ + if(this.elevatorSubsystem.getState()==ElevatorState.GROUND){ + return true; + } + else return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java new file mode 100644 index 00000000..c87f0aab --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class ElevatorToSpeakerCommand extends Command{ + private ElevatorSubsystem elevatorSubsystem; + public ElevatorToSpeakerCommand(ElevatorSubsystem elevatorSubsystem){ + this.addRequirements(elevatorSubsystem); + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public void end(boolean interrupted){ + return; + } + + @Override + public void execute(){ + this.elevatorSubsystem.setTargetState(ElevatorState.SPEAKER); + } + + @Override + public boolean isFinished(){ + if(this.elevatorSubsystem.getState()==ElevatorState.SPEAKER){ + return true; + } + else return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java new file mode 100644 index 00000000..02665c44 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands.intake.pivot; + +import static frc.robot.Constants.RollerandPivotConstants.pivotcounterclockwise; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakePivotSubsystem; + +public class IntakePivotExtendedCommand extends Command{ + private final IntakePivotSubsystem pivotSubsystem; + + public IntakePivotExtendedCommand(IntakePivotSubsystem pivotSubsystem){ + this.pivotSubsystem = pivotSubsystem; + } + + @Override + public void initialize() { + // TODO Auto-generated method stub + pivotSubsystem.movePivot(pivotcounterclockwise); + } + + @Override + public void execute() { + // TODO Auto-generated method stub + } + + @Override + public void end(boolean interrupted) { + // TODO Auto-generated method stub + pivotSubsystem.movePivot(0); + } + + @Override + public boolean isFinished() { + // TODO Auto-generated method stub + return pivotSubsystem.pivotisextended(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotMiddleCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotMiddleCommand.java new file mode 100644 index 00000000..15d1f0a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotMiddleCommand.java @@ -0,0 +1,36 @@ +package frc.robot.commands.intake.pivot; +// package frc.robot.subsystems; + + +// import edu.wpi.first.wpilibj2.command.Command; + +// public class PivottoMiddleCommand extends Command{ +// private final PivotSubsystem pivotSubsystem; + +// public PivottoMiddleCommand(PivotSubsystem pivotSubsystem){ +// this.pivotSubsystem = pivotSubsystem; +// } +// @Override +// public void initialize() { +// // TODO Auto-generated method stub +// pivotSubsystem.movePivot(-1.0); +// } +// @Override +// public void execute() { +// // TODO Auto-generated method stub +// } + +// @Override +// public void end(boolean interrupted) { +// // TODO Auto-generated method stub +// pivotSubsystem.movePivot(0); + +// } + +// @Override +// public boolean isFinished() { +// return pivotSubsystem.encoderPosition()== 2; +// // TODO Auto-generated method stub + +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java new file mode 100644 index 00000000..ac7154bf --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java @@ -0,0 +1,42 @@ + +package frc.robot.commands.intake.pivot; + + +import static frc.robot.Constants.RollerandPivotConstants.pivotclockwise; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakePivotSubsystem; + +public class IntakePivotVerticalCommand extends Command{ + private final IntakePivotSubsystem pivotSubsystem; + + public IntakePivotVerticalCommand(IntakePivotSubsystem pivotSubsystem){ + this.pivotSubsystem = pivotSubsystem; + } + @Override + public void initialize() { + // TODO Auto-generated method stub + pivotSubsystem.movePivot(pivotclockwise); + } + + @Override + public void execute() { + // TODO Auto-generated method stub + + } + + @Override + public void end(boolean interrupted) { + // TODO Auto-generated method stub + pivotSubsystem.movePivot(0); + //pivotSubsystem.resetEncoder(); + } + + @Override + public boolean isFinished() { + // TODO Auto-generated method stub + return pivotSubsystem.pivotisextended(); + + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java new file mode 100644 index 00000000..ac71e654 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java @@ -0,0 +1,48 @@ +package frc.robot.commands.intake.roller; + +import static frc.robot.Constants.RollerandPivotConstants.pastsensortime; +import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; +import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.util.TrackingTimer; + +public class IntakeRollerFeedCommand extends Command{ + private final IntakeRollersSubsystem intakeSubsystem; + private final TrackingTimer timer; + + public IntakeRollerFeedCommand(IntakeRollersSubsystem intakeSubsystem){ + this.intakeSubsystem = intakeSubsystem; + timer = new TrackingTimer(); + } + + @Override + public void initialize() { + // TODO Auto-generated method stub + intakeSubsystem.setAllRollSpeed(rollerscounterclockwise,rollersclockwise); + } + + @Override + public void execute() { + // TODO Auto-generated method stub + if(intakeSubsystem.sensorNow() == false && timer.hasStarted()==false){ + timer.start(); + } + } + + @Override + public void end(boolean interrupted) { + // TODO Auto-generated method stub + intakeSubsystem.setAllRollSpeed(0,0); + + } + + @Override + public boolean isFinished() { + // TODO Auto-generated method stub + return timer.hasElapsed(pastsensortime); + + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java new file mode 100644 index 00000000..fab8ca37 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java @@ -0,0 +1,38 @@ +package frc.robot.commands.intake.roller; + + +import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; +import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; + +public class IntakeRollerIntakeCommand extends Command{ + private final IntakeRollersSubsystem intakeSubsystem; + + public IntakeRollerIntakeCommand(IntakeRollersSubsystem intakeSubsystem){ + this.intakeSubsystem = intakeSubsystem; + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + // TODO Auto-generated method stub + intakeSubsystem.setRollSpeed(rollerscounterclockwise,rollersclockwise); + } + + @Override + public void end(boolean interrupted){ + // TODO Auto-generated method stub + intakeSubsystem.setRollSpeed(0,0); + } + + @Override + public boolean isFinished() { + return intakeSubsystem.sensorNow(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java new file mode 100644 index 00000000..e5e7c805 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java @@ -0,0 +1,49 @@ +package frc.robot.commands.intake.roller; + + +import static frc.robot.Constants.RollerandPivotConstants.pastsensortime; +import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; +import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.util.TrackingTimer; + +public class IntakeRollerOutakeCommand extends Command{ + private final IntakeRollersSubsystem intakeSubsystem; + private final TrackingTimer timer; + + public IntakeRollerOutakeCommand(IntakeRollersSubsystem intakeSubsystem){ + this.intakeSubsystem = intakeSubsystem; + timer = new TrackingTimer(); + } + + @Override + public void initialize() { + intakeSubsystem.setRollSpeed(rollersclockwise,rollerscounterclockwise); + } + + @Override + public void execute() { + // TODO Auto-generated method stub + + if(intakeSubsystem.sensorNow()==false&& timer.hasStarted()==false ){ + timer.start(); + } + + + } + + @Override + public void end(boolean interrupted){ + // TODO Auto-generated method stub + + intakeSubsystem.setRollSpeed(0,0); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(pastsensortime); + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java new file mode 100644 index 00000000..5c4f0665 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.shooter.feed; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterFeederSubsystem; + +public class ShooterFeedLoadCommand extends Command { + + ShooterFeederSubsystem feederSubsystem; + + public ShooterFeedLoadCommand(ShooterFeederSubsystem feederSubsystem){ + this.feederSubsystem = feederSubsystem; + addRequirements(feederSubsystem); + } + + @Override + public void initialize() { + feederSubsystem.setFeederMotorSpeed(feederSubsystem.FEEDER_MOTOR_SPEED); + } + + @Override + public boolean isFinished() { + if(feederSubsystem.getProximity() > feederSubsystem.TOLERANCE){ + return true; + } + return false; + } + + @Override + public void end(boolean interrupted) { + feederSubsystem.setFeederMotorSpeed(0); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java new file mode 100644 index 00000000..cf0fe23a --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java @@ -0,0 +1,30 @@ +package frc.robot.commands.shooter.feed; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterFeederSubsystem; + +public class ShooterFeedShootCommand extends Command{ + ShooterFeederSubsystem feederSubsystem; + + public ShooterFeedShootCommand(ShooterFeederSubsystem feederSubsystem){ + this.feederSubsystem = feederSubsystem; + } + + @Override + public void initialize() { + feederSubsystem.setFeederMotorSpeed(feederSubsystem.FEEDER_MOTOR_SPEED); + } + + @Override + public boolean isFinished() { + if(feederSubsystem.getProximity() < feederSubsystem.NO_NOTE_TOLERANCE){ + return true; + } + return false; + } + + @Override + public void end(boolean interrupted) { + feederSubsystem.setFeederMotorSpeed(0); + } + +} diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java new file mode 100644 index 00000000..06545d36 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.shooter.flywheel; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; + +public class ShooterFlywheelReadyCommand extends Command{ + + ShooterFlywheelSubsystem shooterSubsystem; + + public ShooterFlywheelReadyCommand(ShooterFlywheelSubsystem shooterSubsystem){ + this.shooterSubsystem = shooterSubsystem; + addRequirements(shooterSubsystem); + } + + @Override + public void initialize() { + shooterSubsystem.setShooterMotorSpeed(shooterSubsystem.SHOOTER_MOTOR_SPEED); + } + + //pivot: vertical, auto-aim + //feed: load, shoot + //shooter: stop, ready shooters + + //load note + //shoot + //ready shooter + //vertical + //auto-angle + + @Override + public void end(boolean interrupted) { + System.out.println("Ready Shooter has been interuppted"); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommands.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommands.java new file mode 100644 index 00000000..4f083141 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommands.java @@ -0,0 +1,26 @@ +package frc.robot.commands.shooter.flywheel; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; + +public class ShooterFlywheelStopCommands extends Command{ + ShooterFlywheelSubsystem shooterSubsystem; + + public ShooterFlywheelStopCommands(ShooterFlywheelSubsystem shooterSubsystem){ + this.shooterSubsystem = shooterSubsystem; + addRequirements(shooterSubsystem); + } + + @Override + public void initialize(){ + shooterSubsystem.setShooterMotorSpeed(0); + } + + @Override + public boolean isFinished(){ + return true; + } + +} + + + diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java new file mode 100644 index 00000000..b089c0e1 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java @@ -0,0 +1,30 @@ +package frc.robot.commands.shooter.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; + +public class ShooterPivotAimCommand extends Command{ + + ShooterPivotSubsystem shooterPivotSubsystem; + + public ShooterPivotAimCommand(ShooterPivotSubsystem pivotSubsystem){ + this.shooterPivotSubsystem = pivotSubsystem; + addRequirements(pivotSubsystem); + } + + public void initialize(){ + shooterPivotSubsystem.setAutoAimBoolean(true); + } + + public void end(){ + shooterPivotSubsystem.setAutoAimBoolean(false); + } + + public boolean isFinished(){ + if(Math.abs(shooterPivotSubsystem.getPosition()) - shooterPivotSubsystem.getCurrentAngle() < shooterPivotSubsystem.ERRORTOLERANCE){ + return true; + } + + return false; + } +} diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java new file mode 100644 index 00000000..1e419cda --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java @@ -0,0 +1,5 @@ +package frc.robot.commands.shooter.pivot; + +public class ShooterPivotVerticalCommand { + +} diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java new file mode 100644 index 00000000..062fc2ca --- /dev/null +++ b/src/main/java/frc/robot/controllers/BaseDriveController.java @@ -0,0 +1,25 @@ +package frc.robot.controllers; + +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +public abstract class BaseDriveController { + /** + * Gets the forward power commanded by the controller. + * @return The [-1.0, 1.0] forward power. + */ + public abstract double getForwardPower(); + + /** + * Gets the left power commanded by the controller. + * @return The [-1.0, 1.0] left power. + */ + public abstract double getLeftPower(); + + /** + * Gets the rotational power commanded by the controller. + * @return The [-1.0, 1.0] angular power. + */ + public abstract double getRotatePower(); + + public abstract JoystickButton getFieldResetButton(); +} diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java new file mode 100644 index 00000000..8c58e32b --- /dev/null +++ b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java @@ -0,0 +1,72 @@ +package frc.robot.controllers; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +/** + * A two-joystick drive controller on ports 0 and 1. + */ +public class DualJoystickDriveController extends BaseDriveController { + private final Joystick leftJoystick = new Joystick(0); + private final JoystickButton + leftTrigger = new JoystickButton(leftJoystick, 1), + leftMiddleButton = new JoystickButton(leftJoystick, 2), + leftTopLeftButton = new JoystickButton(leftJoystick, 3), + leftTopRightButton = new JoystickButton(leftJoystick, 4), + leftMiddleLeftButton = new JoystickButton(leftJoystick, 5), + leftMiddleRightButton = new JoystickButton(leftJoystick, 6), + leftBackButton = new JoystickButton(leftJoystick, 7); + + private final Joystick rightJoystick = new Joystick(1); + private final JoystickButton + rightTrigger = new JoystickButton(rightJoystick, 1), + right2 = new JoystickButton(rightJoystick, 2), + rightTopLeftButton = new JoystickButton(rightJoystick, 3), + rightTopRightButton = new JoystickButton(rightJoystick, 4), + rightMiddleLeftButton = new JoystickButton(rightJoystick, 5), + rightMiddleRightButton = new JoystickButton(rightJoystick, 6), + rightBackButton = new JoystickButton(rightJoystick, 7); + + private static final double JOYSTICK_DEADBAND = 0.08; + + @Override + public double getForwardPower() { + double scale = getDriveScaling(); + return MathUtil.applyDeadband(-leftJoystick.getY() * scale, JOYSTICK_DEADBAND); + } + + @Override + public double getLeftPower() { + double scale = getDriveScaling(); + return MathUtil.applyDeadband(leftJoystick.getX() * scale, JOYSTICK_DEADBAND); + } + + @Override + public double getRotatePower() { + return MathUtil.applyDeadband(-rightJoystick.getX() * getTurnScaling(), JOYSTICK_DEADBAND); + } + + /** + * Gets the amount to scale translational input by. + * @return The scale to apply to translational input. + */ + private double getDriveScaling() { + boolean isSlowMode = leftJoystick.getTrigger(); + return isSlowMode ? 0.24 : 1.0; + } + + /** + * Gets the amount to scale rotational input by. + * @return The scale to apply to rotational input. + */ + private double getTurnScaling() { + boolean isSlowMode = leftJoystick.getTrigger(); + return isSlowMode ? 0.125 : 0.5; + } + + public JoystickButton getFieldResetButton() { + return right2; + } + +} diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java new file mode 100644 index 00000000..5a4a4ccc --- /dev/null +++ b/src/main/java/frc/robot/controllers/XboxDriveController.java @@ -0,0 +1,41 @@ +package frc.robot.controllers; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +/** + * A single Xbox controller on port 0. + */ +public class XboxDriveController extends BaseDriveController { + private final XboxController driveController = new XboxController(0); + private final JoystickButton + driveAButton = new JoystickButton(driveController, XboxController.Button.kA.value), + driveBButton = new JoystickButton(driveController, XboxController.Button.kB.value), + driveXButton = new JoystickButton(driveController, XboxController.Button.kX.value), + driveYButton = new JoystickButton(driveController, XboxController.Button.kY.value), + driveLBumper = new JoystickButton(driveController, XboxController.Button.kLeftBumper.value), + driveRBumper = new JoystickButton(driveController, XboxController.Button.kRightBumper.value), + driveLStickButton = new JoystickButton(driveController, XboxController.Button.kLeftStick.value), + driveRStickButton = new JoystickButton(driveController, XboxController.Button.kRightStick.value), + driveBackButton = new JoystickButton(driveController, XboxController.Button.kBack.value), + driveStartButton = new JoystickButton(driveController, XboxController.Button.kStart.value); + + @Override + public double getForwardPower() { + return -driveController.getLeftY(); + } + + @Override + public double getLeftPower() { + return -driveController.getLeftX(); + } + + @Override + public double getRotatePower() { + return -driveController.getRightX(); + } + + public JoystickButton getFieldResetButton() { + return driveAButton; + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorState.java new file mode 100644 index 00000000..6c70dcff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorState.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.util.Units; + +import frc.robot.Constants; + +public enum ElevatorState { + GROUND(Constants.ElevatorConstants.GROUND_POSITION), + SPEAKER(Constants.ElevatorConstants.SPEAKER_POSITION), + AMP(Constants.ElevatorConstants.AMP_POSITION), + CHUTE(Constants.ElevatorConstants.CHUTE_POSITION), + TRAP(Constants.ElevatorConstants.TRAP_POSITION), + START(Constants.ElevatorConstants.START_POSITION); + + private final double extendDistanceMeters; + + private ElevatorState(double extendDistanceMeters){ + this.extendDistanceMeters = extendDistanceMeters; + } + + public double getExtension(){ + return this.extendDistanceMeters; + } + +} diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorSubsystem.java new file mode 100644 index 00000000..97953664 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorSubsystem.java @@ -0,0 +1,156 @@ +package frc.robot.subsystems.elevator; + +import java.util.EnumSet; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableEvent.Kind; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants; + +public class ElevatorSubsystem extends SubsystemBase{ + private NetworkTableInstance elevatorNetworkTableInstance; + private NetworkTable elevatorNetworkTable; + + private volatile boolean IS_MANUAL = false; + + private ElevatorState state = ElevatorState.GROUND; + private ElevatorState targetState = ElevatorState.START; + + private final CANSparkMax extensionMotor; + private RelativeEncoder extensionEncoder; + private SparkPIDController extensionPidController; + + //PID Values + private static final double extensionP = 2.4; + private static final double extensionI = 0; + private static final double extensionD = 0; + private static final double extensionTolerance = 0.003; + + private final CANSparkMax extensionFollow; + + private final DigitalInput zeroLimitSwitch; + + //Controller for testing. + private final XboxController mechController; + + public ElevatorSubsystem(){ + //Print out current position for debug & measurement + //System.out.print(extensionEncoder.getPosition()); + + elevatorNetworkTableInstance = NetworkTableInstance.getDefault(); + elevatorNetworkTable = elevatorNetworkTableInstance.getTable("elevator"); + elevatorNetworkTable.addListener("target_position", EnumSet.of(NetworkTableEvent.Kind.kValueAll), this::acceptNewPosition); + + extensionMotor = new CANSparkMax(Constants.ElevatorConstants.EXTENSION_ID, MotorType.kBrushless); + extensionMotor.setIdleMode(IdleMode.kBrake); + + extensionEncoder = extensionMotor.getEncoder(); + extensionEncoder.setPositionConversionFactor(Constants.ElevatorConstants.POSITIONCONVERSIONFACTOR); + extensionEncoder.setVelocityConversionFactor(Constants.ElevatorConstants.VELOCITYCONVERSIONFACTOR); + extensionEncoder.setPosition(0); + + extensionFollow = new CANSparkMax(Constants.ElevatorConstants.EXTENSION_FOLLOW_ID, MotorType.kBrushless); + extensionFollow.follow(extensionMotor); + extensionFollow.setIdleMode(IdleMode.kBrake); + + extensionPidController = extensionMotor.getPIDController(); + extensionPidController.setP(extensionP); + extensionPidController.setI(extensionI); + extensionPidController.setD(extensionD); + extensionPidController.setSmartMotionAllowedClosedLoopError(extensionTolerance, 0); + + zeroLimitSwitch = new DigitalInput(Constants.ElevatorConstants.ZERO_LIMIT_ID); + + //Controller for testing. + mechController = new XboxController(Constants.OperatorConstants.kDriverControllerPort); + } + @Override + public void periodic(){ + + if(IS_MANUAL){ + //Add some factors for better control. + extensionMotor.set(mechController.getRightY()); + return; + } + if (zeroLimitSwitch != null && !zeroLimitSwitch.get()){ + extensionEncoder.setPosition(0); + } + + extensionMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + + //Start move to target posision + if (targetState != state){ + extensionPidController.setReference(targetState.getExtension(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut); + } + + if(mechController.getAButtonPressed()){ + this.setTargetState(ElevatorState.GROUND); + } + else if(mechController.getBButtonPressed()){ + this.setTargetState(ElevatorState.AMP); + } + else if(mechController.getXButtonPressed()){ + this.setTargetState(ElevatorState.SPEAKER); + } + else if(mechController.getYButtonPressed()){ + this.setTargetState(ElevatorState.CHUTE); + } + + } + + public void setState(ElevatorState state) { + this.state = state; + return; + } + + public void setTargetState(ElevatorState targetState){ + this.targetState = targetState; + return; + } + + public void setManual(){ + this.IS_MANUAL = true; + return; + } + + public void setAuto(){ + this.IS_MANUAL = false; + return; + } + public double getExtensionMeters() { + return extensionEncoder.getPosition(); + } + + public ElevatorState getState() { + return this.state; + } + + public ElevatorState getTargetState(){ + return this.targetState; + } + + + private void acceptNewPosition(NetworkTable table, String key, NetworkTableEvent event){ + System.out.println("got networktablex"); + System.out.println(event.valueData.toString()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java new file mode 100644 index 00000000..ba3bb542 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.climb; + +import static frc.robot.Constants.ClimbConstants.*; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.util.MotorUtil; + +public class ClimbArm { + private final CANSparkMax winchMotor; + private RelativeEncoder extensionEncoder; + + private final DigitalInput zeroLimitSwitch; + + private double desiredExtension = 0; + + public ClimbArm(int WINCH_MOTOR_ID, int ZERO_LIMIT_ID) { + winchMotor = MotorUtil.createSparkMax(WINCH_MOTOR_ID, (sparkMax) -> { + sparkMax.setIdleMode(IdleMode.kBrake); + sparkMax.setInverted(false); + + extensionEncoder = sparkMax.getEncoder(); + extensionEncoder.setPositionConversionFactor(AXLE_PERIMETER_METERS / WINCH_REDUCTION); + extensionEncoder.setPosition(0); + + sparkMax.setSoftLimit(SoftLimitDirection.kForward, (float) EXTENSION_LIMIT_METERS); + sparkMax.enableSoftLimit(SoftLimitDirection.kForward, true); + sparkMax.setSoftLimit(SoftLimitDirection.kReverse, 0); + sparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true); + }); + + zeroLimitSwitch = new DigitalInput(LEFT_ZERO_LIMIT_ID); + } + + public void update() { + if (zeroLimitSwitch != null && zeroLimitSwitch.get()) + resetEncoder(); + + if (extensionEncoder.getPosition() == 0 && !zeroLimitSwitch.get()) + System.out.println(this.toString() + "encoder uncalibrated!"); + + if (isAtExtension()) + winchMotor.set(0); + else + winchMotor.set(desiredExtension > extensionEncoder.getPosition() + ? MAX_WINCH_POWER : -MAX_WINCH_POWER); + + } + + public void goToExtension(double desiredHeight) { + desiredExtension = MathUtil.clamp(desiredHeight, 0, EXTENSION_LIMIT_METERS); + } + + public boolean isAtExtension() { + return Math.abs(extensionEncoder.getPosition() - desiredExtension) < EXTENSION_TOLERANCE_METERS; + } + + private void resetEncoder() { + extensionEncoder.setPosition(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java new file mode 100644 index 00000000..6e88cddd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.climb; + +import static frc.robot.Constants.ClimbConstants.*; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimbSubsystem extends SubsystemBase{ + private final ClimbArm leftClimbArm; + private final ClimbArm rightClimbArm; + + public ClimbSubsystem() { + leftClimbArm = new ClimbArm(LEFT_WINCH_MOTOR_ID, LEFT_ZERO_LIMIT_ID); + rightClimbArm = new ClimbArm(RIGHT_WINCH_MOTOR_ID, RIGHT_ZERO_LIMIT_ID); + } + + @Override + public void periodic() { + leftClimbArm.update(); + rightClimbArm.update(); + } + + public void goToExtension(double height) { + leftClimbArm.goToExtension(height); + rightClimbArm.goToExtension(height); + } + + public boolean isAtExtension() { + return leftClimbArm.isAtExtension() && rightClimbArm.isAtExtension(); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java new file mode 100644 index 00000000..5e09e34b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.Constants.RollerandPivotConstants.*; +//import edu.wpi.first.wpilibj.Encoder; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class IntakePivotSubsystem extends SubsystemBase{ + private final com.ctre.phoenix.motorcontrol.can.TalonSRX motor1; + // private final Encoder intakeencoder; + private final DigitalInput extendedlimitswitch; + private final DigitalInput retractedlimitswitch; + + public IntakePivotSubsystem(){ + motor1 = new TalonSRX(motor1ID); + //intakeencoder = new Encoder(1,2); + extendedlimitswitch = new DigitalInput(extendedlimitswitchID); + retractedlimitswitch = new DigitalInput(retractedlimitswitchID); + } + + // public void resetEncoder(){ + // intakeencoder.reset(); + // } + + // public double encoderPosition(){ + // return intakeencoder.get(); + // } + + public void movePivot(double speed){ + motor1.set(TalonSRXControlMode.PercentOutput, speed); + } + + public boolean pivotisextended(){ + if (extendedlimitswitch.get()){ + return true; + } + else{ + return false; + } + } + + public boolean pivotisretracted(){ + if(retractedlimitswitch.get()){ + return true; + } + else{ + return false; + } + } + + public void setPivotSpeed(double right){ + motor1.set(TalonSRXControlMode.PercentOutput, right); + } + + + + + + + + + + + + + + + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java new file mode 100644 index 00000000..47a52dff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java @@ -0,0 +1,73 @@ +// 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. + +//1 = clockwise, -1 = counterclockwise + +package frc.robot.subsystems.intake; + +import static frc.robot.Constants.RollerandPivotConstants.*; + +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class IntakeRollersSubsystem extends SubsystemBase { + private final TalonSRX topmotor; + private final TalonSRX bottommotor; + private final TalonSRX lastmotor; + private final AnalogPotentiometer sensor; + + + /** Creates a new ExampleSubsystem. */ + public IntakeRollersSubsystem() { + lastmotor = new TalonSRX(lastmotorID); + topmotor = new TalonSRX(topmotorID); + bottommotor = new TalonSRX(bottommotorID); + sensor = new AnalogPotentiometer(sensorID); + } + + public boolean sensorNow(){ + if (sensor.get()<=sensorreached){ + return true; + } + else{ + return false; + } + } + + public void setRollSpeed(double top, double bottom){ + topmotor.set(TalonSRXControlMode.PercentOutput, top); + bottommotor.set(TalonSRXControlMode.PercentOutput, bottom); + } + + public void setAllRollSpeed(double topone, double bottomone){ + topmotor.set(TalonSRXControlMode.PercentOutput, topone); + lastmotor.set(TalonSRXControlMode.PercentOutput, topone); + bottommotor.set(TalonSRXControlMode.PercentOutput, bottomone); + } + + public void setRollersOutwards(Boolean pressedA){ + if(pressedA==true) + topmotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); + bottommotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); + } + + public void setRollersInwards(Boolean pressedB){ + if(pressedB==true) + topmotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); + bottommotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); + } + + + @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/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java new file mode 100644 index 00000000..c3acc43b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.ColorSensorV3; + +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterFeederSubsystem extends SubsystemBase{ + + //finals + public final double FEEDER_MOTOR_SPEED = 0.1; + public final int NO_NOTE_TOLERANCE = 10; //must test with no note in front of sensor + public final int TOLERANCE = 10; //represents the value when half note is in front of sensor + + //motors + private final TalonSRX feederMotor; + + //devices + private final ColorSensorV3 shooterSensor; //distance sensor + + public ShooterFeederSubsystem(){ + //motors + feederMotor = new TalonSRX(10); + + //sensors + shooterSensor = new ColorSensorV3(I2C.Port.kMXP); + } + + public void setFeederMotorSpeed(double speed){ + feederMotor.set(TalonSRXControlMode.PercentOutput, speed); + System.out.println("feeding motor speed is: " + feederMotor.getMotorOutputPercent()); + + } + + public int getProximity(){ + System.out.println("proximity: " + shooterSensor.getProximity()); + return shooterSensor.getProximity(); + } + + public ColorSensorV3 getSensor(){ + System.out.println("returning shooter sensor"); + return shooterSensor; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java new file mode 100644 index 00000000..c86fcc4e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -0,0 +1,57 @@ +// INTAKE one talon for feeding one neo for pivot +// SHOOTER one neo for shooting, one neo for pivot, one talon for conveyer belt + +//create enums, expecting, holding, firing, and no note + +package frc.robot.subsystems.shooter; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterFlywheelSubsystem extends SubsystemBase { + + //change later (ALL OF THEM ARE PLACEHOLDERS) + int IDNUMBER = 10; //so I remember to change them later + public final double SHOOTER_MOTOR_SPEED = 1; + + //motors + private final CANSparkMax shooterMotor; + private final CANSparkMax shooterMotorTwo; + + //devices + private ShooterState currentState; //an enum state thing + + //sensors + + public ShooterFlywheelSubsystem(){ + //motors + shooterMotor = new CANSparkMax(13, MotorType.kBrushless); + shooterMotorTwo = new CANSparkMax(14, MotorType.kBrushless); + + shooterMotor.setIdleMode(IdleMode.kCoast); + shooterMotorTwo.setIdleMode(IdleMode.kCoast); + + + //second motor shooter follows first + shooterMotorTwo.follow(shooterMotor, true); + + //enums + setShooterState(ShooterState.VERTICAL); + } + + //enum functions + public ShooterState getShooterState(ShooterState state){ + return currentState; + } + + public void setShooterState(ShooterState newState){ + currentState = newState; + } + + public void setShooterMotorSpeed(double speed){ + shooterMotor.setVoltage(-speed * 12); + System.out.println("shooter motor speed is: " + shooterMotor.get()); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java new file mode 100644 index 00000000..b8afd04f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -0,0 +1,153 @@ +package frc.robot.subsystems.shooter; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterPivotSubsystem extends SubsystemBase { + + //final vars + public final double PIVOT_SPEED = 0.1; + final double GEARBOX_RATIO = 18.16; //ask cadders + public final int ERRORTOLERANCE = 5; //error tolerance for pid + final int LIMIT_SWITCH_ID = 1; //placeholder + final double CONVERSION_FACTOR = Math.PI/(2*4.57); + + //motors + private final CANSparkMax pivotMotor; + + //devices + private RelativeEncoder rotationEncoder; + private SparkPIDController rotationPIDController; + private final DigitalInput limitSwitch; + + //angle PID (CHANGE LATER) + private static final double ANGLE_P = 2.4; + private static final double ANGLE_I = 0; + private static final double ANGLE_D = 0; + + //field + private boolean alliance; //true equals red alliance + private boolean autoAim; + private double currentEncoderAngle; + private Pose2d currentField = new Pose2d(); + + //center of red speaker: (652.73 218.42) + double RED_X = Units.inchesToMeters(652.73 + 9.05); //9.05 is half of 18.1 which is length of overhang of speaker-- we want halfway point + double RED_Y = Units.inchesToMeters(218.42); + + //center of blue speaker: (-1.50 218.42) + double BLUE_X = Units.inchesToMeters(-1.5+9.05); //9.05 is half of 18.1 which is length of overhang of speaker-- we want halfway point + double BLUE_Y = Units.inchesToMeters(218.42); + + public ShooterPivotSubsystem(boolean alliance){ + + //motors + pivotMotor = new CANSparkMax(12, MotorType.kBrushless); + pivotMotor.setInverted(true); + + //devices + rotationEncoder = pivotMotor.getEncoder(); + rotationEncoder.setPosition(0); + rotationPIDController = pivotMotor.getPIDController(); + limitSwitch = new DigitalInput(LIMIT_SWITCH_ID); + + + //setting PID vars + rotationPIDController.setP(ANGLE_P); + rotationPIDController.setI(ANGLE_I); + rotationPIDController.setD(ANGLE_D); + + //encoder stuff + rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR); + //rotationEncoder.setInverted(true); + rotationPIDController.setSmartMotionAllowedClosedLoopError(ERRORTOLERANCE, 0); //what does 0 do (slotID is from 0-3) + + //field + this.alliance = alliance; + autoAim = false; + } + + //motor speed setting functions + public void setPivotMotorSpeed(double speed){ + pivotMotor.setVoltage(speed * 12); + //System.out.println("flywheer motor speed is: " + pivotMotor.get()); + } + + + public void setAngle(double angle){ //check if it works + rotationPIDController.setReference(angle, CANSparkMax.ControlType.kPosition); + //System.out.println("setting angle to: " + angle); + } + + public void setFieldPosition(Pose2d field){ + //System.out.println("setting field position"); + currentField = field; + } + + public double getAutoAimAngle(double distance){ + double speakerHeight = Units.inchesToMeters(80.51); + //System.out.println("Angle of shooter" + Math.atan(speakerHeight/distance)); + return Math.atan(speakerHeight/distance); + } + + public void printCurrentAngle(){ + System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29); + } + + public double getDistance(){ + + if(alliance){ //true = red + double xLength = Math.pow(currentField.getX()-RED_X, 2); + double yLength = Math.pow(currentField.getY()-RED_Y, 2); + //System.out.println("alliance red:" + alliance); + return Math.sqrt(xLength + yLength); + + } else { + double xLength = Math.pow(currentField.getX()-BLUE_X, 2); + double yLength = Math.pow(currentField.getY()-BLUE_Y, 2); + + return Math.sqrt(xLength + yLength); + } + } + + public double getPosition(){ + //System.out.println("rotation encoder position: " + rotationEncoder.getPosition()); + return rotationEncoder.getPosition(); + } + + public double getCurrentAngle(){ + return currentEncoderAngle; + } + + public void setAutoAimBoolean(boolean red){ + autoAim = red; + } + + public void periodic(){ + //resets relative encoder every time robot starts again + //(check if encoder prints zero when run) + // if(limitSwitch != null && limitSwitch.get()){ //false = limit switch is pressed + // rotationEncoder.setPosition(0); + // // System.out.println(rotationEncoder.getPosition()); //should print 0 + // } + + if(autoAim){ + setAngle(getAutoAimAngle(getDistance())); + } + + printCurrentAngle(); + + // System.out.println("current pos" + rotationEncoder.getPosition()); + + // if(currentState == ShooterState.FIRING && (shooterSensor.getRed() < TOLERANCE)){ //when there is no note + // setShooterState(ShooterState.NO_NOTE); + // } + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterState.java b/src/main/java/frc/robot/subsystems/shooter/ShooterState.java new file mode 100644 index 00000000..f2ec1974 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterState.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems.shooter; + +public enum ShooterState { + LOADING_NOTE, //taken through intake + HOLDING_NOTE, //in the conveyer belt + NO_NOTE, //no note in robot + FIRING, + VERTICAL //for start of the match +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTestSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTestSubsystem.java new file mode 100644 index 00000000..8a70ae27 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTestSubsystem.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.shooter; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.XboxController; + +public class ShooterTestSubsystem extends SubsystemBase{ + + private final XboxController controller; + + private final CANSparkMax topmotor; + private final CANSparkMax bottommotor; + + private double topMotorVel; + private double botMotorVel; + + public ShooterTestSubsystem(){ + + controller = new XboxController(0); + + topmotor = new CANSparkMax(1, MotorType.kBrushless); + bottommotor = new CANSparkMax(2, MotorType.kBrushless); + + topMotorVel = 0.0; + botMotorVel = 0.0; + } + + public void periodic(){ + + if(controller.getRightBumperPressed()){ + topMotorVel += 0.01; + botMotorVel += 0.01; + } + + if(controller.getLeftBumperPressed()){ + topMotorVel -= 0.01; + botMotorVel -= 0.01; + } + + if(controller.getAButtonPressed()){ + topMotorVel += 0.01; + } + + if(controller.getBButtonPressed()){ + topMotorVel -= 0.01; + } + + if(controller.getXButtonPressed()){ + botMotorVel += 0.01; + } + + if(controller.getYButtonPressed()){ + botMotorVel -= 0.01; + } + + System.out.println("TOP: " + topMotorVel); + System.out.println("BOTTOM: " + botMotorVel); + topmotor.set(topMotorVel); + bottommotor.set(botMotorVel); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 887e11da..6a814ca3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.swerve; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; +// import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.ControlType; @@ -23,12 +23,14 @@ public class SwerveModule { private final SwerveDriveMotor driveMotor; private final CANSparkMax steerMotor; - private RelativeEncoder steerRelativeEncoder; + // private RelativeEncoder steerRelativeEncoder; private SparkAnalogSensor steerAbsoluteEncoder; private SparkPIDController steerPidController; private double offsetRads; + private boolean verbose; + private static final double DRIVE_METERS_PER_ROTATION = (13.0 / 90.0) * Math.PI * Units.inchesToMeters(4.0); private static final double DRIVE_ROTATIONS_PER_METER = 1.0 / DRIVE_METERS_PER_ROTATION; @@ -37,23 +39,26 @@ public class SwerveModule { //The encoder board maps the 5V output of the encoder to 3.3V of the Spark Max // TODO: tune PIDs, comments are 2023 constants - private static final double FALCON_DRIVE_P = 0; // .05 + private static final double FALCON_DRIVE_P = 0.0028; // .05 private static final double FALCON_DRIVE_I = 0; // 0 - private static final double FALCON_DRIVE_D = 0; // 0 - private static final double FALCON_DRIVE_FF = .11285266; // 0.186697057706 + private static final double FALCON_DRIVE_D = 0.005; // 0 + private static final double FALCON_DRIVE_FF = .11285266; // .11285266; private static final double VORTEX_DRIVE_P = 0; private static final double VORTEX_DRIVE_I = 0; private static final double VORTEX_DRIVE_D = 0; private static final double VORTEX_DRIVE_FF = 0; - private static final double STEER_P = .7; // .7 + private static final double STEER_P = .68; // .7 private static final double STEER_I = 0; // 0 private static final double STEER_D = 0; // 35 private static final double STEER_FF = 0; // 0 private Timer crimor; + private double maxEncoder = 360; + private double minEncoder = 0; + /** * Constructs a Swerve Module * @param drivePort The CAN ID of the drive motor @@ -137,6 +142,7 @@ public SwerveModulePosition getState(){ * @param state The desired SwerveModuleState */ public void setDesiredState(SwerveModuleState state) { + //System.out.println(state.angle.getDegrees()); Rotation2d currentAngle = getWrappedAngle(); SwerveModuleState optimized = SwerveModuleState.optimize(state, currentAngle); @@ -146,19 +152,19 @@ public void setDesiredState(SwerveModuleState state) { double targetVelocity = optimized.speedMetersPerSecond; //* Math.abs(Math.cos(angleErrorRads)); // double currentVelocity = driveMotor.getVelocity(); + driveMotor.setVelocity(targetVelocity); + + steerPidController.setReference(targetAngleRads, ControlType.kPosition); + } + + public void setVerbose(){ if (crimor.advanceIfElapsed(.1)){ - System.out.print(" current " + twoDecimals(getWrappedAngle().getDegrees())); - System.out.println(" target " + twoDecimals(Math.toDegrees(MathUtil.angleModulus(targetAngleRads)))); + System.out.println(" current " + twoDecimals(getWrappedAngle().getDegrees())); + // System.out.println(" target " + twoDecimals(Math.toDegrees(MathUtil.angleModulus(targetAngleRads)))); // System.out.print(" error " + twoDecimals(driveMotor.getError())); - // System.out.println(" target " + twoDecimals(targetVelocity)); - } + // System.out.println(" target " + twoDecimals(driveMotor.getSetPoint())); - driveMotor.setVelocity(targetVelocity); - // if(Math.abs(Math.toDegrees(angleErrorRads)) < .5){ - // steerMotor.set(0); - // return; - // } - steerPidController.setReference(targetAngleRads, ControlType.kPosition); + } } /** @@ -185,10 +191,6 @@ public void setRawPowersWithAngle(double drivePower, double angleRads){ driveMotor.setPower(drivePower); System.out.println(Math.abs(currentAngle.minus(new Rotation2d(targetAngleRads)).getDegrees())); - // if(Math.abs(currentAngle.minus(new Rotation2d(targetAngleRads)).getDegrees()) < .5){ - // steerMotor.set(0); - // return; - // } //System.out.print("target " + new Rotation2d(targetAngleRads).getDegrees() + "--------"); // System.out.print("error " + (angleRads.minus(currentAngle).getRadians()) + "--------"); @@ -196,18 +198,45 @@ public void setRawPowersWithAngle(double drivePower, double angleRads){ // System.out.println("2"); } + public void configureEncoder(double min, double max){ + minEncoder = min; + maxEncoder = max; + + } + + public double getMappedAngle(){ + //get range of new range + //get "fake" encoder value + offset + //fakevalue/fakerange = x/realrange + //realrange((fakevalue - min)/fakerange) = x + + return 360. * (steerAbsoluteEncoder.getPosition() - minEncoder) / ((double) (maxEncoder - minEncoder)); + } + /** * Gets the current angle of the module * @return Wrapped angle in radians from -pi to pi */ public Rotation2d getWrappedAngle(){ - double angleRads = steerAbsoluteEncoder.getPosition(); + double angleRads = getMappedAngle(); double wrappedAngleRads = MathUtil.angleModulus(angleRads + offsetRads); return new Rotation2d(wrappedAngleRads); } + public Rotation2d getRawAngle(){ + return new Rotation2d(steerAbsoluteEncoder.getPosition()); + } + public double twoDecimals(double num){ return ((int) (num * 100)) / 100.d; } + + public double getDriveAmpDraws(){ + return driveMotor.getAmpDraw(); + } + + public double getSteerAmpDraws(){ + return steerMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2f373dc3..9c2819ab 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -10,19 +10,29 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.robot.Constants.SwerveConstants.*; +import java.security.GeneralSecurityException; + import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; public class SwerveSubsystem extends BaseSwerveSubsystem{ private final AHRS ahrs; - public static final double MAX_VEL = 5; //STUB + private final Timer crimer; + + public static final double MAX_VEL = 4.90245766303; //calculated public static final double MAX_ACCEL = 3; //STUB public static final double MAX_OMEGA = MAX_VEL / FL_POS.getNorm(); public static final double MAX_ALPHA = 8; //STUB @@ -59,18 +69,44 @@ public class SwerveSubsystem extends BaseSwerveSubsystem{ new SwerveModuleState() }; + private final ShuffleboardTab choreoTab; + private final Field2d field; + + private final GenericEntry FLsteer, FLdrive, FRsteer, FRdrive, BLsteer, BLdrive, BRsteer, BRdrive; + public SwerveSubsystem() { ahrs = new AHRS(SPI.Port.kMXP); frontLeftModule = new SwerveModule(FL_DRIVE, FL_STEER, FL_OFFSET); frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET); backLeftModule = new SwerveModule(BL_DRIVE, BL_STEER, BL_OFFSET); - backRightModule = new SwerveModule(BR_DRIVE, BR_STEER); - + backRightModule = new SwerveModule(BR_DRIVE, BR_STEER, BR_OFFSET); + kinematics = new SwerveDriveKinematics(FL_POS, FR_POS, BL_POS, BR_POS); inst.startServer(); + crimer = new Timer(); + crimer.start(); + + choreoTab = Shuffleboard.getTab("Auton"); + field = new Field2d(); + // choreoTab.add("Field", field) + // .withPosition(0, 0) + // .withSize(3, 2); + + FLsteer = choreoTab.add("FLsteer", 0.).withPosition(0, 0).getEntry(); + FLdrive = choreoTab.add("FLdrive", 0.).withPosition(0, 1).getEntry(); + + FRsteer = choreoTab.add("FRsteer", 0.).withPosition(1, 0).getEntry(); + FRdrive = choreoTab.add("FRdrive", 0.).withPosition(1, 1).getEntry(); + + BLsteer = choreoTab.add("BLsteer", 0.).withPosition(2, 0).getEntry(); + BLdrive = choreoTab.add("BLdrive", 0.).withPosition(2, 1).getEntry(); + + BRsteer = choreoTab.add("BRsteer", 0.).withPosition(3, 0).getEntry(); + BRdrive = choreoTab.add("BRdrive", 0.).withPosition(3, 1).getEntry(); + poseEstimator = new SwerveDrivePoseEstimator( kinematics, getGyroHeading(), @@ -84,11 +120,31 @@ public SwerveSubsystem() { } public void periodic() { + + // if (crimer.advanceIfElapsed(.1)){ + // //System.out.println("BR : " + backRightModule.getRawAngle()); + // System.out.println("BL : " + backLeftModule.getRawAngle()); + // } + + FLsteer.setValue(frontLeftModule.getSteerAmpDraws()); + FLdrive.setValue(frontLeftModule.getDriveAmpDraws()); + + FRsteer.setValue(frontRightModule.getSteerAmpDraws()); + FRdrive.setValue(frontRightModule.getDriveAmpDraws()); + + BLsteer.setValue(backLeftModule.getSteerAmpDraws()); + BLdrive.setValue(backLeftModule.getDriveAmpDraws()); + + BRsteer.setValue(backRightModule.getSteerAmpDraws()); + BRdrive.setValue(backRightModule.getDriveAmpDraws()); + Rotation2d gyroAngle = getGyroHeading(); Pose2d estimate = poseEstimator.update( gyroAngle, getModulePositions() ); + + field.setRobotPose(estimate); for(int i = 0; i < 4; i++){ angles[i].set(states[i].angle.getRadians()); diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java index 9cffa720..79ba21bd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java @@ -10,6 +10,7 @@ public class FalconDriveMotor implements SwerveDriveMotor{ private double positionConversionFactor = 0; private double driveRotPerMinPerMetersPerSec = 0; private VelocityVoltage request = new VelocityVoltage(0).withSlot(0); + private double targetRps = 0; public FalconDriveMotor(int port) { motor = new TalonFX(port); @@ -17,11 +18,11 @@ public FalconDriveMotor(int port) { public void setVelocity(double metersPerSec){ - double targetRps = metersPerSec * driveRotPerMinPerMetersPerSec / 60; + targetRps = metersPerSec * driveRotPerMinPerMetersPerSec / 60; // System.out.println(motor.getClosedLoopTarget() + " err: " + motor.getClosedLoopError()); - motor.setControl(request.withVelocity(targetRps * .3)); //TODO: REMOVE LIMIT + motor.setControl(request.withVelocity(targetRps )); //TODO: REMOVE LIMIT } public void setPower(double power){ @@ -59,4 +60,12 @@ public double getError(){ return motor.getClosedLoopError().getValue(); } + public double getSetPoint(){ + return targetRps; + } + + public double getAmpDraw(){ + return motor.getSupplyCurrent().getValueAsDouble(); + } + } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java index eb26ec94..9631df0d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java @@ -17,4 +17,8 @@ public interface SwerveDriveMotor { public void setPositionConversionFactor(double factor); public double getError(); + + public double getSetPoint(); + + public double getAmpDraw(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java index f6e889e9..16fcca62 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java @@ -59,4 +59,12 @@ public void setPositionConversionFactor(double factor){ public double getError(){ return 0; //STUB } + + public double getSetPoint(){ + return 0; //STUB + } + + public double getAmpDraw(){ + return 0; //STUB + } } diff --git a/src/main/java/frc/robot/util/TrackingTimer.java b/src/main/java/frc/robot/util/TrackingTimer.java new file mode 100644 index 00000000..d3a65785 --- /dev/null +++ b/src/main/java/frc/robot/util/TrackingTimer.java @@ -0,0 +1,30 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.Timer; + +/** + * A wrapper around a `Timer` that tracks whether it is started. + */ +public class TrackingTimer extends Timer { + private boolean started = false; + + @Override + public void start() { + started = true; + super.start(); + } + + @Override + public void stop() { + started = false; + super.stop(); + } + + /** + * Gets whether the timer has started. + * @return Whether the timer has started. + */ + public boolean hasStarted() { + return started; + } +} diff --git a/src/main/thing.chor b/src/main/thing.chor new file mode 100644 index 00000000..a1def467 --- /dev/null +++ b/src/main/thing.chor @@ -0,0 +1,154 @@ +{ + "version": "v0.2", + "robotConfiguration": { + "mass": 29.635190801236774, + "rotationalInertia": 6, + "motorMaxTorque": 4.7, + "motorMaxVelocity": 6380, + "gearing": 6.92, + "wheelbase": 0.6921496262392018, + "trackWidth": 0.6921496262392018, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "NewPath": { + "waypoints": [ + { + "x": 0, + "y": 0, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 1.5, + "y": 1, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 3, + "y": 0.018946131691336632, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0, + "y": 0, + "heading": 1.050069725977771e-40, + "angularVelocity": 1.5398262290553578e-16, + "velocityX": 2.794710467944005, + "velocityY": 3.5303599919188637, + "timestamp": 0 + }, + { + "x": 0.23173749164767796, + "y": 0.29282094581619517, + "heading": 1.3440021243249235e-17, + "angularVelocity": 1.7652368051626978e-16, + "velocityX": 3.0436823126628485, + "velocityY": 3.8459634962062363, + "timestamp": 0.07613721401873688 + }, + { + "x": 0.49239298101355006, + "y": 0.5602252989065818, + "heading": -3.323421049691745e-18, + "angularVelocity": -2.2017409632498635e-16, + "velocityX": 3.423496547973215, + "velocityY": 3.5121373501399438, + "timestamp": 0.15227442803747376 + }, + { + "x": 0.7919875221555976, + "y": 0.7831366780255028, + "heading": 5.996654462713521e-18, + "angularVelocity": 1.224115648587833e-16, + "velocityX": 3.934929127683013, + "velocityY": 2.927758547388317, + "timestamp": 0.22841164205621065 + }, + { + "x": 1.1317406170413713, + "y": 0.9381018452824517, + "heading": 3.317621643457253e-17, + "angularVelocity": 3.5698130436912057e-16, + "velocityX": 4.462378867750975, + "velocityY": 2.0353406576828874, + "timestamp": 0.3045488560749475 + }, + { + "x": 1.5, + "y": 1, + "heading": -3.674138992979092e-38, + "angularVelocity": -4.357424534191873e-16, + "velocityX": 4.836785633740269, + "velocityY": 0.8129816084561599, + "timestamp": 0.3806860700936844 + }, + { + "x": 1.8687850971430546, + "y": 0.9472063385941725, + "heading": 2.0043532320076904e-17, + "angularVelocity": 2.6387750181910064e-16, + "velocityX": 4.855136739459322, + "velocityY": -0.6950401387793096, + "timestamp": 0.45664378600020644 + }, + { + "x": 2.210735226102118, + "y": 0.799355927973932, + "heading": -2.8324657515278217e-18, + "angularVelocity": -3.0116753508593537e-16, + "velocityX": 4.501848493976605, + "velocityY": -1.9464831038464674, + "timestamp": 0.5326015019067285 + }, + { + "x": 2.5122510777106877, + "y": 0.5805458963497601, + "heading": 1.603040244881554e-17, + "angularVelocity": 2.4833379961114195e-16, + "velocityX": 3.9695223587330672, + "velocityY": -2.8806820875516777, + "timestamp": 0.6085592178132506 + }, + { + "x": 2.7724292850840286, + "y": 0.31390580296723725, + "heading": 2.4391259489329175e-17, + "angularVelocity": 1.1007251786346634e-16, + "velocityX": 3.425303200180388, + "velocityY": -3.510375347660468, + "timestamp": 0.6845169337197727 + }, + { + "x": 3, + "y": 0.018946131691336632, + "heading": 0, + "angularVelocity": -3.2111628420777353e-16, + "velocityX": 2.996018405734816, + "velocityY": -3.8832088058149257, + "timestamp": 0.7604746496262949 + } + ], + "constraints": [], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + } +} \ No newline at end of file diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json new file mode 100644 index 00000000..e08fc121 --- /dev/null +++ b/vendordeps/ChoreoLib.json @@ -0,0 +1,43 @@ +{ + "fileName": "ChoreoLib.json", + "name": "ChoreoLib", + "version": "2024.1.0", + "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", + "frcYear": "2024", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", + "javaDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-java", + "version": "2024.1.0" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.10.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-cpp", + "version": "2024.1.0", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 00000000..88a68dd0 --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file