From c51ddba9900e1cb88a11029fa7e93cdbb59e73d7 Mon Sep 17 00:00:00 2001 From: William Yuan <66759156+Wi11iamYuan@users.noreply.github.com> Date: Sat, 5 Oct 2024 18:00:59 -0700 Subject: [PATCH] more integration shooter for now will not use PID, might have to add back for feeding @ diff distances right now there will only be two sensors --- src/main/java/frc/team3128/Constants.java | 42 +----- src/main/java/frc/team3128/Robot.java | 22 +-- .../java/frc/team3128/RobotContainer.java | 60 ++++++-- .../frc/team3128/commands/CmdManager.java | 131 +++++++++--------- .../java/frc/team3128/subsystems/Amper.java | 7 +- .../java/frc/team3128/subsystems/Climber.java | 116 ++++++++-------- .../java/frc/team3128/subsystems/Hopper.java | 37 +++-- .../java/frc/team3128/subsystems/Intake.java | 4 +- .../java/frc/team3128/subsystems/Shooter.java | 75 +++++----- vendordeps/3128-common.json | 4 +- 10 files changed, 266 insertions(+), 232 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 7eb6396..48a1909 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -187,42 +187,6 @@ public static class VisionConstants { public static final Matrix SVR_VISION_MEASUREMENT_STD = VecBuilder.fill(0.5,0.5,Units.degreesToRadians(5)); - public static final HashMap APRIL_TAG_POS = new HashMap(); - - static { - APRIL_TAG_POS.put(1, new Pose2d( - new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19)), - Rotation2d.fromDegrees(180)) - ); - APRIL_TAG_POS.put(2, new Pose2d( - new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19)), - Rotation2d.fromDegrees(180)) - ); - APRIL_TAG_POS.put(3, new Pose2d( - new Translation2d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19)), - Rotation2d.fromDegrees(180)) - ); - APRIL_TAG_POS.put(4, new Pose2d( - new Translation2d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74)), - Rotation2d.fromDegrees(180)) - ); - APRIL_TAG_POS.put(5, new Pose2d( - new Translation2d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74)), - Rotation2d.fromDegrees(0)) - ); - APRIL_TAG_POS.put(6, new Pose2d( - new Translation2d( Units.inchesToMeters(40.45), Units.inchesToMeters(174.19)), - Rotation2d.fromDegrees(0)) - ); - APRIL_TAG_POS.put(7, new Pose2d( - new Translation2d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19)), - Rotation2d.fromDegrees(0)) - ); - APRIL_TAG_POS.put(8, new Pose2d( - new Translation2d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19)), - Rotation2d.fromDegrees(0)) - ); - } } public static class FieldConstants{ @@ -342,7 +306,7 @@ public static class IntakeConstants { public static final double UNIT_CONV_FACTOR = GEAR_RATIO * 360; public static final int ROLLER_MOTOR_ID = 30; - public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID); + public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX); public static final double STALL_CURRENT = 50; public static final double STALL_POWER = .05; @@ -420,7 +384,7 @@ public enum Colors { } public static class ClimberConstants { public static final int CLIMB_MOTOR_ID = 50; - public static final NAR_CANSpark CLIMB_MOTOR = new NAR_CANSpark(CLIMB_MOTOR_ID); + // public static final NAR_CANSpark CLIMB_MOTOR = new NAR_CANSpark(CLIMB_MOTOR_ID); public static final PIDFFConfig PIDConstants = new PIDFFConfig(2, 0, 0, 0.18, 0, 0, 0.3);//240 public static final double MAX_VELOCTIY = 10000000; @@ -441,7 +405,7 @@ public static class ClimberConstants { public static class HopperConstants { public static final int HOPPER_MOTOR_ID = 40; public static final NAR_CANSpark HOPPER_MOTOR = new NAR_CANSpark(HOPPER_MOTOR_ID); - public static final int HOPPER_FRONT_SENSOR_ID = 1; + public static final int HOPPER_FRONT_SENSOR_ID = 0; public static final int HOPPER_BACK_SENSOR_ID = 2; public static final double STALL_CURRENT = 50; diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index a3c0a82..f43d993 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -91,9 +91,9 @@ public void driverStationConnected() { Log.info("State", "DS Connected"); Log.info("Alliance", getAlliance().toString()); if (getAlliance() == Alliance.Red) { - Camera.addTags(3, 4, 5, 11, 12); + Camera.addIgnoredTags(3, 4, 5, 11, 12); } else { - Camera.addTags(6, 7, 8, 15, 16); + Camera.addIgnoredTags(6, 7, 8, 15, 16); } if (!NAR_Robot.logWithAdvantageKit) return; if(DriverStation.getMatchType() != MatchType.None){ @@ -110,15 +110,15 @@ public void driverStationConnected() { public void robotPeriodic(){ Camera.updateAll(); // TODO: this may break everything - if (Hopper.getInstance().hasObjectPresent()) { - if (timer.hasElapsed(2.5)) { - // CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(Hopper.getInstance())); - CommandScheduler.getInstance().schedule(CmdManager.hopperOuttake()); - timer.reset(); - } - return; - } - timer.reset(); + // if (Hopper.getInstance().hasObjectPresent()) { + // if (timer.hasElapsed(2.5)) { + // // CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(Hopper.getInstance())); + // CommandScheduler.getInstance().schedule(CmdManager.hopperOuttake()); + // timer.reset(); + // } + // return; + // } + // timer.reset(); } @Override diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 734065d..e7d3690 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -38,8 +38,11 @@ import common.utility.shuffleboard.NAR_Shuffleboard; import common.utility.sysid.CmdSysId; import common.utility.tester.Tester; +import frc.team3128.subsystems.Hopper; +import frc.team3128.subsystems.Intake; // import common.utility.tester.Tester.UnitTest; import frc.team3128.subsystems.Leds; +import frc.team3128.subsystems.Shooter; import frc.team3128.subsystems.Swerve; import java.util.ArrayList; import edu.wpi.first.apriltag.AprilTagFields; @@ -77,7 +80,7 @@ public RobotContainer() { buttonPad = new NAR_ButtonBoard(3); //uncomment line below to enable driving - CommandScheduler.getInstance().setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); + CommandScheduler.getInstance().setDefaultCommand(Swerve.getInstance(), new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); initRobotTest(); @@ -86,8 +89,8 @@ public RobotContainer() { configureButtonBindings(); - NAR_Shuffleboard.addData("Limelight", "ValidTarget", ()-> limelight.hasValidTarget(), 0, 0); - NAR_Shuffleboard.addData("Limelight", "TX", ()-> limelight.getValue(LimelightKey.HORIZONTAL_OFFSET), 0, 1); + // NAR_Shuffleboard.addData("Limelight", "ValidTarget", ()-> limelight.hasValidTarget(), 0, 0); + // NAR_Shuffleboard.addData("Limelight", "TX", ()-> limelight.getValue(LimelightKey.HORIZONTAL_OFFSET), 0, 1); } private void configureButtonBindings() { @@ -106,22 +109,51 @@ private void configureButtonBindings() { controller.getLeftPOVButton().onTrue(runOnce(()-> { CmdSwerveDrive.setTurnSetpoint(Robot.getAlliance() == Alliance.Red ? 270 : 90); })); + + controller.getButton(XboxButton.kLeftBumper).onTrue(intake(Intake.Setpoint.GROUND)); + controller.getButton(XboxButton.kRightBumper).onTrue(retractIntake()); + + controller.getButton(XboxButton.kA).onTrue(Shooter.getInstance().runShooter(0.8)); + controller.getButton(XboxButton.kY).onTrue(Shooter.getInstance().runShooter(0)); + controller.getButton(XboxButton.kB).onTrue(Shooter.getInstance().runKickMotor(0.8)).onFalse(Shooter.getInstance().runKickMotor(0)); + + controller.getButton(XboxButton.kY).whileTrue(ampUp()).onFalse(ampFinAndDown()); + + + // new Trigger(()->true).onTrue(queueNote()); + + new Trigger(()-> Intake.getInstance().getMeasurement() > 90) + .and(()->!Hopper.getInstance().hasObjectPresent()) + .onTrue(Hopper.getInstance().runManipulator(0.8)) + .onFalse(Hopper.getInstance().runManipulator(0)); + + new Trigger(()-> Shooter.getInstance().noteInRollers()).negate() + .and(()->Hopper.getInstance().hasObjectPresent()) + .onTrue(sequence( + Shooter.getInstance().runKickMotor(0.8), + Hopper.getInstance().runManipulator(0.8) + )) + .onFalse(sequence( + Shooter.getInstance().runKickMotor(0), + Hopper.getInstance().runManipulator(0) + )); + } @SuppressWarnings("unused") public void initCameras() { Camera.disableAll(); - Camera.configCameras(AprilTagFields.k2024Crescendo, PoseStrategy.LOWEST_AMBIGUITY, (pose, time) -> swerve.addVisionMeasurement(pose, time), () -> swerve.getPose()); - Camera.setAmbiguityThreshold(0.3); - Camera.overrideThreshold = 30; - Camera.validDist = 0.5; + Camera.setResources(()-> swerve.getYaw(), (pose, time) -> swerve.addVisionMeasurement(pose, time), AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(), () -> swerve.getPose()); + Camera.setThresholds(5, 0.5); + // Camera.overrideThreshold = 30; + // Camera.validDist = 0.5; // Camera.addIgnoredTags(13.0, 14.0); if (Robot.isReal()) { - final Camera camera = new Camera("FRONT_LEFT", Units.inchesToMeters(10.055), Units.inchesToMeters(9.79), Units.degreesToRadians(30), Units.degreesToRadians(-28.125), 0); - final Camera camera2 = new Camera("FRONT_RIGHT", Units.inchesToMeters(10.055), -Units.inchesToMeters(9.79), Units.degreesToRadians(-30), Units.degreesToRadians(-28.125), 0); - camera.setCamDistanceThreshold(3.5); - camera2.setCamDistanceThreshold(5); + // final Camera camera = new Camera("FRONT_LEFT", Units.inchesToMeters(10.055), Units.inchesToMeters(9.79), Units.degreesToRadians(30), Units.degreesToRadians(-28.125), 0); + // final Camera camera2 = new Camera("FRONT_RIGHT", Units.inchesToMeters(10.055), -Units.inchesToMeters(9.79), Units.degreesToRadians(-30), Units.degreesToRadians(-28.125), 0); + // camera.setCamDistanceThreshold(3.5); + // camera2.setCamDistanceThreshold(5); } // final Camera camera3 = new Camera("LEFT", Units.inchesToMeters(-3.1), Units.inchesToMeters(12.635), Units.degreesToRadians(90), Units.degreesToRadians(-10), 0); // final Camera camera4 = new Camera("RIGHT", Units.inchesToMeters(-3.1), Units.inchesToMeters(-12.635), Units.degreesToRadians(-90), Units.degreesToRadians(0), 0); @@ -129,7 +161,7 @@ public void initCameras() { // sideCams.add(camera3); // sideCams.add(camera4); - limelight = new Limelight("limelight-mason", 0, 0, 0); + // limelight = new Limelight("limelight-mason", 0, 0, 0); } public static void toggleSideCams(boolean enable) { @@ -172,7 +204,7 @@ public boolean isConnected() { } private void initRobotTest() { - Tester tester = Tester.getInstance(); - tester.getTest("Robot").setTimeBetweenTests(0.5); + // Tester tester = Tester.getInstance(); + // tester.getTest("Robot").setTimeBetweenTests(0.5); } } diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 1e3c316..030d559 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -19,6 +19,7 @@ import frc.team3128.subsystems.Intake.Setpoint; import frc.team3128.subsystems.Shooter; import frc.team3128.subsystems.Swerve; +// import frc.team3128.subsystems.Climber; public class CmdManager { @@ -27,6 +28,7 @@ public class CmdManager { private static Shooter shooter = Shooter.getInstance(); private static Amper amper = Amper.getInstance(); private static Hopper hopper = Hopper.getInstance(); + //private static Climber climber = Climber.getInstance(); private static NAR_XboxController controller = RobotContainer.controller; @@ -34,20 +36,15 @@ public static Command vibrateController(){ return new ScheduleCommand(new StartEndCommand(()-> controller.startVibrate(), ()-> controller.stopVibrate()).withTimeout(1)); } - public static Command rampUpShoot() { - return shooter.shoot(SHOOTER_RPM); - } - public static Command kick() { return either( sequence( shooter.runKickMotor(KICK_POWER), - waitUntil(() -> shooter.noteInRollers()).withTimeout(0.3), - waitUntil(() -> !shooter.noteInRollers()), + waitUntil(() -> !shooter.noteInRollers()).withTimeout(0.3), shooter.runKickMotor(0) ), none(), - () -> shooter.noteInKick() + () -> shooter.noteInRollers() ); } @@ -55,10 +52,11 @@ public static Command kick(boolean once) { return sequence( kick(), either( - queueNote(), + // queueNote(), + none(), sequence( - queueNote(), - waitUntil(() -> shooter.atSetpoint()), + // queueNote(), + waitSeconds(0.3), kick() ), () -> once @@ -75,7 +73,7 @@ public static Command ramShoot(boolean once) { public static Command ramShootNoStop(boolean once) { return sequence( - rampUpShoot(), + shooter.rampUpShooter(), waitUntil(()->shooter.atSetpoint()), kick(once) ); @@ -91,88 +89,87 @@ public static Command ramShootNoStop(boolean once) { // ).andThen(ramShoot(once)).andThen(runOnce(() -> CmdSwerveDrive.disableTurn())); // } - public static Command ampUp(){ - return sequence( - amper.partExtend(), - amper.runRollers(), - shooter.runShooter(0.3) - ); - } - - public static Command ampFinAndDown(){ - return sequence( - amper.fullExtend(), - waitUntil(() -> amper.atSetpoint()), - shooter.runKickMotor(1), - waitSeconds(1), - amper.stopRollers(), - shooter.stopMotors(), - amper.retract() - ); - } - - // TODO: this - // public static Command autoAmp() { - - // } - public static Command intake(Intake.Setpoint setpoint) { return sequence( - intake.pivotTo(setpoint), + // intake.pivotTo(setpoint), intake.runIntakeRollers(), hopper.runManipulator(HOPPER_INTAKE_POWER), - waitUntil(() -> shooter.noteInKick()), - waitUntil(() -> hopper.noteInFront()) + waitUntil(()->hopper.hasObjectPresent()), + hopper.runManipulator(0) ); } - public static Command stopIntake() { + public static Command retractIntake() { return sequence( - intake.stopRollers(), - hopper.runManipulator(0), - intake.retract() + intake.stopRollers() + // hopper.runManipulator(0), + // intake.retract() ); } public static Command intakeAndStop(Intake.Setpoint setpoint) { return sequence( intake(setpoint), - stopIntake() + retractIntake() ); } - public static Command feed(double rpm, double angle, boolean once) { - return sequence( - parallel( - swerve.turnInPlace(() -> Robot.getAlliance() == Alliance.Blue ? 180-angle : angle).asProxy().withTimeout(1), - runOnce(() -> shooter.startPID(rpm)), - waitUntil(() -> shooter.atSetpoint()) - ), - kick(once), - shooter.stopMotors() - ); - } + // public static Command feed(double rpm, double angle, boolean once) { + // return sequence( + // parallel( + // swerve.turnInPlace(() -> Robot.getAlliance() == Alliance.Blue ? 180-angle : angle).asProxy().withTimeout(1), + // runOnce(() -> shooter.startPID(rpm)), + // waitUntil(() -> shooter.atSetpoint()) + // ), + // kick(once), + // shooter.stopMotors() + // ); + // } - public static Command queueNote() { - return either( - none(), - sequence( - hopper.runManipulator(HOPPER_INTAKE_POWER), - waitUntil(()-> shooter.noteInKick()), - hopper.runManipulator(0) - ), - () -> (!hopper.hasObjectPresent() || shooter.noteInKick()) - ); - } + // public static Command queueNote() { + // return either( + // none(), + // sequence( + // shooter.runKickMotor(0.5), + // hopper.runManipulator(HOPPER_INTAKE_POWER), + // waitUntil(()-> shooter.noteInRollers()), + // hopper.runManipulator(0), + // shooter.runKickMotor(0) + // ), + // () -> (!hopper.hasObjectPresent() || shooter.noteInRollers()), + // hopper.runManipulator(0).onlyIf(()-> hopper.hasObjectPresent()) + + // ); + // } public static Command hopperOuttake() { return sequence( intake.pivotTo(Setpoint.NEUTRAL), // TODO: will this slow things down - waitUntil(()->intake.atSetpoint()), + waitSeconds(0.3), hopper.runManipulator(HOPPER_OUTTAKE_POWER), waitSeconds(0.35), hopper.runManipulator(0) ); } + + public static Command ampUp(){ + return sequence( + amper.partExtend(), + amper.runRollers(), + shooter.runShooter(0.3) + ); + } + + public static Command ampFinAndDown(){ + return sequence( + amper.fullExtend(), + waitUntil(() -> amper.atSetpoint()), + shooter.runKickMotor(1), + waitSeconds(1), + amper.stopRollers(), + shooter.stopMotors(), + amper.retract() + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Amper.java b/src/main/java/frc/team3128/subsystems/Amper.java index e843902..b396114 100644 --- a/src/main/java/frc/team3128/subsystems/Amper.java +++ b/src/main/java/frc/team3128/subsystems/Amper.java @@ -16,6 +16,7 @@ public class Amper extends ElevatorTemplate { public enum Setpoint { + //TODO: correct EXTEND setpoints FULLEXTEND(21.25), PARTEXTEND(21.25*0.8), RETRACTED(0); @@ -37,17 +38,17 @@ public static synchronized Amper getInstance() { private Amper() { super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), ELEV_MOTOR); + //TODO: remove once done testing this.setSafetyThresh(100); // setkG_Function(() -> getMeasurement()*Math.sin(AMPER_ANGLE)); setTolerance(POSITION_TOLERANCE); setConstraints(MIN_SETPOINT, MAX_SETPOINT); - initShuffleboard(); + // initShuffleboard(); } @Override protected void configMotors() { - // TODO: figure unit conversion out ELEV_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR); ELEV_MOTOR.setCurrentLimit(CURRENT_LIMIT); ELEV_MOTOR.setInverted(true); @@ -63,9 +64,11 @@ public void setVoltage(double volts) { ELEV_MOTOR.set(0, Control.Position); ELEV_MOTOR.setVolts(volts); } + public double getVelocity() { return ELEV_MOTOR.getVelocity(); } + public double getPosition() { return ELEV_MOTOR.getPosition(); } diff --git a/src/main/java/frc/team3128/subsystems/Climber.java b/src/main/java/frc/team3128/subsystems/Climber.java index a54e219..db64f5b 100644 --- a/src/main/java/frc/team3128/subsystems/Climber.java +++ b/src/main/java/frc/team3128/subsystems/Climber.java @@ -1,54 +1,62 @@ -package frc.team3128.subsystems; - -import common.core.controllers.TrapController; -import common.core.subsystems.ElevatorTemplate; -import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; -import common.hardware.motorcontroller.NAR_Motor.Neutral; -import edu.wpi.first.wpilibj2.command.Command; - -import static frc.team3128.Constants.ClimberConstants.*; - -public class Climber extends ElevatorTemplate { - - public enum Setpoint { - EXTENDED(30), - RETRACTED(0); - - private double setpoint; - private Setpoint(double setpoint) { - this.setpoint = setpoint; - } - } - - private static Climber instance; - - public static synchronized Climber getInstance() { - if (instance == null) - instance = new Climber(); - return instance; - } - - private Climber() { - super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), CLIMB_MOTOR); - - // TODO: figure out kG function - // setkG_Function(()); - setTolerance(POSITION_TOLERANCE); - setConstraints(MIN_SETPOINT, MAX_SETPOINT); - initShuffleboard(); - } - - @Override - protected void configMotors() { - // TODO: figure unit conversion out - CLIMB_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR); - CLIMB_MOTOR.setCurrentLimit(CURRENT_LIMIT); - CLIMB_MOTOR.setNeutralMode(Neutral.BRAKE); - CLIMB_MOTOR.setStatusFrames(SparkMaxConfig.POSITION); - } - - public Command moveElevator(Setpoint setpoint) { - return moveElevator(setpoint.setpoint); - } - -} +// package frc.team3128.subsystems; + +// import common.core.controllers.TrapController; +// import common.core.subsystems.ElevatorTemplate; +// import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; +// import common.hardware.motorcontroller.NAR_Motor.Neutral; +// import edu.wpi.first.wpilibj2.command.Command; + +// import static frc.team3128.Constants.ClimberConstants.*; + +// public class Climber extends ElevatorTemplate { + +// public enum Setpoint { +// //TODO: correct EXTEND setpoint +// EXTENDED(30), +// RETRACTED(0); + +// private double setpoint; +// private Setpoint(double setpoint) { +// this.setpoint = setpoint; +// } +// } + +// private static Climber instance; + +// public static synchronized Climber getInstance() { +// if (instance == null) +// instance = new Climber(); +// return instance; +// } + +// private Climber() { +// super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), CLIMB_MOTOR); + +// // TODO: figure out kG function +// // setkG_Function(()); +// setTolerance(POSITION_TOLERANCE); +// setConstraints(MIN_SETPOINT, MAX_SETPOINT); +// initShuffleboard(); +// } + +// @Override +// protected void configMotors() { +// // TODO: figure unit conversion out +// CLIMB_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR); +// CLIMB_MOTOR.setCurrentLimit(CURRENT_LIMIT); + +// //TODO: set back to BRAKE ONCE PID DONE +// CLIMB_MOTOR.setNeutralMode(Neutral.COAST); + +// CLIMB_MOTOR.setStatusFrames(SparkMaxConfig.POSITION); +// } + +// public Command extendElevator() { +// return moveElevator(Setpoint.EXTENDED.setpoint); +// } + +// public Command retractElevator() { +// return moveElevator(Setpoint.RETRACTED.setpoint); +// } + +// } diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index 46f37e1..b5cc792 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -4,6 +4,7 @@ import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; import common.hardware.motorcontroller.NAR_Motor.Neutral; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; import static frc.team3128.Constants.HopperConstants.*; @@ -23,23 +24,25 @@ public static synchronized Hopper getInstance() { private Hopper() { super(STALL_CURRENT, HOPPER_INTAKE_POWER, HOPPER_OUTTAKE_POWER, STALL_POWER, 0.3, HOPPER_MOTOR); frontSensor = new DigitalInput(HOPPER_FRONT_SENSOR_ID); - backSensor = new DigitalInput(HOPPER_BACK_SENSOR_ID); + // backSensor = new DigitalInput(HOPPER_BACK_SENSOR_ID); configMotors(); - initShuffleboard(); + // initShuffleboard(); } @Override protected void configMotors() { HOPPER_MOTOR.enableVoltageCompensation(VOLT_COMP); - HOPPER_MOTOR.setNeutralMode(Neutral.BRAKE); HOPPER_MOTOR.setCurrentLimit(CURRENT_LIMIT); + + HOPPER_MOTOR.setNeutralMode(Neutral.COAST); + HOPPER_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY); - HOPPER_MOTOR.setNeutralMode(Neutral.BRAKE); } @Override public boolean hasObjectPresent() { - return noteInFront() || noteInBack(); + return noteInFront(); + // return noteInFront() || noteInBack(); } // shooter side @@ -47,8 +50,24 @@ public boolean noteInFront() { return !frontSensor.get(); } - // intake side - public boolean noteInBack() { - return !backSensor.get(); - } + // public Command runHopper(double power){ + // return runOnce(() -> HOPPER_MOTOR.set(power)); + // } + + // public Command intakeHopper(){ + // return runHopper(HOPPER_INTAKE_POWER); + // } + + // public Command outtakeHopper(){ + // return runHopper(HOPPER_OUTTAKE_POWER); + // } + + // public Command stopHopper(){ + // return runHopper(0); + // } + + // // intake side + // public boolean noteInBack() { + // return !backSensor.get(); + // } } diff --git a/src/main/java/frc/team3128/subsystems/Intake.java b/src/main/java/frc/team3128/subsystems/Intake.java index 23a3a4a..85745d3 100644 --- a/src/main/java/frc/team3128/subsystems/Intake.java +++ b/src/main/java/frc/team3128/subsystems/Intake.java @@ -38,11 +38,13 @@ private Intake() { super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), PIVOT_MOTOR); setkG_Function(()-> Math.cos(Units.degreesToRadians(getSetpoint()))); + setTolerance(ANGLE_TOLERANCE); setConstraints(MIN_SETPOINT, MAX_SETPOINT); setSafetyThresh(5); - initShuffleboard(); + // initShuffleboard(); + //TODO: remove once done testing this.setSafetyThresh(1000); } diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index 3b9c29d..36c6ef0 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -7,6 +7,7 @@ import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; import common.hardware.motorcontroller.NAR_Motor.Neutral; import common.utility.narwhaldashboard.NarwhalDashboard.State; +import common.utility.shuffleboard.NAR_Shuffleboard; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj2.command.Command; @@ -24,13 +25,16 @@ public class Shooter extends ShooterTemplate { private DigitalInput kickSensor, rollersSensor; private Shooter() { - super(new Controller(PIDConstants, Type.VELOCITY)); + super(new Controller(PIDConstants, Type.VELOCITY), SHOOTER_MOTOR); setConstraints(MIN_RPM, MAX_RPM); setTolerance(TOLERANCE); - kickSensor = new DigitalInput(KICK_SENSOR_ID); + + // kickSensor = new DigitalInput(KICK_SENSOR_ID); rollersSensor = new DigitalInput(ROLLERS_SENSOR_ID); + // NAR_Shuffleboard.addData + configMotors(); - initShuffleboard(); + // initShuffleboard(); ControllerBase controller = getController(); controller.setkS(()-> controller.getkS()); @@ -62,28 +66,33 @@ protected void configMotors() { KICK_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY); } - @Override - public void startPID(double setpoint) { - enable(); - getController().setSetpoint(setpoint); - } - - @Override - protected void useOutput(double output, double setpoint) { - SHOOTER_MOTOR.setVolts(setpoint != 0 ? output + kF_Func.getAsDouble() : 0); - } - - @Override - public double getMeasurement() { - return SHOOTER_MOTOR.getVelocity(); - } - - public Command shoot(double setpoint) { - return runOnce(()-> startPID(setpoint)); + //NOT USING PID FOR NOW + // @Override + // public void startPID(double setpoint) { + // enable(); + // getController().setSetpoint(setpoint); + // } + + // @Override + // protected void useOutput(double output, double setpoint) { + // SHOOTER_MOTOR.setVolts(setpoint != 0 ? output + kF_Func.getAsDouble() : 0); + // } + + // @Override + // public double getMeasurement() { + // return SHOOTER_MOTOR.getVelocity(); + // } + + // public Command shoot(double setpoint) { + // return runOnce(()-> startPID(setpoint)); + // } + + public Command rampUpShooter(){ + return runShooter(SHOOTER_RPM); } public Command runShooter(double power) { - return runOnce(()-> setPower(power)); + return runOnce(()-> SHOOTER_MOTOR.set(power)); } public Command runKickMotor(double power) { @@ -97,17 +106,17 @@ public Command stopMotors(){ }); } - public State getRunningState() { - if (SHOOTER_MOTOR.getState() != State.DISCONNECTED && KICK_MOTOR.getState() != State.DISCONNECTED) - return State.RUNNING; - if (SHOOTER_MOTOR.getState() != State.DISCONNECTED || KICK_MOTOR.getState() != State.DISCONNECTED) - return State.PARTIALLY_RUNNING; - return State.DISCONNECTED; - } - - public boolean noteInKick() { - return !kickSensor.get(); - } + // public State getRunningState() { + // if (SHOOTER_MOTOR.getState() != State.DISCONNECTED && KICK_MOTOR.getState() != State.DISCONNECTED) + // return State.RUNNING; + // if (SHOOTER_MOTOR.getState() != State.DISCONNECTED || KICK_MOTOR.getState() != State.DISCONNECTED) + // return State.PARTIALLY_RUNNING; + // return State.DISCONNECTED; + // } + + // public boolean noteInKick() { + // return !kickSensor.get(); + // } public boolean noteInRollers() { return !rollersSensor.get(); diff --git a/vendordeps/3128-common.json b/vendordeps/3128-common.json index 580d624..ed3d581 100644 --- a/vendordeps/3128-common.json +++ b/vendordeps/3128-common.json @@ -1,6 +1,6 @@ { "name": "3128-common", - "version": "1.8.2", + "version": "1.8.5", "uuid": "ae3fa5a2-78d9-47e8-921a-dba45b889445", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.github.Team3128", "artifactId": "3128-common", - "version": "1.8.2" + "version": "1.8.5" } ], "jniDependencies": [],