From 8378e2f874f5d4218cd3b0ddfef477a7e1bfb62a Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Fri, 27 Feb 2026 19:42:56 -0800 Subject: [PATCH] a --- .../autos/Left(No SOTM) - Under Trench.auto | 50 +++++++++++++++++ .../paths/#1 Left under the trench.path | 27 +++++++++- .../#2 Left(No SOTM) under the trench.path | 27 +++++++++- .../#3(No SOTM) Left under the trench.path | 53 ++++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 14 +++-- .../drive_comm/DefaultDriveCommand.java | 4 +- .../frc/robot/constants/IntakeConstants.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 22 +++++--- .../subsystems/turret/TurretConstants.java | 3 +- 9 files changed, 185 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto b/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto index 4ef131d..db0b7a0 100644 --- a/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto +++ b/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto @@ -16,11 +16,61 @@ "pathName": "#2 Left(No SOTM) under the trench" } }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 8.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + ] + } + }, { "type": "path", "data": { "pathName": "#3(No SOTM) Left under the trench" } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 15.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/paths/#1 Left under the trench.path b/src/main/deploy/pathplanner/paths/#1 Left under the trench.path index a5b24f8..587ab10 100644 --- a/src/main/deploy/pathplanner/paths/#1 Left under the trench.path +++ b/src/main/deploy/pathplanner/paths/#1 Left under the trench.path @@ -95,7 +95,32 @@ } ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path index 2d8b84a..0ae7192 100644 --- a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path +++ b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path @@ -90,7 +90,32 @@ } ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Retract Intake", + "waypointRelativePos": 0.65466816647919, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Retract Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop Intake Rollers" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path b/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path index dab8783..53e7d77 100644 --- a/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path +++ b/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path @@ -45,7 +45,58 @@ } ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Getting Ready", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Stop Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Stop Spindexer" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + } + ] + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5dcfa84..b22ab22 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.commands.DoNothing; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.commands.gpm.ClimbDriveCommand; +import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.Superstructure; import frc.robot.commands.vision.ShutdownAllPis; import frc.robot.constants.AutoConstants; @@ -147,7 +148,7 @@ public class RobotContainer { e.printStackTrace(); } if(turret != null){ - turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer)); + //turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer)); } drive.setDefaultCommand(new DefaultDriveCommand(drive, driver)); break; @@ -192,8 +193,8 @@ public class RobotContainer { public void registerCommands() { if (intake != null){ - NamedCommands.registerCommand("Extend intake", new InstantCommand(()-> intake.extend())); - NamedCommands.registerCommand("Retract intake", new InstantCommand(()-> intake.retract())); + NamedCommands.registerCommand("Extend Intake", new InstantCommand(()-> intake.extend())); + NamedCommands.registerCommand("Retract Intake", new InstantCommand(()-> intake.retract())); } if (intake != null && spindexer != null){ @@ -203,11 +204,16 @@ public class RobotContainer { NamedCommands.registerCommand("Stop Intake Rollers", new ParallelCommandGroup( new InstantCommand(()->intake.spinStop()) )); + Command intakeMovement = new IntakeMovementCommand(intake); + NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(()-> intakeMovement.schedule())); + NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(()-> intakeMovement.cancel())); + + } if (turret != null && drive != null && hood != null && shooter != null && spindexer != null){ NamedCommands.registerCommand("Auto shoot", new AutoShootCommand(turret, drive, hood, shooter, spindexer)); - NamedCommands.registerCommand("Spin Spindexer", new InstantCommand(()-> spindexer.maxSpindexer(), spindexer)); + NamedCommands.registerCommand("Start Spindexer", new InstantCommand(()-> spindexer.maxSpindexer(), spindexer)); NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(()-> spindexer.stopSpindexer())); } diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index c481e0d..daa9b1e 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -12,7 +12,7 @@ import frc.robot.Robot; import frc.robot.constants.swerve.DriveConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.TrenchAssist.TrenchAssist2; +import frc.robot.util.TrenchAssist.TrenchAssist; import frc.robot.util.TrenchAssist.TrenchAssistConstants; import frc.robot.util.Vision.DriverAssist; @@ -88,7 +88,7 @@ public class DefaultDriveCommand extends Command { Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist()); if (swerve.getTrenchAssist()) { - drive(TrenchAssist2.calculate(swerve, corrected, trenchAssistPid)); + drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid)); } else { trenchAssistPid.reset(); drive(corrected); diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index e09f459..b58189c 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.util.Units; public class IntakeConstants { /** Intake roller motor speed in range [-1, 1] */ - public static final double SPEED = 0.8; + public static final double SPEED = 1.0; /** 12 tooth pinion driving 36 tooth driven gear */ public static final double GEAR_RATIO = 36.0/12.0; /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */ diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 69969eb..15f13af 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -74,10 +74,10 @@ public class Turret extends SubsystemBase implements TurretIO{ TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Slot0.kP = 10.0; + config.Slot0.kP = 12.0; config.Slot0.kS = 0.1; // Static friction compensation config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.20; // The "Braking" term to stop overshoot + config.Slot0.kD = 0.50; // The "Braking" term to stop overshoot var mm = config.MotionMagic; mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; @@ -115,10 +115,7 @@ public class Turret extends SubsystemBase implements TurretIO{ crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT); double turretRot = crt.solve(leftAbs, rightAbs); - //SmartDashboard.putNumber("Turret Index", turretIndex); - - SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot)); - + double motorRotations = turretRot * TurretConstants.GEAR_RATIO; //Sets the initial motor position @@ -243,6 +240,19 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad())); SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble()); + + + double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble(); + double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET); + + double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble(); + double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET); + + crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT); + + double turretRot = crt.solve(leftAbs, rightAbs); + + SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot)); } /* ---------------- Simulation ---------------- */ diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index e14c1a7..d630951 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -10,7 +10,8 @@ public class TurretConstants { public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop public static double MAX_VELOCITY = 600; // rad/s - public static double MAX_ACCELERATION = 120.0; // rad/s^2 + // public static double MAX_ACCELERATION = 120.0; // rad/s^2 + public static double MAX_ACCELERATION = 320.0; // rad/s^2 // Not using this, but just in case public static double TURRET_WIDTH = Units.inchesToMeters(6.4); -- 2.39.5