From 21c0421a88b093b6db82e2f25b5d469e77d93442 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sun, 8 Mar 2026 19:20:07 -0400 Subject: [PATCH] end of pine tree --- .../deploy/pathplanner/autos/Left Shoot.auto | 37 +++++++++++++ .../paths/start to score left.path | 54 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 40 ++++++++++---- .../constants/IntakeRollerConstants.java | 2 +- .../frc/robot/subsystems/IntakeRoller.java | 2 +- 5 files changed, 122 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Left Shoot.auto create mode 100644 src/main/deploy/pathplanner/paths/start to score left.path diff --git a/src/main/deploy/pathplanner/autos/Left Shoot.auto b/src/main/deploy/pathplanner/autos/Left Shoot.auto new file mode 100644 index 0000000..89e9dc7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Shoot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake down" + } + }, + { + "type": "named", + "data": { + "name": "spinup" + } + }, + { + "type": "path", + "data": { + "pathName": "start to score left" + } + }, + { + "type": "named", + "data": { + "name": "shoot close" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/start to score left.path b/src/main/deploy/pathplanner/paths/start to score left.path new file mode 100644 index 0000000..0ce67c5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/start to score left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.697342823250297, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.922680901542112, + "y": 5.890960854092527 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9872360616844604, + "y": 4.599857651245552 + }, + "prevControl": { + "x": 3.1809015421115063, + "y": 5.116298932384343 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -22.619864948040433 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a2c886..b7232f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { - private PhotonVision vision; + // private PhotonVision vision; private Drivetrain drivetrain; private Hood hood; private Shooter shooter; @@ -60,7 +60,7 @@ public class RobotContainer { private Timer shiftTimer; public RobotContainer() { - vision = new PhotonVision(); + //vision = new PhotonVision(); drivetrain = new Drivetrain(null); hood = new Hood(); shooter = new Shooter(); @@ -68,8 +68,9 @@ public class RobotContainer { intakeRoller = new IntakeRoller(); spindexer = new Spindexer(); //climber = new Climber(); + configureNamedCommands(); - + /* vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { Logger.recordOutput( @@ -77,6 +78,7 @@ public class RobotContainer { vp.visualPose() ); }); + */ driver = new CommandXboxController(OIConstants.kDriverControllerPort); secondary = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -91,7 +93,7 @@ public class RobotContainer { if(AutoConstants.kAutoConfigOk) { autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - configureNamedCommands(); + } } @@ -268,7 +270,7 @@ public class RobotContainer { driver.rightTrigger().whileTrue(spindexer.spinToShooter()); driver.b().whileTrue(spindexer.spinToIntake()); - driver.b().whileTrue( + /* driver.b().whileTrue( drivetrain.lockToYaw( () -> { OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID()); @@ -278,13 +280,17 @@ public class RobotContainer { driver::getLeftY, driver::getLeftX ) - ); + );*/ secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)); secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed)); secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40))); + //40 good for feeding secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30))); + //30 degrees good for shooter far near outpost + secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10))); + //10 degrees good for shooting ~33in away from hub /* hood.setDefaultCommand(hood.trackToAngle(() -> { @@ -324,14 +330,26 @@ public class RobotContainer { false // TODO Should this be true by default? ) ); + + NamedCommands.registerCommand( + "intake down", + intakePivot.manualSpeed(()->-0.75) + .withTimeout(1) + ); + + NamedCommands.registerCommand("spinup", + shooter.maintainSpeed(ShooterSpeeds.kHubSpeed) + .withTimeout(3)); + + NamedCommands.registerCommand("shoot close", + spindexer.spinToShooter() + .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)) + .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10))) + .withTimeout(3)); } public Command getAutonomousCommand() { - if(AutoConstants.kAutoConfigOk) { - return autoChooser.getSelected(); - } else { - return new PrintCommand("Robot Config loading failed, autonomous disabled"); - } + return autoChooser.getSelected(); } /** diff --git a/src/main/java/frc/robot/constants/IntakeRollerConstants.java b/src/main/java/frc/robot/constants/IntakeRollerConstants.java index 94bf7df..3558a1a 100644 --- a/src/main/java/frc/robot/constants/IntakeRollerConstants.java +++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java @@ -7,7 +7,7 @@ public class IntakeRollerConstants { // TODO Real values public static final int kMotorCANID = 20; - public static final int kCurrentLimit = 40; + public static final int kCurrentLimit = 65; public static final boolean kInvertMotors = true; diff --git a/src/main/java/frc/robot/subsystems/IntakeRoller.java b/src/main/java/frc/robot/subsystems/IntakeRoller.java index e31f84e..509c185 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRoller.java +++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java @@ -24,7 +24,7 @@ public class IntakeRoller extends SubsystemBase { public Command runIn() { return run(() -> { - motor.set(IntakeRollerConstants.kSpeed); + motor.set(IntakeRollerConstants.kSpeed*0.8); }); }