From 0a2559bb4b6b4934de13bd2653b92afb436b0d08 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 4 Apr 2026 10:27:44 -0600 Subject: [PATCH] Adding trench autos and linear model for roller --- .../pathplanner/autos/DriveShoot-Hori.auto | 2 +- .../pathplanner/autos/ShootPreloadBump.auto | 2 +- .../autos/X. LeftTrench-Neutral-Shoot.auto | 55 ++++++ .../autos/X. RightTrench-Neutral-Shoot.auto | 49 ++++++ .../paths/LeftTrench-Neutral-Shoot.path | 152 ++++++++++++++++ .../paths/RightTrench-Neutral-Shoot.path | 165 ++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/subsystems/intake/Intake.java | 14 +- .../subsystems/intake/IntakeConstants.java | 10 +- 9 files changed, 443 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto create mode 100644 src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto create mode 100644 src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path diff --git a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto index a537fa2..8765ce9 100644 --- a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto +++ b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto @@ -39,6 +39,6 @@ } }, "resetOdom": true, - "folder": "Spring Break Tests", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto index bc6176b..43867c9 100644 --- a/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto +++ b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto @@ -38,6 +38,6 @@ } }, "resetOdom": true, - "folder": "Spring Break Tests", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto new file mode 100644 index 0000000..3230788 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "path", + "data": { + "pathName": "LeftTrench-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto new file mode 100644 index 0000000..1ca254e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "RightTrench-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path new file mode 100644 index 0000000..679eeb1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path @@ -0,0 +1,152 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5373205128205143, + "y": 7.430435897435898 + }, + "prevControl": null, + "nextControl": { + "x": 3.7870116206619695, + "y": 7.418012103131955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.642076923076923, + "y": 7.069961538461539 + }, + "prevControl": { + "x": 7.178228149431453, + "y": 7.5205380189223625 + }, + "nextControl": { + "x": 8.216115793872147, + "y": 6.51234787481454 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.781615384615385, + "y": 4.837346153846154 + }, + "prevControl": { + "x": 7.786585532087434, + "y": 5.241663699788379 + }, + "nextControl": { + "x": 7.778542446002628, + "y": 4.5873650404629975 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572282051282052, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 6.957648225000392, + "y": 5.601152104386376 + }, + "nextControl": { + "x": 6.204516600433754, + "y": 5.426493771942126 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 3.7826390804558825, + "y": 5.585258005415613 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": 112.5 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.374307862679942, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.9302325581395282, + "maxWaypointRelativePos": 2.1705426356589252, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 7.0, + "y": 0.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 2.4983388704318887, + "maxWaypointRelativePos": 3.2956810631229274, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.7353266888150656, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 101.70667085951649 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 3.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path new file mode 100644 index 0000000..0c80fd9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path @@ -0,0 +1,165 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6070897435897447, + "y": 0.6046794871794885 + }, + "prevControl": null, + "nextControl": { + "x": 3.8833980386504074, + "y": 0.5273348158056345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.595564102564104, + "y": 0.9884102564102579 + }, + "prevControl": { + "x": 7.151835398129718, + "y": 0.5429530943613806 + }, + "nextControl": { + "x": 7.93843114627953, + "y": 1.332612871042205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.793243589743589, + "y": 3.4070769230769233 + }, + "prevControl": { + "x": 7.74538518514461, + "y": 3.9526634447090423 + }, + "nextControl": { + "x": 7.8344917146104915, + "y": 2.936847688346859 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572282051282052, + "y": 2.5119999999999996 + }, + "prevControl": { + "x": 7.050706524183416, + "y": 2.443033938208306 + }, + "nextControl": { + "x": 6.24953920807272, + "y": 2.5585241728388555 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 2.512 + }, + "prevControl": { + "x": 3.841498071206661, + "y": 2.6514755211571583 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.2059800664451794, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.771871539313429, + "maxWaypointRelativePos": 2.542635658914713, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 6.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.859357696566985, + "maxWaypointRelativePos": 2.1705426356589252, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 2.4983388704318887, + "maxWaypointRelativePos": 3.2956810631229274, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.6 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6910299003322272, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 3.0, + "rotation": -113.07 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.3, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ 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 6550504..f3c2f23 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -81,7 +81,7 @@ public class RobotContainer { //Testing of Colors public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive); public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); @@ -109,7 +109,7 @@ public class RobotContainer { private Command IntakeExtended = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME), m_robotIntake) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake) ); // private Command LidarIntake = new SequentialCommandGroup( @@ -146,7 +146,7 @@ public class RobotContainer { ); private Command IntakeRetracted = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RetractedREMOVEME), m_robotIntake) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake) ); private Command RobotShoot = new SequentialCommandGroup( diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 5ff8b7e..545b7d2 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -2,7 +2,6 @@ package frc4388.robot.subsystems.intake; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.function.Supplier; @@ -11,19 +10,23 @@ import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class Intake extends SubsystemBase { public IntakeIO io; IntakeStateAutoLogged state = new IntakeStateAutoLogged(); - + SwerveDrive m_SwerveDrive; Supplier m_swervePoseSupplier; public Intake( - IntakeIO io + IntakeIO io, + SwerveDrive m_SwerveDrive // Supplier swervePoseSupplier ) { this.io = io; + this.m_SwerveDrive = m_SwerveDrive; // this.m_swervePoseSupplier = swervePoseSupplier; } @@ -92,8 +95,9 @@ 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; - + double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); Logger.processInputs("Intake", state); Logger.recordOutput("Intake/IntakeState", this.mode); @@ -127,7 +131,7 @@ public class Intake extends SubsystemBase { case ExtendingRolling: io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, IntakeConstants. ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed)); break; case Retracting: diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index a5f7c02..79362e7 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -6,6 +6,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.units.measure.AngularVelocity; + import com.revrobotics.spark.config.LimitSwitchConfig.Behavior; import com.revrobotics.spark.config.LimitSwitchConfig.Type; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -59,8 +62,9 @@ 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", .80); + public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50); public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40); + public static final ConfigurableDouble ROLLER_MULTIPLIER_CONST = new ConfigurableDouble("Roller Multiplier Constant", 0.4); // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); @@ -79,7 +83,9 @@ public class IntakeConstants { // public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); // public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05); - + public static double getTargetRollerSpeed(double chassisSpeed) { + return ROLLER_PERCENT_OUTPUT.get() * (1 + ROLLER_MULTIPLIER_CONST.get() * chassisSpeed); + } // 0 is paralell to the ground, 90 is directly up