From f9b9a7dd30b53213e360f67c5b7bcd4567fa30f3 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Sat, 4 Apr 2026 20:41:00 -0600 Subject: [PATCH] highlanders changes --- .../deploy/pathplanner/autos/X. Stupid.auto | 19 +++++++ .../RightTrench-Neutral-Trench-Shoot.path | 38 ++++++------- .../paths/ShotRight-PlayerStation.path | 47 ++++------------ .../deploy/pathplanner/paths/Stupidauto.path | 54 +++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 12 +++-- .../robot/constants/BuildConstants.java | 10 ++-- .../robot/subsystems/intake/Intake.java | 19 ++++--- .../subsystems/intake/IntakeConstants.java | 1 + .../robot/subsystems/intake/IntakeReal.java | 12 +++-- 9 files changed, 138 insertions(+), 74 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/X. Stupid.auto create mode 100644 src/main/deploy/pathplanner/paths/Stupidauto.path diff --git a/src/main/deploy/pathplanner/autos/X. Stupid.auto b/src/main/deploy/pathplanner/autos/X. Stupid.auto new file mode 100644 index 0000000..4991348 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. Stupid.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Stupidauto" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path index ff04591..6caa935 100644 --- a/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path +++ b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path @@ -64,16 +64,16 @@ }, { "anchor": { - "x": 3.711743589743591, - "y": 0.6046794871794885 + "x": 3.3990740740740737, + "y": 0.7403418803418822 }, "prevControl": { - "x": 3.995405188035349, - "y": 0.5680757693806849 + "x": 3.7877883597883595, + "y": 0.6506385836385846 }, "nextControl": { - "x": 3.4637993695130525, - "y": 0.6366742313458394 + "x": 2.947066329127951, + "y": 0.8446513599448344 }, "isLocked": false, "linkedName": null @@ -89,7 +89,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "end of RightTrench-Neutral-Trench-Shoot" } ], "rotationTargets": [], @@ -99,7 +99,7 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 5.0, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, @@ -121,22 +121,22 @@ }, { "fieldPosition": { - "x": 0.0, + "x": 180.0, "y": 0.6 }, - "rotationOffset": 180.0, - "minWaypointRelativePos": 0.0, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.15384615384614939, "maxWaypointRelativePos": 0.6910299003322272, "name": "Point Towards Zone" }, { "fieldPosition": { - "x": 0.0, + "x": 180.0, "y": 0.7 }, "rotationOffset": 180.0, - "minWaypointRelativePos": 2.5913621262458837, - "maxWaypointRelativePos": 4.352159468438559, + "minWaypointRelativePos": 2.952853598014871, + "maxWaypointRelativePos": 4.105334233625949, "name": "Point Towards Zone" }, { @@ -145,7 +145,7 @@ "y": 4.021 }, "rotationOffset": 180.0, - "minWaypointRelativePos": 4.529346622369888, + "minWaypointRelativePos": 4.280891289669132, "maxWaypointRelativePos": 5.0, "name": "Point Towards Zone" } @@ -157,16 +157,16 @@ "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { - "velocity": 3.0, - "rotation": -145.9260531715447 + "velocity": 0.0, + "rotation": -136.00851404351897 }, "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0.3, + "velocity": 0.6, "rotation": 0.0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/ShotRight-PlayerStation.path b/src/main/deploy/pathplanner/paths/ShotRight-PlayerStation.path index 96e1e1b..1d97cf3 100644 --- a/src/main/deploy/pathplanner/paths/ShotRight-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/ShotRight-PlayerStation.path @@ -3,32 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.444, + "x": 2.4442692307692324, "y": 1.302 }, "prevControl": null, "nextControl": { - "x": 2.2968028935074876, - "y": 1.0999282012743195 + "x": 1.7698333333333336, + "y": 1.1628333333333343 }, "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5754761904761914, - "y": 1.0 - }, - "prevControl": { - "x": 2.2272563574654534, - "y": 0.9727386663284083 - }, - "nextControl": { - "x": 0.9236960234869294, - "y": 1.0272613336715914 - }, - "isLocked": false, - "linkedName": null + "linkedName": "end of RightTrench-Neutral-Trench-Shoot" }, { "anchor": { @@ -49,9 +33,9 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 2.0, + "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, @@ -60,18 +44,7 @@ } } ], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 0.0, - "y": 0.7 - }, - "rotationOffset": 180.0, - "minWaypointRelativePos": 0.6, - "maxWaypointRelativePos": 1.0956834532374118, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -83,13 +56,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -95.0 + "rotation": -90.16147282505071 }, "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0, - "rotation": -149.42767342080234 + "velocity": 0.0, + "rotation": -136.00851404351897 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupidauto.path b/src/main/deploy/pathplanner/paths/Stupidauto.path new file mode 100644 index 0000000..2b34f3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Stupidauto.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 0.5365714285714285 + }, + "prevControl": null, + "nextControl": { + "x": 3.119535714285715, + "y": 0.7633214285714292 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.547261904761905, + "y": 3.160392857142857 + }, + "prevControl": { + "x": 2.320511904761905, + "y": 2.696095238095238 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -159.67686317033716 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 177.94339863785623 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fa57d0b..cd7e487 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -158,9 +158,9 @@ public class RobotContainer { //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(5), + new WaitCommand(4), IntakeRetracted, - new WaitCommand(10), + new WaitCommand(7), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) ); @@ -375,7 +375,13 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.ExtendingRolling); })); - + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.LabubuGrowl); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.ExtendingRolling); + })); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Retracting); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 6d9cb93..a3a13fe 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 216; - public static final String GIT_SHA = "8fb8d8d43669f24867bc94d6e4175a7325ec72c8"; - public static final String GIT_DATE = "2026-04-04 13:03:52 MDT"; + public static final int GIT_REVISION = 218; + public static final String GIT_SHA = "74009b86bbde67d814d16020ce4cec00b8267411"; + public static final String GIT_DATE = "2026-04-04 16:17:27 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-04-04 15:23:52 MDT"; - public static final long BUILD_UNIX_TIME = 1775337832371L; + public static final String BUILD_DATE = "2026-04-04 19:50:19 MDT"; + public static final long BUILD_UNIX_TIME = 1775353819651L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 0373c08..e6f7852 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -42,7 +42,8 @@ public class Intake extends SubsystemBase { Idle, RectractTorque, Bouncing, - ExpelBalls + ExpelBalls, + LabubuGrowl } private boolean overCompressed = false; @@ -101,20 +102,20 @@ public class Intake extends SubsystemBase { public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter // System.out.println(m_armLimitSwitch.get()); - ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; + // ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; - double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + // double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); Logger.processInputs("Intake", state); Logger.recordOutput("Intake/IntakeState", this.mode); io.updateInputs(state); - overCompressed = - Math.abs(state.armMotorCurrent.in(Amps)) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get(); - // Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get(); + // overCompressed = + // Math.abs(state.armMotorCurrent.in(Amps)) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get(); + // // Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get(); - Logger.recordOutput("overCompressed", overCompressed); + // Logger.recordOutput("overCompressed", overCompressed); // getCurrentTime @@ -196,6 +197,10 @@ public class Intake extends SubsystemBase { io.armOutput(0); io.setRollerOutput(state, IntakeConstants.ROLLER_EJECT_PERCENT_OUTPUT.get()); break; + case LabubuGrowl: + io.armOutput(0); + io.setRollerOutput(state, IntakeConstants.ROLLER_LABUBU_GROWL_PERCENT_OUTPUT.get()); + break; } // if (state.retractedLimit){ // this.mode = IntakeMode.Retracted; diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 4832c58..0d8e46c 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -63,6 +63,7 @@ public class IntakeConstants { public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17); public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50); + public static final ConfigurableDouble ROLLER_LABUBU_GROWL_PERCENT_OUTPUT = new ConfigurableDouble("Roller Labubu Growl Percent Output", .75); public static final ConfigurableDouble ROLLER_EJECT_PERCENT_OUTPUT = new ConfigurableDouble("Roller eject Percent Output", -.20); public static final ConfigurableDouble ROLLER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Roller IDLE Percent Output", .20); public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 77206ba..4ce5ce0 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -7,6 +7,8 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkMax; import edu.wpi.first.units.measure.Angle; @@ -15,6 +17,8 @@ import frc4388.utility.compute.JankCoder; public class IntakeReal implements IntakeIO { SparkMax m_armMotor; + RelativeEncoder arm_encoder; + SparkLimitSwitch reverse_limit; TalonFX m_rollerMotor; JankCoder m_encoder; @@ -26,6 +30,8 @@ public class IntakeReal implements IntakeIO { // m_angleMotor = angleMotor; // m_pitchMotor = pitchMotor; m_armMotor = armMotor; + arm_encoder = m_armMotor.getEncoder(); + reverse_limit = m_armMotor.getReverseLimitSwitch(); m_rollerMotor = rollerMotor; m_encoder = jankCoder; @@ -96,8 +102,8 @@ public class IntakeReal implements IntakeIO { m_encoder.update(); - state.armAngle = Rotations.of(m_armMotor.getEncoder().getPosition()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); - state.armMotorVelocity = RotationsPerSecond.of(m_armMotor.getEncoder().getVelocity()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armAngle = Rotations.of(arm_encoder.getPosition()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorVelocity = RotationsPerSecond.of(arm_encoder.getVelocity()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); // state.armMotorAcceleration = RotationsPerSecondPerSecond.of(m_armMotor.getEncoder().ge); state.armMotorCurrent = Amps.of(m_armMotor.getOutputCurrent()); @@ -110,7 +116,7 @@ public class IntakeReal implements IntakeIO { state.intakeEncoder = m_encoder.getRotations(); state.encoderConnected = m_encoder.isConnected(); - state.retractedLimitSwitch = m_armMotor.getReverseLimitSwitch().isPressed(); + state.retractedLimitSwitch = reverse_limit.isPressed(); if(state.retractedLimitSwitch) { m_encoder.resetRotations();