diff --git a/build.gradle b/build.gradle index dd3c3df..280461b 100644 --- a/build.gradle +++ b/build.gradle @@ -101,7 +101,7 @@ dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" - def coppercoreVersion = "2025.1.7" // mavenLocal version + def coppercoreVersion = "2025.1.8" // mavenLocal version implementation "io.github.team401.coppercore:controls:$coppercoreVersion" implementation "io.github.team401.coppercore:geometry:$coppercoreVersion" diff --git a/simgui-ds.json b/simgui-ds.json index 79cc689..23d49fe 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,11 +90,9 @@ } ], "robotJoysticks": [ + {}, { - "guid": "030000006d04000014c2000000000000" - }, - { - "guid": "030000006d04000014c2000000000000" + "guid": "030000004f0400000ab1000000050000" } ] } diff --git a/src/main/deploy/constants/RedFieldLocations.json b/src/main/deploy/constants/RedFieldLocations.json index c1e1d42..0ad8d09 100644 --- a/src/main/deploy/constants/RedFieldLocations.json +++ b/src/main/deploy/constants/RedFieldLocations.json @@ -1,7 +1,7 @@ { "redReefCenterTranslation": { - "x": 176.75, - "y": 158.5 + "x": 12.5, + "y": 4.0259 }, "redReefCenterRotation": { "radians": 0.0 diff --git a/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json b/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json index 9c48e1e..8c86bae 100644 --- a/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json +++ b/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json @@ -30,7 +30,7 @@ "driveAlongTrackMultiplier": -1, "driveAlongTrackOffset": 0.25, "driveCrossTrackFrontRightOffset": 0.0, - "otfPoseDistanceLimit": 0.1, + "otfPoseDistanceLimit": 0.2, "kSteerClosedLoopOutput": "Voltage", "kDriveClosedLoopOutput": "TorqueCurrentFOC", "kDriveMotorType": "TalonFX_Integrated", @@ -330,7 +330,7 @@ "kInvertRightSide": true, "kPigeonId": 20, "OTFMaxLinearVelocity": 3.5, - "OTFMaxLinearAccel": 0.5, + "OTFMaxLinearAccel": 2.0, "OTFMaxAngularVelocity": 3.141592653589793, "OTFMaxAngularAccel": 1.5707963267948966, "DrivetrainConstants": { diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7356250..b7e23b1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,15 +3,15 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-2"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 46; - public static final String GIT_SHA = "25f2cfcfd7a62559b6af5fc71c2258639513996c"; - public static final String GIT_DATE = "2025-02-05 19:00:34 EST"; - public static final String GIT_BRANCH = "add-lock-on-reef"; - public static final String BUILD_DATE = "2025-02-05 19:11:03 EST"; - public static final long BUILD_UNIX_TIME = 1738800663170L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 44; + public static final String GIT_SHA = "1fa7923e24ab57346b4e564aac14edfa3d0d542a"; + public static final String GIT_DATE = "2025-02-07 08:09:28 EST"; + public static final String GIT_BRANCH = "drive-state"; + public static final String BUILD_DATE = "2025-02-07 08:12:26 EST"; + public static final long BUILD_UNIX_TIME = 1738933946909L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index b5e362b..2369aaa 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -13,6 +13,7 @@ import frc.robot.constants.JsonConstants; import frc.robot.constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.Drive.DriveTrigger; public final class InitBindings { // Controller @@ -44,7 +45,7 @@ public static void initDriveBindings(Drive drive) { .onTrue( new InstantCommand( () -> { - drive.setAutoAlignment(true); + drive.fireTrigger(DriveTrigger.BeginAutoAlignment); }, drive)); rightJoystick @@ -52,7 +53,7 @@ public static void initDriveBindings(Drive drive) { .onFalse( new InstantCommand( () -> { - drive.setAutoAlignment(false); + drive.fireTrigger(DriveTrigger.CancelAutoAlignment); }, drive)); @@ -101,5 +102,24 @@ public static void initDriveBindings(Drive drive) { new Pose2d( Meters.of(14.350), Meters.of(4.0), new Rotation2d(Degrees.of(180)))); })); + rightJoystick + .top() + .onTrue( + new InstantCommand( + () -> { + drive.setGoToIntake(true); + drive.fireTrigger(DriveTrigger.BeginAutoAlignment); + }, + drive)); + + rightJoystick + .top() + .onFalse( + new InstantCommand( + () -> { + drive.setGoToIntake(false); + drive.fireTrigger(DriveTrigger.CancelAutoAlignment); + }, + drive)); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 476d2cb..456be14 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -130,9 +130,6 @@ public void testPeriodic() { if (FeatureFlags.synced.getObject().runScoring) { scoringSubsystem.testPeriodic(); } - if (FeatureFlags.synced.getObject().runDrive) { - drive.testPeriodic(); - } } public void disabledPeriodic() { diff --git a/src/main/java/frc/robot/constants/JsonConstants.java b/src/main/java/frc/robot/constants/JsonConstants.java index 5a63168..fd888cc 100644 --- a/src/main/java/frc/robot/constants/JsonConstants.java +++ b/src/main/java/frc/robot/constants/JsonConstants.java @@ -22,7 +22,6 @@ public static void loadConstants() { ScoringSetpoints.synced.loadData(); RedFieldLocations.synced.loadData(); BlueFieldLocations.synced.loadData(); - DrivetrainConstants.synced.loadData(); VisionConstants.synced.loadData(); elevatorConstants = ElevatorConstants.synced.getObject(); diff --git a/src/main/java/frc/robot/constants/field/RedFieldLocations.java b/src/main/java/frc/robot/constants/field/RedFieldLocations.java index 4da1191..13a19db 100644 --- a/src/main/java/frc/robot/constants/field/RedFieldLocations.java +++ b/src/main/java/frc/robot/constants/field/RedFieldLocations.java @@ -17,7 +17,7 @@ public class RedFieldLocations { .toString(), new JSONSyncConfigBuilder().setPrettyPrinting(true).build()); - public Translation2d redReefCenterTranslation = new Translation2d(176.75, (158.5)); + public Translation2d redReefCenterTranslation = new Translation2d(12.5, 4.0259); public Rotation2d redReefCenterRotation = new Rotation2d(); public Translation2d redReef01Translation = new Translation2d(11.5, 4.1); diff --git a/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java b/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java index e172e6a..508bcd9 100644 --- a/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java @@ -142,7 +142,7 @@ public class DrivetrainConstants { // OTF Max speed / accel public final Double OTFMaxLinearVelocity = 3.5; - public final Double OTFMaxLinearAccel = 0.5; + public final Double OTFMaxLinearAccel = 2.0; public final Double OTFMaxAngularVelocity = Math.PI; public final Double OTFMaxAngularAccel = Math.PI / 2; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a44b0f2..3b516b1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -9,17 +9,18 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; -import coppercore.parameter_tools.LoggedTunableNumber; +import coppercore.controls.state_machine.StateMachine; +import coppercore.controls.state_machine.StateMachineConfiguration; +import coppercore.controls.state_machine.state.PeriodicStateInterface; +import coppercore.controls.state_machine.state.StateContainer; import coppercore.vision.VisionLocalizer.DistanceToTag; import coppercore.wpilib_interface.DriveTemplate; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -34,7 +35,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -46,11 +46,15 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; -import frc.robot.TestModeManager; import frc.robot.constants.JsonConstants; +import frc.robot.subsystems.drive.states.IdleState; +import frc.robot.subsystems.drive.states.JoystickDrive; +import frc.robot.subsystems.drive.states.LineupState; +import frc.robot.subsystems.drive.states.OTFState; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -163,76 +167,51 @@ public enum DesiredLocation { }; private DesiredLocation desiredLocation = DesiredLocation.Reef0; - - private boolean isOTF = false; - - private boolean isLiningUp = false; - - private Command driveToPose = null; - - private VisionAlignment alignmentSupplier; - - // along track pid test mode - private LoggedTunableNumber alongTrackkP = - new LoggedTunableNumber( - "DriveLineupGains/AlongTrackkP", JsonConstants.drivetrainConstants.driveAlongTrackkP); - private LoggedTunableNumber alongTrackkI = - new LoggedTunableNumber( - "DriveLineupGains/AlongTrackkI", JsonConstants.drivetrainConstants.driveAlongTrackkI); - private LoggedTunableNumber alongTrackkD = - new LoggedTunableNumber( - "DriveLineupGains/AlongTrackkD", JsonConstants.drivetrainConstants.driveAlongTrackkD); - - // cross tack pid test mode - private LoggedTunableNumber crossTrackkP = - new LoggedTunableNumber( - "DriveLineupGains/CrossTrackkP", JsonConstants.drivetrainConstants.driveCrossTrackkP); - private LoggedTunableNumber crossTrackkI = - new LoggedTunableNumber( - "DriveLineupGains/CrossTrackkI", JsonConstants.drivetrainConstants.driveCrossTrackkI); - private LoggedTunableNumber crossTrackkD = - new LoggedTunableNumber( - "DriveLineupGains/CrossTrackkD", JsonConstants.drivetrainConstants.driveCrossTrackkD); - - // rotation pid test mode - private LoggedTunableNumber rotationkP = - new LoggedTunableNumber( - "DriveLineupGains/rotationkP", JsonConstants.drivetrainConstants.driveRotationkP); - private LoggedTunableNumber rotationkI = - new LoggedTunableNumber( - "DriveLineupGains/rotationkI", JsonConstants.drivetrainConstants.driveRotationkI); - private LoggedTunableNumber rotationkD = - new LoggedTunableNumber( - "DriveLineupGains/rotationkD", JsonConstants.drivetrainConstants.driveRotationkD); - - private PIDController driveAlongTrackLineupController = - new PIDController( - JsonConstants.drivetrainConstants.driveAlongTrackkP, - JsonConstants.drivetrainConstants.driveAlongTrackkI, - JsonConstants.drivetrainConstants.driveAlongTrackkD); - private PIDController driveCrossTrackLineupController = - new PIDController( - JsonConstants.drivetrainConstants.driveCrossTrackkP, - JsonConstants.drivetrainConstants.driveCrossTrackkI, - JsonConstants.drivetrainConstants.driveCrossTrackkD); - - private Constraints driveAlongTrackProfileConstraints = - new Constraints( - JsonConstants.drivetrainConstants.driveAlongTrackVelocity, - JsonConstants.drivetrainConstants.driveAlongTrackVelocity); - private TrapezoidProfile driveAlongTrackProfile = - new TrapezoidProfile(driveAlongTrackProfileConstraints); - - private PIDController rotationController = - new PIDController( - JsonConstants.drivetrainConstants.driveRotationkP, - JsonConstants.drivetrainConstants.driveRotationkI, - JsonConstants.drivetrainConstants.driveRotationkD); + private DesiredLocation intakeLocation = DesiredLocation.CoralStationLeft; + private boolean goToIntake = false; private NetworkTableInstance inst = NetworkTableInstance.getDefault(); private NetworkTable table = inst.getTable(""); private DoubleSubscriber reefLocationSelector = table.getDoubleTopic("reefTarget").subscribe(-1); + private BooleanSupplier waitOnScore = () -> false; + private VisionAlignment alignmentSupplier = null; + + private static Drive instance; + + private enum DriveState implements StateContainer { + Idle(new IdleState(instance)), + OTF(new OTFState(instance)), + Lineup(new LineupState(instance)), + Joystick(new JoystickDrive(instance)); + private final PeriodicStateInterface state; + + DriveState(PeriodicStateInterface state) { + this.state = state; + } + + @Override + public PeriodicStateInterface getState() { + return state; + } + } + + public enum DriveTrigger { + ManualJoysticks, + BeginAutoAlignment, + CancelAutoAlignment, + FinishOTF, + CancelOTF, + BeginLineup, + CancelLineup, + FinishLineup, + WaitForScore, + } + + private StateMachineConfiguration stateMachineConfiguration; + + private StateMachine stateMachine; + private boolean isAligningToFieldElement = false; private Translation2d lockedAlignPosition = new Translation2d(); @@ -242,7 +221,6 @@ public Drive( ModuleIO frModuleIO, ModuleIO blModuleIO, ModuleIO brModuleIO) { - this.alignmentSupplier = null; this.gyroIO = gyroIO; modules[0] = new Module(flModuleIO, 0, DriveConfiguration.getInstance().FrontLeft); modules[1] = new Module(frModuleIO, 1, DriveConfiguration.getInstance().FrontRight); @@ -291,7 +269,47 @@ public Drive( new SysIdRoutine.Mechanism( (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); - rotationController.enableContinuousInput(-Math.PI / 2, Math.PI / 2); + configureStates(); + } + + public void configureStates() { + instance = this; + + stateMachineConfiguration = new StateMachineConfiguration<>(); + + stateMachineConfiguration + .configure(DriveState.Joystick) + .permitIf( + DriveTrigger.BeginAutoAlignment, + DriveState.OTF, + () -> !this.isDriveCloseToFinalLineupPose()) + .permitIf( + DriveTrigger.BeginAutoAlignment, + DriveState.Lineup, + () -> this.isDriveCloseToFinalLineupPose()); + + stateMachineConfiguration + .configure(DriveState.OTF) + .permit(DriveTrigger.CancelOTF, DriveState.Joystick) + .permitIf( + DriveTrigger.FinishOTF, + DriveState.Joystick, + () -> this.isDriveCloseToFinalLineupPose() && !this.isDesiredLocationReef()) + .permitIf( + DriveTrigger.FinishOTF, + DriveState.Lineup, + () -> (this.isDriveCloseToFinalLineupPose() && this.isDesiredLocationReef())) + .permit(DriveTrigger.CancelAutoAlignment, DriveState.Joystick) + .permitIf(DriveTrigger.BeginLineup, DriveState.Lineup, () -> this.isDesiredLocationReef()); + + stateMachineConfiguration + .configure(DriveState.Lineup) + .permit(DriveTrigger.CancelLineup, DriveState.Joystick) + .permitIf(DriveTrigger.FinishLineup, DriveState.Idle, () -> this.isWaitingOnScore()) + .permitIf(DriveTrigger.FinishLineup, DriveState.Joystick, () -> !this.isWaitingOnScore()) + .permit(DriveTrigger.CancelAutoAlignment, DriveState.Joystick); + + stateMachine = new StateMachine<>(stateMachineConfiguration, DriveState.Joystick); } @Override @@ -311,32 +329,15 @@ public void periodic() { } } + stateMachine.periodic(); + Logger.recordOutput("Drive/State", stateMachine.getCurrentState()); + // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - // OTF Command - if (driveToPose != null) { - Logger.recordOutput("Drive/OTF/OnTheFlyCommandStatus", this.driveToPose.isScheduled()); - - // cancel path following command once OTF cancelled (likely via trigger) - if (!isOTF) { - driveToPose.cancel(); - } - } - - if (isDriveCloseToFinalLineupPose() && isOTF) { - this.setOTF(false); - driveToPose.cancel(); - this.setLiningUp(true); - } - - if (isLiningUp) { - this.LineupWithReefLocation(); - } - // check for update from reef touchscreen this.updateDesiredLocationFromNetworkTables(); @@ -381,39 +382,6 @@ public void periodic() { gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); } - public void testPeriodic() { - switch (TestModeManager.getTestMode()) { - case DriveLineupTuning: - LoggedTunableNumber.ifChanged( - hashCode(), - (pid) -> { - this.setAlongTrackPID(pid[0], pid[1], pid[2]); - }, - alongTrackkP, - alongTrackkI, - alongTrackkD); - LoggedTunableNumber.ifChanged( - hashCode(), - (pid) -> { - this.setCrossTrackPID(pid[0], pid[1], pid[2]); - }, - crossTrackkP, - crossTrackkI, - crossTrackkD); - LoggedTunableNumber.ifChanged( - hashCode(), - (pid) -> { - this.setRotationLineupPID(pid[0], pid[1], pid[2]); - }, - rotationkP, - rotationkI, - rotationkD); - break; - default: - break; - } - } - /** * sets desired speeds of robot * @@ -422,7 +390,7 @@ public void testPeriodic() { * control), false for auto (robot centric) */ public void setGoalSpeeds(ChassisSpeeds speeds, boolean fieldCentric) { - if (fieldCentric) { + if (fieldCentric && stateMachine.inState(DriveState.Joystick)) { // Adjust for field-centric control boolean isFlipped = DriverStation.getAlliance().isPresent() @@ -455,37 +423,13 @@ public void disableAlign() { } /** - * sets lineup along track pid gains + * set supplier that interfaces with scoring used to make drive set 0 speeds once lined up (so no + * error movement occurs) * - * @param kP proportional gain - * @param kI integral gain - * @param kD derivative gain + * @param waitOnScore BooleanSupplier to let drive know if scoring is done scoring */ - public void setAlongTrackPID(double kP, double kI, double kD) { - this.driveAlongTrackLineupController = new PIDController(kP, kI, kD); - } - - /** - * sets lineup cross track pid gains - * - * @param kP proportional gain - * @param kI integral gain - * @param kD derivative gain - */ - public void setCrossTrackPID(double kP, double kI, double kD) { - this.driveCrossTrackLineupController = new PIDController(kP, kI, kD); - } - - /** - * sets lineup rotation pid gains - * - * @param kP proportional gain - * @param kI integral gain - * @param kD derivative gain - */ - public void setRotationLineupPID(double kP, double kI, double kD) { - this.rotationController = new PIDController(kP, kI, kD); - this.rotationController.enableContinuousInput(-Math.PI / 2, Math.PI / 2); + public void setWaitOnScoreSupplier(BooleanSupplier waitOnScore) { + this.waitOnScore = waitOnScore; } /** @@ -498,20 +442,21 @@ public void setAlignmentSupplier(VisionAlignment alignmentSupplier) { } /** - * sets isOTF of robot true will cause robot to create a path from current location to the set - * PathLocation + * gets the alignment supplier for use in lineup state * - * @param isOTF boolean telling robot if it should create a OTF path + * @return VisionAlignment supplier (possibly null) */ - public void setOTF(boolean isOTF) { - this.isOTF = isOTF; + public VisionAlignment getVisionAlignment() { + return alignmentSupplier; + } - if (isOTF) { - this.driveToPose = this.getDriveToPoseCommand(); - if (this.driveToPose != null) { - this.driveToPose.schedule(); - } - } + /** + * checks if drive needs to wait on scoring subsystem + * + * @return true if drive is waiting + */ + public boolean isWaitingOnScore() { + return waitOnScore.getAsBoolean(); } /** @@ -521,7 +466,7 @@ public void setOTF(boolean isOTF) { */ @AutoLogOutput(key = "Drive/OTF/isOTF") public boolean isDriveOTF() { - return isOTF; + return stateMachine.inState(DriveState.OTF); } /** @@ -529,26 +474,17 @@ public boolean isDriveOTF() { * * @return true if robot pose is close to desired pose */ + @AutoLogOutput(key = "Drive/OTF/isDriveCloseToFinalLineupPose") public boolean isDriveCloseToFinalLineupPose() { // relative to transforms first pose into distance from desired pose // then get distance between poses (if less than 0.1 meters we are good) return this.getPose() - .relativeTo(this.findOTFPoseFromDesiredLocation()) + .relativeTo(OTFState.findOTFPoseFromDesiredLocation(this)) .getTranslation() .getNorm() < JsonConstants.drivetrainConstants.otfPoseDistanceLimit; } - /** - * sets isLiningUp of robot true will cause robot to gather distance from reef tag and drive - * towards it PathLocation - * - * @param isLiningUp boolean telling robot if it should create a OTF path - */ - public void setLiningUp(boolean isLiningUp) { - this.isLiningUp = isLiningUp; - } - /** * checks if drive is currently lining up to a reef * @@ -556,7 +492,7 @@ public void setLiningUp(boolean isLiningUp) { */ @AutoLogOutput(key = "Drive/Lineup/isLiningUp") public boolean isDriveLiningUp() { - return isLiningUp; + return stateMachine.inState(DriveState.Lineup); } /** @@ -566,26 +502,19 @@ public boolean isDriveLiningUp() { */ public boolean isDesiredLocationReef() { return !(desiredLocation == DesiredLocation.CoralStationLeft - || desiredLocation == DesiredLocation.CoralStationRight - || desiredLocation == DesiredLocation.Processor); + || desiredLocation == DesiredLocation.CoralStationRight + || desiredLocation == DesiredLocation.Processor) + && !goToIntake; // only want reef if intake is false } /** - * allows drive to be controlled by on the fly / landing zone alignment + * checks if location is a scoring location * - * @param autoAlignment true allows drive to go into otf and alignment + * @return true if location is scoring (reef / processor) */ - public void setAutoAlignment(boolean autoAlignment) { - if (autoAlignment) { - if (isDriveCloseToFinalLineupPose() && isDesiredLocationReef()) { - this.setLiningUp(true); - } else { - this.setOTF(true); - } - } else { - setOTF(false); - setLiningUp(false); - } + public boolean isLocationScoring(DesiredLocation location) { + return !(location == DesiredLocation.CoralStationLeft + || location == DesiredLocation.CoralStationRight); } /** @@ -595,7 +524,17 @@ public void setAutoAlignment(boolean autoAlignment) { * @param location desired location for robot to pathfind to */ public void setDesiredLocation(DesiredLocation location) { - this.desiredLocation = location; + if (isLocationScoring(location)) { + this.desiredLocation = location; + } + } + + /** + * @return the desired location for otf and lineup + */ + @AutoLogOutput(key = "Drive//DesiredLocation") + public DesiredLocation getDesiredLocation() { + return goToIntake ? this.intakeLocation : this.desiredLocation; } /** @@ -615,7 +554,7 @@ public void updateDesiredLocationFromNetworkTables() { return; } if (locationArray[(int) desiredIndex] != desiredLocation) { - if (isOTF) { + if (isDriveOTF()) { this.updateDesiredLocation((int) desiredIndex); } else { this.setDesiredLocation((int) desiredIndex); @@ -643,305 +582,48 @@ public void updateDesiredLocation(int locationIndex) { public void updateDesiredLocation(DesiredLocation location) { this.setDesiredLocation(location); - if (isOTF) { - this.driveToPose.cancel(); - this.driveToPose = this.getDriveToPoseCommand(); - this.driveToPose.schedule(); + if (isDriveOTF()) { + stateMachine.fire(DriveTrigger.ManualJoysticks); + stateMachine.fire(DriveTrigger.BeginAutoAlignment); } } /** - * finds a pose to pathfind to based on desiredLocation enum - * - * @return a pose representing the corresponding scoring location - */ - public Pose2d findOTFPoseFromDesiredLocation() { - switch (this.desiredLocation) { - // NOTE: pairs of reef sides (ie 0 and 1) will have the same otf pose (approximately 0.5-1 - // meter away from center of tag) - case Reef0: - case Reef1: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef01Translation, - JsonConstants.redFieldLocations.redReef01Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef01Translation, - JsonConstants.blueFieldLocations.blueReef01Rotation); - case Reef2: - case Reef3: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef23Translation, - JsonConstants.redFieldLocations.redReef23Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef23Translation, - JsonConstants.blueFieldLocations.blueReef23Rotation); - case Reef4: - case Reef5: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef45Translation, - JsonConstants.redFieldLocations.redReef45Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef45Translation, - JsonConstants.blueFieldLocations.blueReef45Rotation); - case Reef6: - case Reef7: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef67Translation, - JsonConstants.redFieldLocations.redReef67Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef67Translation, - JsonConstants.blueFieldLocations.blueReef67Rotation); - case Reef8: - case Reef9: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef89Translation, - JsonConstants.redFieldLocations.redReef89Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef89Translation, - JsonConstants.blueFieldLocations.blueReef89Rotation); - case Reef10: - case Reef11: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef1011Translation, - JsonConstants.redFieldLocations.redReef1011Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef1011Translation, - JsonConstants.blueFieldLocations.blueReef1011Rotation); - case CoralStationRight: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redCoralStationRightTranslation, - JsonConstants.redFieldLocations.redCoralStationRightRotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueCoralStationRightTranslation, - JsonConstants.blueFieldLocations.blueCoralStationRightRotation); - case CoralStationLeft: - return isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redCoralStationLeftTranslation, - JsonConstants.redFieldLocations.redCoralStationLeftRotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueCoralStationLeftTranslation, - JsonConstants.blueFieldLocations.blueCoralStationLeftRotation); - default: - this.setOTF(false); - return null; - } - } - - /** - * gets the path from current pose to the desired pose found from location - * - * @return command that drive can schedule to follow the path found - */ - public Command getDriveToPoseCommand() { - Pose2d targetPose = findOTFPoseFromDesiredLocation(); - - if (targetPose == null) { - return null; - } - - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints( - JsonConstants.drivetrainConstants.OTFMaxLinearVelocity, - JsonConstants.drivetrainConstants.OTFMaxLinearAccel, - JsonConstants.drivetrainConstants.OTFMaxAngularVelocity, - JsonConstants.drivetrainConstants.OTFMaxAngularAccel); - - return AutoBuilder.pathfindToPose(targetPose, constraints, 0.0); - } - - /** - * checks if driver station alliance is red - * - * @return true if alliance is red - */ - public boolean isAllianceRed() { - return DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red); - } - - /** - * gets tag to use for final alignment with vision + * set which intake to go to * - * @return int representing tag id to use + * @param location coral station location (left v right) */ - public int getTagIdForReef() { - boolean allianceRed = this.isAllianceRed(); - switch (desiredLocation) { - case Reef0: - case Reef1: - return allianceRed ? 10 : 21; - case Reef2: - case Reef3: - return allianceRed ? 9 : 22; - case Reef4: - case Reef5: - return allianceRed ? 8 : 17; - case Reef6: - case Reef7: - return allianceRed ? 7 : 18; - case Reef8: - case Reef9: - return allianceRed ? 6 : 19; - case Reef10: - case Reef11: - return allianceRed ? 11 : 20; - default: - return -1; + public void setDesiredIntakeLocation(DesiredLocation location) { + if (!isLocationScoring(location)) { + this.intakeLocation = location; } } /** - * gets cross track offset for lineup + * tells drive to otf to intake instead of reef * - * @param cameraIndex camera to check offset - * @return offset for camera + * @param goToIntake true if drive should go to coral stations */ - public Double getCrossTrackOffset(int cameraIndex) { - if (cameraIndex == 0) { - return JsonConstants.drivetrainConstants.driveCrossTrackFrontRightOffset; - } - return 0.0; // front left offset (not added) + public void setGoToIntake(boolean goToIntake) { + this.goToIntake = goToIntake; } /** - * gets camera index for vision single tag lineup + * attempts to change state of state machine * - * @return 0 for Front Left camera; 1 for Front Right camera + * @param trigger trigger to give to state for transition */ - public int getCameraIndexForLineup() { - switch (desiredLocation) { - // Right Side of reef side (align to left camera) - case Reef0: - case Reef2: - case Reef4: - case Reef6: - case Reef8: - case Reef10: - return JsonConstants.visionConstants.FrontLeftCameraIndex; - // Left side of reef side (align to right camera) - case Reef1: - case Reef3: - case Reef5: - case Reef7: - case Reef9: - case Reef11: - return JsonConstants.visionConstants.FrontRightCameraIndex; - default: - return -1; - } + public void fireTrigger(DriveTrigger trigger) { + stateMachine.fire(trigger); } /** - * gets rotation for each side of hexagonal reef for lineup + * checks if driver station alliance is red * - * @return Rotation2d representing desired rotation for lineup + * @return true if alliance is red */ - public Rotation2d getRotationForReefSide() { - switch (desiredLocation) { - case Reef0: - case Reef1: - return isAllianceRed() - ? JsonConstants.redFieldLocations.redReef01Rotation - : JsonConstants.blueFieldLocations.blueReef01Rotation; - case Reef2: - case Reef3: - return isAllianceRed() - ? JsonConstants.redFieldLocations.redReef23Rotation - : JsonConstants.blueFieldLocations.blueReef23Rotation; - case Reef4: - case Reef5: - return isAllianceRed() - ? JsonConstants.redFieldLocations.redReef45Rotation - : JsonConstants.blueFieldLocations.blueReef45Rotation; - case Reef6: - case Reef7: - return isAllianceRed() - ? JsonConstants.redFieldLocations.redReef67Rotation - : JsonConstants.blueFieldLocations.blueReef67Rotation; - case Reef8: - case Reef9: - return isAllianceRed() - ? JsonConstants.redFieldLocations.redReef89Rotation - : JsonConstants.blueFieldLocations.blueReef89Rotation; - case Reef10: - case Reef11: - return isAllianceRed() - ? JsonConstants.redFieldLocations.redReef1011Rotation - : JsonConstants.blueFieldLocations.blueReef1011Rotation; - default: - return new Rotation2d(); - } - } - - private DistanceToTag latestObservation; - private int observationAge; - - /** take over goal speeds to align to reef exactly */ - public void LineupWithReefLocation() { - int tagId = this.getTagIdForReef(); - int cameraIndex = this.getCameraIndexForLineup(); - - if (tagId == -1 || cameraIndex == -1 || alignmentSupplier == null) { - // cancel lineup or whatever - this.setLiningUp(false); - return; - } - - System.out.println(tagId); - - DistanceToTag observation = - alignmentSupplier.get( - tagId, - cameraIndex, - this.getCrossTrackOffset(cameraIndex), - JsonConstants.drivetrainConstants.driveAlongTrackOffset); - - if (!observation.isValid()) { - if (latestObservation != null && observationAge < 5) { - observation = latestObservation; - observationAge++; - } else { - return; - } - } else { - latestObservation = observation; - observationAge = 0; - } - - Logger.recordOutput("Drive/Lineup/AlongTrackDistance", observation.alongTrackDistance()); - Logger.recordOutput("Drive/Lineup/CrossTrackDistance", observation.crossTrackDistance()); - Logger.recordOutput("Drive/Lineup/IsObservationValid", observation.isValid()); - - // give to PID Controllers and setGoalSpeeds (robotCentric) - double vx = - JsonConstants.drivetrainConstants.driveAlongTrackMultiplier - * driveAlongTrackLineupController.calculate(observation.alongTrackDistance()); - // ChassisSpeeds speeds = getChassisSpeeds(); - // Pose2d pose = getPose(); - // double velocityToTarget = (pose.getX() * speeds.vxMetersPerSecond + pose.getY() * - // speeds.vyMetersPerSecond) / Math.sqrt(pose.getX() * pose.getX() + pose.getY() * pose.getY()); - // double velocityToTarget = -getChassisSpeeds().vxMetersPerSecond; - // Logger.recordOutput("Drive/Lineup/velocityToTarget", velocityToTarget); - // double vx = - // -driveAlongTrackProfile.calculate( - // 0.02, - // new State(observation.alongTrackDistance(), velocityToTarget), - // new State(0, 0)) - // .velocity; - double vy = driveCrossTrackLineupController.calculate(observation.crossTrackDistance()); - double omega = - rotationController.calculate( - this.getRotation().getRadians(), this.getRotationForReefSide().getRadians()); - - this.setGoalSpeeds(new ChassisSpeeds(vx, vy, omega), false); + public boolean isAllianceRed() { + return DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red); } public void alignToTarget() { diff --git a/src/main/java/frc/robot/subsystems/drive/states/IdleState.java b/src/main/java/frc/robot/subsystems/drive/states/IdleState.java new file mode 100644 index 0000000..e508e9a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/states/IdleState.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.drive.states; + +import coppercore.controls.state_machine.state.PeriodicStateInterface; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.Drive.DriveTrigger; + +public class IdleState implements PeriodicStateInterface { + private Drive drive; + + public IdleState(Drive drive) { + this.drive = drive; + } + + public void periodic() { + drive.setGoalSpeeds(new ChassisSpeeds(), false); + + // set to joysticks once score subsystem is finished + if (!drive.isWaitingOnScore()) { + drive.fireTrigger(DriveTrigger.ManualJoysticks); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/states/JoystickDrive.java b/src/main/java/frc/robot/subsystems/drive/states/JoystickDrive.java new file mode 100644 index 0000000..651ce8d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/states/JoystickDrive.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.drive.states; + +import coppercore.controls.state_machine.state.PeriodicStateInterface; +import frc.robot.subsystems.drive.Drive; + +public class JoystickDrive implements PeriodicStateInterface { + private Drive drive; + + public JoystickDrive(Drive drive) { + this.drive = drive; + } + + public void periodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/states/LineupState.java b/src/main/java/frc/robot/subsystems/drive/states/LineupState.java new file mode 100644 index 0000000..c771352 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/states/LineupState.java @@ -0,0 +1,359 @@ +package frc.robot.subsystems.drive.states; + +import coppercore.controls.state_machine.state.PeriodicStateInterface; +import coppercore.controls.state_machine.transition.Transition; +import coppercore.parameter_tools.LoggedTunableNumber; +import coppercore.vision.VisionLocalizer.DistanceToTag; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.TestModeManager; +import frc.robot.constants.JsonConstants; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.Drive.VisionAlignment; +import org.littletonrobotics.junction.Logger; + +public class LineupState implements PeriodicStateInterface { + private Drive drive; + + private DistanceToTag latestObservation; + private int observationAge; + + // along track pid test mode + private LoggedTunableNumber alongTrackkP = + new LoggedTunableNumber( + "DriveLineupGains/AlongTrackkP", JsonConstants.drivetrainConstants.driveAlongTrackkP); + private LoggedTunableNumber alongTrackkI = + new LoggedTunableNumber( + "DriveLineupGains/AlongTrackkI", JsonConstants.drivetrainConstants.driveAlongTrackkI); + private LoggedTunableNumber alongTrackkD = + new LoggedTunableNumber( + "DriveLineupGains/AlongTrackkD", JsonConstants.drivetrainConstants.driveAlongTrackkD); + + // cross tack pid test mode + private LoggedTunableNumber crossTrackkP = + new LoggedTunableNumber( + "DriveLineupGains/CrossTrackkP", JsonConstants.drivetrainConstants.driveCrossTrackkP); + private LoggedTunableNumber crossTrackkI = + new LoggedTunableNumber( + "DriveLineupGains/CrossTrackkI", JsonConstants.drivetrainConstants.driveCrossTrackkI); + private LoggedTunableNumber crossTrackkD = + new LoggedTunableNumber( + "DriveLineupGains/CrossTrackkD", JsonConstants.drivetrainConstants.driveCrossTrackkD); + + // rotation pid test mode + private LoggedTunableNumber rotationkP = + new LoggedTunableNumber( + "DriveLineupGains/rotationkP", JsonConstants.drivetrainConstants.driveRotationkP); + private LoggedTunableNumber rotationkI = + new LoggedTunableNumber( + "DriveLineupGains/rotationkI", JsonConstants.drivetrainConstants.driveRotationkI); + private LoggedTunableNumber rotationkD = + new LoggedTunableNumber( + "DriveLineupGains/rotationkD", JsonConstants.drivetrainConstants.driveRotationkD); + + private PIDController driveAlongTrackLineupController = + new PIDController( + JsonConstants.drivetrainConstants.driveAlongTrackkP, + JsonConstants.drivetrainConstants.driveAlongTrackkI, + JsonConstants.drivetrainConstants.driveAlongTrackkD); + private PIDController driveCrossTrackLineupController = + new PIDController( + JsonConstants.drivetrainConstants.driveCrossTrackkP, + JsonConstants.drivetrainConstants.driveCrossTrackkI, + JsonConstants.drivetrainConstants.driveCrossTrackkD); + + private Constraints driveAlongTrackProfileConstraints = + new Constraints( + JsonConstants.drivetrainConstants.driveAlongTrackVelocity, + JsonConstants.drivetrainConstants.driveAlongTrackVelocity); + private TrapezoidProfile driveAlongTrackProfile = + new TrapezoidProfile(driveAlongTrackProfileConstraints); + + private PIDController rotationController = + new PIDController( + JsonConstants.drivetrainConstants.driveRotationkP, + JsonConstants.drivetrainConstants.driveRotationkI, + JsonConstants.drivetrainConstants.driveRotationkD); + + public LineupState(Drive drive) { + this.drive = drive; + rotationController.enableContinuousInput(-Math.PI / 2, Math.PI / 2); + } + + public void onEntry(Transition transition) { + // cancel rotation lock on center + drive.disableAlign(); + } + + /** + * gets rotation for each side of hexagonal reef for lineup + * + * @return Rotation2d representing desired rotation for lineup + */ + public Rotation2d getRotationForReefSide() { + switch (drive.getDesiredLocation()) { + case Reef0: + case Reef1: + return drive.isAllianceRed() + ? JsonConstants.redFieldLocations.redReef01Rotation + : JsonConstants.blueFieldLocations.blueReef01Rotation; + case Reef2: + case Reef3: + return drive.isAllianceRed() + ? JsonConstants.redFieldLocations.redReef23Rotation + : JsonConstants.blueFieldLocations.blueReef23Rotation; + case Reef4: + case Reef5: + return drive.isAllianceRed() + ? JsonConstants.redFieldLocations.redReef45Rotation + : JsonConstants.blueFieldLocations.blueReef45Rotation; + case Reef6: + case Reef7: + return drive.isAllianceRed() + ? JsonConstants.redFieldLocations.redReef67Rotation + : JsonConstants.blueFieldLocations.blueReef67Rotation; + case Reef8: + case Reef9: + return drive.isAllianceRed() + ? JsonConstants.redFieldLocations.redReef89Rotation + : JsonConstants.blueFieldLocations.blueReef89Rotation; + case Reef10: + case Reef11: + return drive.isAllianceRed() + ? JsonConstants.redFieldLocations.redReef1011Rotation + : JsonConstants.blueFieldLocations.blueReef1011Rotation; + default: + return new Rotation2d(); + } + } + + /** + * checks if lineup is within 0.01 of tag + offsets + * + * @return true if error is small enoguh + */ + public boolean lineupFinished() { + return latestObservation != null + && (latestObservation.alongTrackDistance() < 0.01 + && latestObservation.crossTrackDistance() < 0.01); + } + + public void periodic() { + // run test through here if in test mode + if (DriverStation.isTest()) { + this.testPeriodic(); + } + this.LineupWithReefLocation(); + + // if (lineupFinished()) { + // drive.fireTrigger(DriveTrigger.FinishLineup); + // } + } + + /** + * gets tag to use for final alignment with vision + * + * @return int representing tag id to use + */ + public int getTagIdForReef() { + boolean allianceRed = drive.isAllianceRed(); + switch (drive.getDesiredLocation()) { + case Reef0: + case Reef1: + return allianceRed ? 10 : 21; + case Reef2: + case Reef3: + return allianceRed ? 9 : 22; + case Reef4: + case Reef5: + return allianceRed ? 8 : 17; + case Reef6: + case Reef7: + return allianceRed ? 7 : 18; + case Reef8: + case Reef9: + return allianceRed ? 6 : 19; + case Reef10: + case Reef11: + return allianceRed ? 11 : 20; + default: + return -1; + } + } + + /** + * gets cross track offset for lineup + * + * @param cameraIndex camera to check offset + * @return offset for camera + */ + public Double getCrossTrackOffset(int cameraIndex) { + if (cameraIndex == 0) { + return JsonConstants.drivetrainConstants.driveCrossTrackFrontRightOffset; + } + return 0.0; // front left offset (not added) + } + + /** + * gets camera index for vision single tag lineup + * + * @return 0 for Front Left camera; 1 for Front Right camera + */ + public int getCameraIndexForLineup() { + switch (drive.getDesiredLocation()) { + // Right Side of reef side (align to left camera) + case Reef0: + case Reef2: + case Reef4: + case Reef6: + case Reef8: + case Reef10: + return JsonConstants.visionConstants.FrontLeftCameraIndex; + // Left side of reef side (align to right camera) + case Reef1: + case Reef3: + case Reef5: + case Reef7: + case Reef9: + case Reef11: + return JsonConstants.visionConstants.FrontRightCameraIndex; + default: + return -1; + } + } + + /** take over goal speeds to align to reef exactly */ + public void LineupWithReefLocation() { + int tagId = this.getTagIdForReef(); + int cameraIndex = this.getCameraIndexForLineup(); + + if (tagId == -1 || cameraIndex == -1) { + // TODO: check if this might be false first time, but on another loop true + // drive.fireTrigger(DriveTrigger.CancelLineup); + return; + } + + VisionAlignment alignmentSupplier = drive.getVisionAlignment(); + + if (alignmentSupplier == null) { + return; + } + + DistanceToTag observation = + alignmentSupplier.get( + tagId, + cameraIndex, + this.getCrossTrackOffset(cameraIndex), + JsonConstants.drivetrainConstants.driveAlongTrackOffset); + + if (!observation.isValid()) { + if (latestObservation != null && observationAge < 5) { + observation = latestObservation; + observationAge++; + } else { + return; + } + } else { + latestObservation = observation; + observationAge = 0; + } + + Logger.recordOutput("Drive/Lineup/AlongTrackDistance", observation.alongTrackDistance()); + Logger.recordOutput("Drive/Lineup/CrossTrackDistance", observation.crossTrackDistance()); + Logger.recordOutput("Drive/Lineup/IsObservationValid", observation.isValid()); + + // give to PID Controllers and setGoalSpeeds (robotCentric) + double vx = + JsonConstants.drivetrainConstants.driveAlongTrackMultiplier + * driveAlongTrackLineupController.calculate(observation.alongTrackDistance()); + // ChassisSpeeds speeds = getChassisSpeeds(); + // Pose2d pose = getPose(); + // double velocityToTarget = (pose.getX() * speeds.vxMetersPerSecond + pose.getY() * + // speeds.vyMetersPerSecond) / Math.sqrt(pose.getX() * pose.getX() + pose.getY() * pose.getY()); + // double velocityToTarget = -getChassisSpeeds().vxMetersPerSecond; + // Logger.recordOutput("Drive/Lineup/velocityToTarget", velocityToTarget); + // double vx = + // -driveAlongTrackProfile.calculate( + // 0.02, + // new State(observation.alongTrackDistance(), velocityToTarget), + // new State(0, 0)) + // .velocity; + double vy = driveCrossTrackLineupController.calculate(observation.crossTrackDistance()); + double omega = + rotationController.calculate( + drive.getRotation().getRadians(), this.getRotationForReefSide().getRadians()); + + drive.setGoalSpeeds(new ChassisSpeeds(vx, vy, omega), false); + } + + /** + * sets lineup along track pid gains + * + * @param kP proportional gain + * @param kI integral gain + * @param kD derivative gain + */ + public void setAlongTrackPID(double kP, double kI, double kD) { + this.driveAlongTrackLineupController = new PIDController(kP, kI, kD); + } + + /** + * sets lineup cross track pid gains + * + * @param kP proportional gain + * @param kI integral gain + * @param kD derivative gain + */ + public void setCrossTrackPID(double kP, double kI, double kD) { + this.driveCrossTrackLineupController = new PIDController(kP, kI, kD); + } + + /** + * sets lineup rotation pid gains + * + * @param kP proportional gain + * @param kI integral gain + * @param kD derivative gain + */ + public void setRotationLineupPID(double kP, double kI, double kD) { + this.rotationController = new PIDController(kP, kI, kD); + this.rotationController.enableContinuousInput(-Math.PI / 2, Math.PI / 2); + } + + public void testPeriodic() { + switch (TestModeManager.getTestMode()) { + case DriveLineupTuning: + LoggedTunableNumber.ifChanged( + hashCode(), + (pid) -> { + this.setAlongTrackPID(pid[0], pid[1], pid[2]); + }, + alongTrackkP, + alongTrackkI, + alongTrackkD); + LoggedTunableNumber.ifChanged( + hashCode(), + (pid) -> { + this.setCrossTrackPID(pid[0], pid[1], pid[2]); + }, + crossTrackkP, + crossTrackkI, + crossTrackkD); + LoggedTunableNumber.ifChanged( + hashCode(), + (pid) -> { + this.setRotationLineupPID(pid[0], pid[1], pid[2]); + }, + rotationkP, + rotationkI, + rotationkD); + break; + default: + break; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/states/OTFState.java b/src/main/java/frc/robot/subsystems/drive/states/OTFState.java new file mode 100644 index 0000000..d3b10ca --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/states/OTFState.java @@ -0,0 +1,172 @@ +package frc.robot.subsystems.drive.states; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.path.PathConstraints; +import coppercore.controls.state_machine.state.PeriodicStateInterface; +import coppercore.controls.state_machine.transition.Transition; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.JsonConstants; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.Drive.DriveTrigger; +import org.littletonrobotics.junction.Logger; + +public class OTFState implements PeriodicStateInterface { + private Drive drive; + + private Command driveToPose = null; + + private Pose2d otfPose = null; + + public OTFState(Drive drive) { + this.drive = drive; + } + + public void onEntry(Transition transition) { + if (PathfindingCommand.warmupCommand().isScheduled()) { + PathfindingCommand.warmupCommand().cancel(); + } + driveToPose = this.getDriveToPoseCommand(); + if (driveToPose == null) { + drive.fireTrigger(DriveTrigger.CancelOTF); + } + this.driveToPose.schedule(); + } + + public void onExit(Transition transition) { + otfPose = null; + System.out.println("Cancelling command"); + if (driveToPose != null) { + this.driveToPose.cancel(); + } + } + + /** + * finds a pose to pathfind to based on desiredLocation enum + * + * @return a pose representing the corresponding scoring location + */ + public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { + switch (driveInput.getDesiredLocation()) { + // NOTE: pairs of reef sides (ie 0 and 1) will have the same otf pose (approximately 0.5-1 + // meter away from center of tag) + case Reef0: + case Reef1: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef01Translation, + JsonConstants.redFieldLocations.redReef01Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef01Translation, + JsonConstants.blueFieldLocations.blueReef01Rotation); + case Reef2: + case Reef3: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef23Translation, + JsonConstants.redFieldLocations.redReef23Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef23Translation, + JsonConstants.blueFieldLocations.blueReef23Rotation); + case Reef4: + case Reef5: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef45Translation, + JsonConstants.redFieldLocations.redReef45Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef45Translation, + JsonConstants.blueFieldLocations.blueReef45Rotation); + case Reef6: + case Reef7: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef67Translation, + JsonConstants.redFieldLocations.redReef67Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef67Translation, + JsonConstants.blueFieldLocations.blueReef67Rotation); + case Reef8: + case Reef9: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef89Translation, + JsonConstants.redFieldLocations.redReef89Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef89Translation, + JsonConstants.blueFieldLocations.blueReef89Rotation); + case Reef10: + case Reef11: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef1011Translation, + JsonConstants.redFieldLocations.redReef1011Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef1011Translation, + JsonConstants.blueFieldLocations.blueReef1011Rotation); + case CoralStationRight: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redCoralStationRightTranslation, + JsonConstants.redFieldLocations.redCoralStationRightRotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueCoralStationRightTranslation, + JsonConstants.blueFieldLocations.blueCoralStationRightRotation); + case CoralStationLeft: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redCoralStationLeftTranslation, + JsonConstants.redFieldLocations.redCoralStationLeftRotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueCoralStationLeftTranslation, + JsonConstants.blueFieldLocations.blueCoralStationLeftRotation); + default: + return null; + } + } + + /** + * gets the path from current pose to the desired pose found from location + * + * @return command that drive can schedule to follow the path found + */ + public Command getDriveToPoseCommand() { + otfPose = findOTFPoseFromDesiredLocation(drive); + + if (otfPose == null) { + return null; + } + + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + JsonConstants.drivetrainConstants.OTFMaxLinearVelocity, + JsonConstants.drivetrainConstants.OTFMaxLinearAccel, + JsonConstants.drivetrainConstants.OTFMaxAngularVelocity, + JsonConstants.drivetrainConstants.OTFMaxAngularAccel); + + return AutoBuilder.pathfindToPose(otfPose, constraints, 0.0); + } + + public void periodic() { + // checks if location has changed (so path can be rescheduled) + if (otfPose == null || !otfPose.equals(findOTFPoseFromDesiredLocation(drive))) { + this.onEntry(null); + } + + // finishes otf when we are 0.1 meters away + if (drive.isDriveCloseToFinalLineupPose()) { + drive.fireTrigger(DriveTrigger.FinishOTF); + } + + if (driveToPose != null) { + Logger.recordOutput("Drive/OTF/commandScheduled", driveToPose.isScheduled()); + + // this runs when we accidentally go into otf (too close to reef for final pose to be true) + if (driveToPose.isFinished()) { + drive.fireTrigger(DriveTrigger.BeginLineup); + } + } + } +}