From 741e097e31f38bde771122224987235c8a208c9c Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Mon, 3 Mar 2025 22:12:52 -0800 Subject: [PATCH 01/37] Refactor GameInfo --- src/main/java/frc/robot/Robot.java | 17 ++- .../java/frc/robot/commands/PIDtoNearest.java | 102 -------------- .../java/frc/robot/subsystems/Swerve.java | 16 +++ src/main/java/frc/robot/util/GameInfo.java | 128 +++++++++--------- 4 files changed, 97 insertions(+), 166 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/PIDtoNearest.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b43d13e..4386e72 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,7 +31,6 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.MoveSuperStructure; -import frc.robot.commands.PIDtoNearest; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; @@ -135,9 +134,23 @@ public Robot() { .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) .withRotationalRate( applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.1) * MaxRotationalRate))); + + SmartDashboard.putData("Face Hex", Swerve.get().applyRequest( + () -> Swerve.fcDriveReq.withVelocityX( + applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) + .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) + .withRotationalRate( + (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / 30 * MaxRotationalRate))); + + SmartDashboard.putData("Face Net", Swerve.get().applyRequest( + () -> Swerve.fcDriveReq.withVelocityX( + applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) + .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) + .withRotationalRate( + (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceNet())).getDegrees() / 30 * MaxRotationalRate))); + SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); - NamedCommands.registerCommand("PIDtoNearest", new PIDtoNearest(true)); NamedCommands.registerCommand("ScoreL4", Elevator.get().positionCommand(GameInfo.L4)); NamedCommands.registerCommand("ScoreL3", diff --git a/src/main/java/frc/robot/commands/PIDtoNearest.java b/src/main/java/frc/robot/commands/PIDtoNearest.java deleted file mode 100644 index 1184c10..0000000 --- a/src/main/java/frc/robot/commands/PIDtoNearest.java +++ /dev/null @@ -1,102 +0,0 @@ - -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Swerve; -import frc.robot.util.GameInfo; -import frc.robot.util.PoseUtil; -import frc.team696.lib.Logging.BackupLogger; - -/** - * PIDToPosition but it takes goes to the nearest scoring pose defined in GameInfo. - * @see PIDToPosition - * @see GameInfo - */ -public class PIDtoNearest extends Command { - private ProfiledPIDController xController, yController, omegaController; - private Pose2d goalPose; - private boolean ignoreLR; - - private double calculateWithTolerance(ProfiledPIDController controller, double measurement, double goal){ - double tolerance=controller.getPositionTolerance(); - double error=goal-measurement; - return Math.abs(error)> ScoringPosesBlue; + static { - blue = new FieldSide(); - // TODO: determine the real scoring poses - blue.left = new Pose2d[] { - new Pose2d(3.539, 4.912, Rotation2d.fromDegrees(-60 + 180)), - new Pose2d(3.179, 3.752, Rotation2d.fromDegrees(0 + 180)), - new Pose2d(4.056, 2.689, Rotation2d.fromDegrees(60 + 180)), - new Pose2d(5.450, 2.992, Rotation2d.fromDegrees(120 + 180)), - new Pose2d(5.850, 4.376, Rotation2d.fromDegrees(180 + 180)), - new Pose2d(4.924, 5.351, Rotation2d.fromDegrees(-120 + 180)) - }; - blue.right = new Pose2d[] { - new Pose2d(4.046, 5.322, Rotation2d.fromDegrees(-60 + 180)), - new Pose2d(3.188, 4.473, Rotation2d.fromDegrees(0 + 180)), - new Pose2d(3.481, 3.050, Rotation2d.fromDegrees(60 + 180)), - new Pose2d(4.836, 2.679, Rotation2d.fromDegrees(120 + 180)), - new Pose2d(5.821, 3.762, Rotation2d.fromDegrees(180 + 180)), - new Pose2d(5.489, 4.932, Rotation2d.fromDegrees(-120 + 180)) - }; - blue.both = Stream.concat(Arrays.stream(blue.left), Arrays.stream(blue.right)) - .toArray(size -> new Pose2d[size]); - - // The red poses are mirrored from the blue scoring poses - red = new FieldSide(); - red.left = Arrays.stream(blue.left).map(pose -> PoseUtil.mirrorPoseX(pose)).toArray(size -> new Pose2d[size]); - - red.right = Arrays.stream(blue.right).map(pose -> PoseUtil.mirrorPoseX(pose)).toArray(size -> new Pose2d[size]); - - red.both = Stream.concat(Arrays.stream(red.left), Arrays.stream(red.right)).toArray(size -> new Pose2d[size]); + ScoringPosesBlue = Map.of( + Index.One, Map.of( + Side.Right, new Translation2d(4.046, 5.322), + Side.Left, new Translation2d(3.539, 4.912) + ), + + Index.Two, Map.of( + Side.Right, new Translation2d(3.188, 4.473), + Side.Left, new Translation2d(3.179, 3.752) + ), + + Index.Three, Map.of( + Side.Right, new Translation2d(3.481, 3.050), + Side.Left, new Translation2d(4.056, 2.689) + ), + + Index.Four, Map.of( + Side.Right, new Translation2d(4.836, 2.679), + Side.Left, new Translation2d(5.450, 2.992) + ), + + Index.Five, Map.of( + Side.Right, new Translation2d(5.821, 3.762), + Side.Left, new Translation2d(5.850, 4.376) + ), + + Index.Six, Map.of( + Side.Right, new Translation2d(5.489, 4.932), + Side.Left, new Translation2d(4.924, 5.351) + ) + ); + + L1 = new CoralScoringPosition(0., 1.75, 0.45); L2 = new CoralScoringPosition(8., 1.75, 0.45); @@ -98,29 +125,6 @@ public static Pose2d[] getScoringPoses() { ClimbDown=new CoralScoringPosition(2, -8., 0); Intake = new CoralScoringPosition(0, -1.75, 3.6); ground=new CoralScoringPosition(5., 8.2, 4.3); - /** - * this puts the values on networktables so scoring positions can be quickly - * changed - * comment this out once the scoring positions are finalized - - new TriggerNTDouble("testing/L1/height", L1.height, height -> L1.height = height); - new TriggerNTDouble("testing/L2/height", L2.height, height -> L2.height = height); - new TriggerNTDouble("testing/L3/height", L3.height, height -> L3.height = height); - new TriggerNTDouble("testing/L4/height", L4.height, height -> L4.height = height); - new TriggerNTDouble("testing/ground/height", ground.height, height -> ground.height = height); - - new TriggerNTDouble("testing/L1/armRot", L1.armRot.in(Rotation), rot -> L1.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L2/armRot", L2.armRot.in(Rotation), rot -> L2.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L3/armRot", L3.armRot.in(Rotation), rot -> L3.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L4/armRot", L4.armRot.in(Rotation), rot -> L4.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/ground/armRot", ground.armRot.in(Rotation), rot -> ground.armRot = Rotation.of(rot)); - - new TriggerNTDouble("testing/L1/wristRot", L1.wristRot.in(Rotation), rot -> L1.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L2/wristRot", L2.wristRot.in(Rotation), rot -> L2.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L3/wristRot", L3.wristRot.in(Rotation), rot -> L3.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L4/wristRot", L4.wristRot.in(Rotation), rot -> L4.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/ground/wristRot", ground.wristRot.in(Rotation), rot -> ground.wristRot = Rotation.of(rot)); - */ } } From 0b946a3ebbd5133982aa8ae4e5efada59e58c244 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Thu, 6 Mar 2025 22:54:07 -0800 Subject: [PATCH 02/37] Mostly Working Teleop --- src/main/java/frc/robot/BotConstants.java | 30 ++----- src/main/java/frc/robot/BuildConstants.java | 12 +-- src/main/java/frc/robot/HumanControls.java | 44 +++++----- src/main/java/frc/robot/Robot.java | 78 +++++++++-------- .../robot/commands/MoveSuperStructure.java | 29 +++++-- src/main/java/frc/robot/subsystems/Arm.java | 1 - .../java/frc/robot/subsystems/Elevator.java | 1 - .../frc/robot/subsystems/EndEffector.java | 30 ++++++- .../java/frc/robot/subsystems/Swerve.java | 18 +++- src/main/java/frc/robot/subsystems/Wrist.java | 15 +++- src/main/java/frc/robot/util/GameInfo.java | 85 +++++++++++++------ 11 files changed, 215 insertions(+), 128 deletions(-) diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 4831a02..ca7ee45 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -33,13 +33,13 @@ public static class Elevator { static { cfg = new TalonFXConfiguration(); cfg.Slot0.kP = 6.; - //cfg.Slot0.kG = 0.05; + cfg.Slot0.kG = 0.05; //cfg.Slot0.kS = 0.02; cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; - cfg.MotionMagic.MotionMagicAcceleration = 75.; + cfg.MotionMagic.MotionMagicAcceleration = 120.; cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.CurrentLimits.StatorCurrentLimit = 120.; cfg.CurrentLimits.StatorCurrentLimitEnable = true; @@ -57,9 +57,9 @@ public static class Arm { cfg.Slot0.GravityType = GravityTypeValue.Arm_Cosine; cfg.Slot0.kP = 8.; cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 120.; + cfg.CurrentLimits.StatorCurrentLimit = 100.; cfg.MotionMagic.MotionMagicCruiseVelocity = 80.; - cfg.MotionMagic.MotionMagicAcceleration = 80.; + cfg.MotionMagic.MotionMagicAcceleration = 65.; cfg.Slot0.kG = 0; } @@ -71,11 +71,11 @@ public static class Wrist { static { cfg.Slot0.kP = 32.; - cfg.MotionMagic.MotionMagicAcceleration = 16.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 10.; + cfg.MotionMagic.MotionMagicAcceleration = 40.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; cfg.CurrentLimits.StatorCurrentLimitEnable = false; cfg.CurrentLimits.SupplyCurrentLimitEnable=false; - cfg.CurrentLimits.StatorCurrentLimit = 80.; + cfg.CurrentLimits.StatorCurrentLimit = 120.; } } @@ -85,21 +85,7 @@ public static class EndEffector { public static TalonFXConfiguration cfg = new TalonFXConfiguration(); static { cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - } - } - - public static class ClimberIntake { - public static int masterID = 0; - public static int slaveID = 1; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.MotorOutput.NeutralMode = NeutralModeValue.Coast; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; - cfg.MotionMagic.MotionMagicAcceleration = 15.; - cfg.MotionMagic.MotionMagicJerk = 30.; + cfg.CurrentLimits.StatorCurrentLimit = 80.; } } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 536f0f3..42838bc 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 36; - public static final String GIT_SHA = "93c25fb9e2162579fff06051c4e0eaa98931c0f9"; - public static final String GIT_DATE = "2025-03-01 18:57:22 PST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2025-03-03 14:02:14 PST"; - public static final long BUILD_UNIX_TIME = 1741039334068L; + public static final int GIT_REVISION = 38; + public static final String GIT_SHA = "741e097e31f38bde771122224987235c8a208c9c"; + public static final String GIT_DATE = "2025-03-03 22:12:52 PST"; + public static final String GIT_BRANCH = "Oscar"; + public static final String BUILD_DATE = "2025-02-13 20:45:12 PST"; + public static final long BUILD_UNIX_TIME = 1739508312539L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/HumanControls.java b/src/main/java/frc/robot/HumanControls.java index de1e7f8..a5730e4 100644 --- a/src/main/java/frc/robot/HumanControls.java +++ b/src/main/java/frc/robot/HumanControls.java @@ -4,14 +4,12 @@ package frc.robot; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team696.lib.Swerve.Commands.TeleopSwerve; /** Add your docs here. */ public final class HumanControls { @@ -39,32 +37,32 @@ public final class HumanControls { public static final JoystickButton Leftest = new JoystickButton(operatorPanelB, 8); }*/ public final class OperatorPanel2025{ - public static final JoystickButton L1=null; - public static final JoystickButton L2=null; - public static final JoystickButton L3=null; - public static final JoystickButton L4=null; - public static final JoystickButton F1=null; - public static final JoystickButton F2=null; - public static final JoystickButton F3=null; - public static final JoystickButton releaseCoral=null; - public static final JoystickButton pickupAlgae=null; - public static final JoystickButton gyro=null; - public static final JoystickButton Climb1=null; - public static final JoystickButton Climb2=null; - public static final JoystickButton SouceCoral=null; - public static final JoystickButton GroundCoral=null; - public static final JoystickButton Barge=null; - public static final JoystickButton Processor=null; + public static final Joystick OperatorPanel=new Joystick(1); + + + public static final JoystickButton L1=new JoystickButton(OperatorPanel, 9); + public static final JoystickButton L2=new JoystickButton(OperatorPanel, 10); + public static final JoystickButton L3=new JoystickButton(OperatorPanel, 11); + public static final JoystickButton L4=new JoystickButton(OperatorPanel, 12); + //public static final JoystickButton F1=new JoystickButton(null, 0); + //public static final JoystickButton F2=null; + //public static final JoystickButton F3=null; + public static final JoystickButton releaseCoral=new JoystickButton(OperatorPanel, 7); + public static final JoystickButton pickupAlgae=new JoystickButton(OperatorPanel, 8); + public static final JoystickButton gyro=new JoystickButton(OperatorPanel, 13); + public static final JoystickButton Climb1=new JoystickButton(OperatorPanel, 4); + public static final JoystickButton Climb2=new JoystickButton(OperatorPanel, 3); + public static final JoystickButton SouceCoral=new JoystickButton(OperatorPanel, 6); + public static final JoystickButton GroundCoral=new JoystickButton(OperatorPanel, 0); + public static final JoystickButton Barge=new JoystickButton(OperatorPanel,2); + public static final JoystickButton Processor=new JoystickButton(OperatorPanel, 1); - public static final JoystickButton leftOrRight=null; + public static final JoystickButton leftOrRight=new JoystickButton(OperatorPanel, 14); // Use one of the switches on the driver station for this - public static final BooleanSupplier manualOverride=null; + //public static final BooleanSupplier manualOverride=new JoystickButton(null, 0); } public final class DriverPanel{ public static final Joystick DriverPanel=new Joystick(0); - public static final Joystick OperatorPanelA=new Joystick(1); - public static final Joystick OperatorPanelB=new Joystick(2); - public static final Joystick OperatorPanelC=new Joystick(3); public static final JoystickButton resetGyro=new JoystickButton(DriverPanel, 1); public static final JoystickButton OtherButton=new JoystickButton(DriverPanel,2); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4386e72..9eb88c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -45,7 +46,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxRotationalRate = RotationsPerSecond.of(3).in(RadiansPerSecond); + private double MaxRotationalRate = RotationsPerSecond.of(10).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); private void configureBinds() { @@ -80,17 +81,7 @@ public double applyDeadband(double x, double deadband) { public void putCommandButtons() { SmartDashboard.putData("pathfindToMiddle", new PrintCommand("s").andThen( AutoBuilder.pathfindToPose(new Pose2d(7, 3, Rotation2d.fromDegrees(12)), new PathConstraints(1, 1, Math.PI, Math.PI)))); - SmartDashboard.putData("ScoreNet", new MoveSuperStructure(GameInfo.Net, 0)); - SmartDashboard.putData("ScoreL4", new MoveSuperStructure(GameInfo.L4,0.6)); - SmartDashboard.putData("ScoreL3", new MoveSuperStructure(GameInfo.L3, 0.6)); - SmartDashboard.putData("ScoreL2", new MoveSuperStructure(GameInfo.L2, 0.6)); - SmartDashboard.putData("ScoreL1", new MoveSuperStructure(GameInfo.L1, 0.6)); - SmartDashboard.putData("Ground", new MoveSuperStructure(GameInfo.ground, 0)); - SmartDashboard.putData("Spin Rollers Forward", EndEffector.get().spin(0.6)); - SmartDashboard.putData("Spin Rollers Reverse", EndEffector.get().spin(-0.6)); - SmartDashboard.putData("IntakeSource", new MoveSuperStructure(GameInfo.Intake, 0.0)); - SmartDashboard.putData("Climb Up", new MoveSuperStructure(GameInfo.ClimbUp, 0)); - SmartDashboard.putData("Climb Down", new MoveSuperStructure(GameInfo.ClimbDown, 0)); + } @@ -108,13 +99,8 @@ public void putSwerveSysIDCalibrationButtons() { private final SendableChooser autoChooser; public Robot() { - - // For remote layout downloading - WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); - SmartDashboard.putData(CommandScheduler.getInstance()); SmartDashboard.putData(Elevator.get()); - // SmartDashboard.putData(ClimberIntake.get()); SmartDashboard.putData(Arm.get()); SmartDashboard.putData(EndEffector.get()); SmartDashboard.putData(Wrist.get()); @@ -130,17 +116,17 @@ public Robot() { // TODO: Restore TeleopSwerve Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( - applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) - .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) + Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.08),2) * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.08),2) * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) .withRotationalRate( - applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.1) * MaxRotationalRate))); + Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.08), 2) * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); SmartDashboard.putData("Face Hex", Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) .withRotationalRate( - (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / 30 * MaxRotationalRate))); + (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / -120 * MaxRotationalRate))); SmartDashboard.putData("Face Net", Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( @@ -151,33 +137,53 @@ public Robot() { SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); - NamedCommands.registerCommand("ScoreL4", - Elevator.get().positionCommand(GameInfo.L4)); - NamedCommands.registerCommand("ScoreL3", - Arm.get().Position(GameInfo.L3).alongWith(Elevator.get().positionCommand(GameInfo.L3))); - NamedCommands.registerCommand("ScoreL2", Elevator.get().positionCommand(GameInfo.L2)); - NamedCommands.registerCommand("ScoreL1", Elevator.get().positionCommand(GameInfo.L1)); putCommandButtons(); NamedCommands.registerCommand("hello", new InstantCommand( () -> SmartDashboard.putBoolean("auto", true))); - // TODO: decide whether or not this will be a temporary fix autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); // Warmup Commands for PathPlanner PathfindingCommand.warmupCommand().schedule(); + + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); + Wrist.get().setDefaultCommand(Wrist.get().Position(0)); + Arm.get().setDefaultCommand(Arm.get().Position(()->0)); + EndEffector.get().setDefaultCommand(EndEffector.get().spin(()->EndEffector.get().idlePower)); } private void configureDriverStationBinds() { - // HumanControls.DriverPanel.resetGyro.onTrue(Commands.runOnce(()->Swerve.get().seedFieldCentric())); - // HumanControls.OperatorPanel2025.L1.whileTrue(Elevator.get().positionCommand(GameInfo.L1)); - // HumanControls.OperatorPanel2025.L2.whileTrue(Elevator.get().positionCommand(GameInfo.L2)); - // HumanControls.OperatorPanel2025.L3.whileTrue(Elevator.get().positionCommand(GameInfo.L3)); - // HumanControls.OperatorPanel2025.L4.whileTrue(Elevator.get().positionCommand(GameInfo.L4)); - - HumanControls.DriverPanel.OtherButton.whileTrue(new MoveSuperStructure(GameInfo.ground, 0.6)); - HumanControls.DriverPanel.resetGyro.whileTrue(new MoveSuperStructure(GameInfo.L3, 0.6)); + HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(()->Swerve.get().seedFieldCentric())); + HumanControls.OperatorPanel2025.releaseCoral.onTrue(new InstantCommand(()-> {EndEffector.get().idlePower = -0.6;})); + + HumanControls.OperatorPanel2025.L1.whileTrue( + new ConditionalCommand( + new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.4), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean + )); + + HumanControls.OperatorPanel2025.L2.whileTrue( + new ConditionalCommand( + new MoveSuperStructure(GameInfo.L2Algae, -0.8, false, -0.8), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Back), -0.6), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean + )); + + HumanControls.OperatorPanel2025.L3.whileTrue( + new ConditionalCommand( + new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean + )); + + HumanControls.OperatorPanel2025.L4.whileTrue(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6)); + HumanControls.OperatorPanel2025.Climb2.whileTrue(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.5, false, 0.2)); + HumanControls.OperatorPanel2025.Barge.whileTrue(new MoveSuperStructure(GameInfo.Net, 1.)); + HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); + HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6)); + //HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); } @Override diff --git a/src/main/java/frc/robot/commands/MoveSuperStructure.java b/src/main/java/frc/robot/commands/MoveSuperStructure.java index 211f6c1..52fad78 100644 --- a/src/main/java/frc/robot/commands/MoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/MoveSuperStructure.java @@ -7,6 +7,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Wrist; +import frc.robot.HumanControls; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.EndEffector; @@ -18,12 +19,26 @@ public class MoveSuperStructure extends Command { double runRollers = 0; - public MoveSuperStructure(CoralScoringPosition position, double runRollers) { + double postRollerState = 0; + + boolean requirePress = true; + + public MoveSuperStructure(CoralScoringPosition position, double runRollers, boolean requirePress, double postRollerState) { this.position = position; this.runRollers = runRollers; - addRequirements(Arm.get(), Elevator.get(), Wrist.get()); + this.postRollerState = postRollerState; + + this.requirePress = requirePress; + + addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); + } + + + + public MoveSuperStructure(CoralScoringPosition position, double runRollers) { + this(position, runRollers, true, 0.); } // Called when the command is initially scheduled. @@ -37,10 +52,10 @@ public void execute() { Wrist.get().goToPosition(position); Elevator.get().goToPosition(position); - //if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < 2 && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < 2 && Math.abs(Elevator.get().getPosition() - position.height) < 2 ) - // EndEffector.get().run(runRollers); - //else - // EndEffector.get().stop(); + if ((!requirePress || HumanControls.OperatorPanel2025.releaseCoral.getAsBoolean()) && Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 && Math.abs(Elevator.get().getPosition() - position.height) < .5 ) + EndEffector.get().run(runRollers); + else + EndEffector.get().run(EndEffector.get().idlePower); } // Called once the command ends or is interrupted. @@ -49,7 +64,7 @@ public void end(boolean interrupted) { Arm.get().stop(); Wrist.get().stop(); Elevator.get().stop(); - //EndEffector.get().stop(); + EndEffector.get().idlePower = postRollerState; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 46257a6..928548b 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -67,7 +67,6 @@ private Arm() { zeroArm(); // this.setDefaultCommand(Position(()->0)); - this.setDefaultCommand(ArmWithNTPosition()); new TriggerNTDouble("testing/armAngle", ntpos, (ev) -> ntpos = ev); SmartDashboard.putData("ArmPlus", Spin(0.1)); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 0e9ef7f..ddf2c69 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -48,7 +48,6 @@ private Elevator() { .linearPosition(Meters.of(master.getPosition())) .linearVelocity(MetersPerSecond.of(master.getVelocity())); }, this)); - this.setDefaultCommand(positionCommand(0)); SmartDashboard.putData("Elevator duty cycle up", runDutyCycle()); } diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java index 0a221a9..052f5b3 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector.java @@ -20,7 +20,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; @@ -52,6 +51,8 @@ public static final synchronized EndEffector get() { StatusSignal voltageSignal; StatusSignal currentSignal; + public double idlePower = 0; + public EndEffector() { motor.getConfigurator().apply(BotConstants.EndEffector.cfg); velocitySignal = motor.getVelocity(); @@ -76,11 +77,18 @@ public void run(double output) { * @return the command */ public Command spin(double power) { - return this.startEnd(() -> motor.setControl(VoltageRequest.withOutput(power * 12)), () -> motor.set(0)); + return this.startEnd(() -> run(power), () -> motor.set(0)); + } + + public boolean isStalling(){ + + return motor.getStatorCurrent().getValue().in(Amps) >= 40 && velocitySignal.getValue().in(RotationsPerSecond) < 5; } + + public Command spin(DoubleSupplier power) { - return this.runEnd(() -> motor.setControl(VoltageRequest.withOutput(power.getAsDouble() * 12)), () -> motor.set(0)); + return this.runEnd(() -> run(power.getAsDouble()), () -> motor.set(0)); } public Command spinVelocity(double velocity) { @@ -89,6 +97,22 @@ public Command spinVelocity(double velocity) { @Override public void periodic() { + + // if (isStalling() && BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity == 10. ) { + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 6.; + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 6.; + // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); + // // idlePower = -0.8; + // } else if (!isStalling()) { + // if (BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity < 16.) { + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 10.; + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 16.; + // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); + // } + // //idlePower = 0.; + // } + + // This method will be called once per scheduler run BackupLogger.addToQueue("EndEffector/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); BackupLogger.addToQueue("EndEffector/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index b23ed15..0a6d773 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -370,6 +370,22 @@ public Rotation2d FaceHexFace() { } public Rotation2d FaceNet() { - return Util.getAlliance() == Alliance.Blue ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180); + return Util.getAlliance() == Alliance.Blue ? Rotation2d.fromDegrees(-90) : Rotation2d.fromDegrees(90); + } + + public Rotation2d FaceSource() { + if (Util.getAlliance() == Alliance.Blue) { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(-45); + } else { + return Rotation2d.fromDegrees(45); + } + } else { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(135); + } else { + return Rotation2d.fromDegrees(-135); + } + } } } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 70e2143..047afdd 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -35,7 +35,7 @@ public static final synchronized Wrist get() { return m_Wrist; } - private TalonFX motor = new TalonFX(BotConstants.Wrist.motorID, BotConstants.rioBus); + public TalonFX motor = new TalonFX(BotConstants.Wrist.motorID, BotConstants.rioBus); StatusSignal velocitySignal; StatusSignal positionSignal; @@ -53,7 +53,6 @@ private Wrist() { positionSignal = motor.getPosition(); voltageSignal = motor.getMotorVoltage(); currentSignal = motor.getStatorCurrent(); - this.setDefaultCommand(Position(0)); SmartDashboard.putData("Zero Wrist", this.runOnce(() -> zero()).ignoringDisable(true)); } @@ -72,17 +71,25 @@ public void stop() { public double getPosition() { return positionSignal.getValueAsDouble(); } + public Command Position(double position){ - return this.startEnd(()->motor.setControl(WristPoistionRequest.withPosition(position)), ()->motor.set(0.0)); + return this.runEnd(()->goToPosition(position), ()->motor.set(0.0)); + } + + public void goToPosition(double position) { + motor.setControl(WristPoistionRequest.withPosition(position)); + } public void goToPosition(CoralScoringPosition position){ - motor.setControl(WristPoistionRequest.withPosition(position.wristRot.in(Rotation))); + goToPosition(position.wristRot.in(Rotation)); } @Override public void periodic() { + + // This method will be called once per scheduler run BackupLogger.addToQueue("Wrist/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); BackupLogger.addToQueue("Wrist/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 111d1bd..1901fc7 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -52,11 +52,11 @@ public CoralScoringPosition(double height, double armRot, double wristRot) { public Angle wristRot; } - public static CoralScoringPosition L1, L2, L3, L4, Net, Intake, ground, ClimbUp, ClimbDown; + public static CoralScoringPosition Net, ground, Processor, ClimbUp, ClimbDown, L2Algae, L3Algae; /* Looking at the Index Dead On */ - public enum Side { + public enum ReefSide { Right, Left } @@ -72,6 +72,7 @@ public enum Index { } public final static Distance fieldLengthMeters = Feet.of(57.53); + public final static Distance fieldWidthMeters = Feet.of(26.75); public static Translation2d mirrorTranslation(Translation2d starting) { return new Translation2d(fieldLengthMeters.in(Meters) - starting.getY(), starting.getY()); @@ -79,52 +80,88 @@ public static Translation2d mirrorTranslation(Translation2d starting) { public final static Translation2d blueReef = new Translation2d(4.5,4.); - public final static Map> ScoringPosesBlue; + public final static Map> ScoringPosesBlue; + public enum Position { + L1, + L2, + L3, + L4, + Intake + } + + public enum RobotSide { + Front, + Back + } + + public final static Map> RobotState; + static { ScoringPosesBlue = Map.of( Index.One, Map.of( - Side.Right, new Translation2d(4.046, 5.322), - Side.Left, new Translation2d(3.539, 4.912) + ReefSide.Right, new Translation2d(4.046, 5.322), + ReefSide.Left, new Translation2d(3.539, 4.912) ), Index.Two, Map.of( - Side.Right, new Translation2d(3.188, 4.473), - Side.Left, new Translation2d(3.179, 3.752) + ReefSide.Right, new Translation2d(3.188, 4.473), + ReefSide.Left, new Translation2d(3.179, 3.752) ), Index.Three, Map.of( - Side.Right, new Translation2d(3.481, 3.050), - Side.Left, new Translation2d(4.056, 2.689) + ReefSide.Right, new Translation2d(3.481, 3.050), + ReefSide.Left, new Translation2d(4.056, 2.689) ), Index.Four, Map.of( - Side.Right, new Translation2d(4.836, 2.679), - Side.Left, new Translation2d(5.450, 2.992) + ReefSide.Right, new Translation2d(4.836, 2.679), + ReefSide.Left, new Translation2d(5.450, 2.992) ), Index.Five, Map.of( - Side.Right, new Translation2d(5.821, 3.762), - Side.Left, new Translation2d(5.850, 4.376) + ReefSide.Right, new Translation2d(5.821, 3.762), + ReefSide.Left, new Translation2d(5.850, 4.376) ), Index.Six, Map.of( - Side.Right, new Translation2d(5.489, 4.932), - Side.Left, new Translation2d(4.924, 5.351) + ReefSide.Right, new Translation2d(5.489, 4.932), + ReefSide.Left, new Translation2d(4.924, 5.351) ) ); + RobotState = Map.of( + Position.L1, Map.of( + RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1), + RobotSide.Back, new CoralScoringPosition(0, -1., -8.) + ), + Position.L2, Map.of( + RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56), + RobotSide.Back, new CoralScoringPosition(7., -1., -7.6) + ), + Position.L3, Map.of( + RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1), + RobotSide.Back, new CoralScoringPosition(25., -1., -7.9) + ), + Position.L4, Map.of( + RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6), + RobotSide.Back, new CoralScoringPosition(62, -1.1, -8.3) + ), + Position.Intake, Map.of( + RobotSide.Front, new CoralScoringPosition(6., 1., -0.9), + RobotSide.Back, new CoralScoringPosition(0, 0, 0) + ) + + ); + L2Algae = new CoralScoringPosition (18., -3., 1.); + L3Algae = new CoralScoringPosition (44., -3., 1.); - L1 = new CoralScoringPosition(0., 1.75, 0.45); - L2 = new CoralScoringPosition(8., 1.75, 0.45); - L3 = new CoralScoringPosition(30., 1.75, .45); - L4 = new CoralScoringPosition(67., 0.7, .65); - Net = new CoralScoringPosition(67., 0., 2.5); - ClimbUp=new CoralScoringPosition(27, -8., 0); - ClimbDown=new CoralScoringPosition(2, -8., 0); - Intake = new CoralScoringPosition(0, -1.75, 3.6); - ground=new CoralScoringPosition(5., 8.2, 4.3); + Net = new CoralScoringPosition(67., 0., 5.8); + ClimbUp=new CoralScoringPosition(27, 0, 0); + ClimbDown=new CoralScoringPosition(2, 0, 0); + ground=new CoralScoringPosition(5., -6.3, -.5); + Processor = new CoralScoringPosition(0, 7., 11.9); } } From ed07c595a6bc9b84e63ca36fb01089c7dbe58554 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Thu, 6 Mar 2025 23:07:33 -0800 Subject: [PATCH 03/37] Removed All Problems --- src/main/java/frc/robot/Auto.java | 11 --- src/main/java/frc/robot/BotConstants.java | 2 - src/main/java/frc/robot/Robot.java | 20 ------ .../frc/robot/commands/PIDtoPosition.java | 5 -- src/main/java/frc/robot/subsystems/Arm.java | 3 - .../java/frc/robot/subsystems/Swerve.java | 52 -------------- .../java/frc/robot/util/DynamicNTBoolean.java | 46 ------------- .../java/frc/robot/util/DynamicNTDouble.java | 44 ------------ .../java/frc/robot/util/DynamicNTVal.java | 68 ------------------- src/main/java/frc/robot/util/GameInfo.java | 26 ++----- src/main/java/frc/robot/util/PoseUtil.java | 2 +- 11 files changed, 8 insertions(+), 271 deletions(-) delete mode 100644 src/main/java/frc/robot/util/DynamicNTBoolean.java delete mode 100644 src/main/java/frc/robot/util/DynamicNTDouble.java delete mode 100644 src/main/java/frc/robot/util/DynamicNTVal.java diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index a5c8c5e..4647ed8 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -1,17 +1,8 @@ package frc.robot; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; - import java.util.ArrayList; import java.util.List; -import java.util.Optional; -import java.util.function.Supplier; - import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; @@ -33,9 +24,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.subsystems.Swerve; diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index ca7ee45..5880960 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -10,8 +10,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import frc.robot.util.TriggerNTDouble; - /** * Constants for robot mechanisms unrelated to the drive train */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9eb88c4..dfb230b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,14 +12,9 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; -import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -29,19 +24,16 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.MoveSuperStructure; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; import frc.robot.util.GameInfo; -import frc.robot.util.GameInfo.CoralScoringPosition; import frc.team696.lib.Camera.LimelightHelpers; import frc.team696.lib.Logging.BackupLogger; import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; -import frc.robot.HumanControls.OperatorPanel2025; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -49,10 +41,6 @@ public class Robot extends TimedRobot { private double MaxRotationalRate = RotationsPerSecond.of(10).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); - private void configureBinds() { - OperatorPanel2025.gyro.onTrue(new InstantCommand(() -> Swerve.get().seedFieldCentric())); - OperatorPanel2025.releaseCoral.onTrue(new InstantCommand()); - } private void logBuildInfo() { BackupLogger.addToQueue("BuildConstants/ProjectName", BuildConstants.MAVEN_NAME); @@ -78,12 +66,6 @@ public double applyDeadband(double x, double deadband) { return Math.abs(x) < deadband ? 0 : x; } - public void putCommandButtons() { - SmartDashboard.putData("pathfindToMiddle", new PrintCommand("s").andThen( - AutoBuilder.pathfindToPose(new Pose2d(7, 3, Rotation2d.fromDegrees(12)), new PathConstraints(1, 1, Math.PI, Math.PI)))); - - - } public void putSwerveSysIDCalibrationButtons() { SmartDashboard.putData("CTRESwerveCalibrationc/DynamicForward", @@ -113,7 +95,6 @@ public Robot() { // Log Build information logBuildInfo(); - // TODO: Restore TeleopSwerve Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.08),2) * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) @@ -137,7 +118,6 @@ public Robot() { SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); - putCommandButtons(); NamedCommands.registerCommand("hello", new InstantCommand( () -> SmartDashboard.putBoolean("auto", true))); diff --git a/src/main/java/frc/robot/commands/PIDtoPosition.java b/src/main/java/frc/robot/commands/PIDtoPosition.java index 6411754..aca382b 100644 --- a/src/main/java/frc/robot/commands/PIDtoPosition.java +++ b/src/main/java/frc/robot/commands/PIDtoPosition.java @@ -23,11 +23,6 @@ public class PIDtoPosition extends Command { private ProfiledPIDController xController, yController, omegaController; private Pose2d goalPose; - private double calculateWithTolerance(ProfiledPIDController controller, double measurement, double goal){ - double tolerance=controller.getPositionTolerance(); - double error=goal-measurement; - return Math.abs(error) SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per secondĀ², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - /* The SysId routine to test */ private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; diff --git a/src/main/java/frc/robot/util/DynamicNTBoolean.java b/src/main/java/frc/robot/util/DynamicNTBoolean.java deleted file mode 100644 index a6753d4..0000000 --- a/src/main/java/frc/robot/util/DynamicNTBoolean.java +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.HashMap; -import java.util.Map; -import java.util.function.DoubleConsumer; - -import edu.wpi.first.networktables.BooleanEntry; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.function.BooleanConsumer; - -/** Add your docs here. */ -public class DynamicNTBoolean { - String name; - boolean val; - BooleanEntry ntEntry; - BooleanConsumer update; - public static Map avail=new HashMap(); - public static void periodic(){ - for(DynamicNTBoolean v:avail.values()){ - boolean prev=v.val; - v.val=v.ntEntry.get(false); - if(v.val!=prev){ - v.update.accept(v.val); - } - - } - } - public void Register(){ - if(avail.containsKey(name)){ - throw new IllegalArgumentException("DynamicNTBoolean with name "+name+" already exists"); - } - avail.put(name, this); - } - public DynamicNTBoolean(String name, boolean val, BooleanConsumer update){ - this.name=name; - this.val=val; - this.update=update; - this.ntEntry=NetworkTableInstance.getDefault().getBooleanTopic(name).getEntry(false); - this.ntEntry.set(val); - } -} diff --git a/src/main/java/frc/robot/util/DynamicNTDouble.java b/src/main/java/frc/robot/util/DynamicNTDouble.java deleted file mode 100644 index fc0db4c..0000000 --- a/src/main/java/frc/robot/util/DynamicNTDouble.java +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.HashMap; -import java.util.Map; -import java.util.function.DoubleConsumer; - -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -/** Add your docs here. */ -public class DynamicNTDouble { - String name; - double val; - DoubleEntry ntEntry; - DoubleConsumer update; - public static Map avail=new HashMap(); - public static void periodic(){ - for(DynamicNTDouble v:avail.values()){ - double prev=v.val; - v.val=v.ntEntry.get(0); - if(v.val!=prev){ - v.update.accept(v.val); - } - - } - } - public void Register(){ - if(avail.containsKey(name)){ - throw new IllegalArgumentException("DynamicNTDouble with name "+name+" already exists"); - } - avail.put(name, this); - } - public DynamicNTDouble(String name, double val, DoubleConsumer update){ - this.name=name; - this.val=val; - this.update=update; - this.ntEntry=NetworkTableInstance.getDefault().getDoubleTopic(name).getEntry(0); - this.ntEntry.set(val); - } -} diff --git a/src/main/java/frc/robot/util/DynamicNTVal.java b/src/main/java/frc/robot/util/DynamicNTVal.java deleted file mode 100644 index 7b3b34c..0000000 --- a/src/main/java/frc/robot/util/DynamicNTVal.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -/** Add your docs here. */ -public class DynamicNTVal { - String name; - T val; - T prev; - NetworkTableEntry ntEntry; - Consumer update; - public static Map avail=new HashMap(); - public static void periodic(){ - for(DynamicNTVal v:avail.values()){ - v.prev=v.val; - v.val=v.MagicPull(); - if(!v.val.equals(v.prev)){ - v.update.accept(v.val); - } - - } - } - public void Register(){ - if(avail.containsKey(name)){ - throw new IllegalArgumentException("DynamicNTVal with name "+name+" already exists"); - } - avail.put(name, this); - } - public T MagicPull(){ - switch(ntEntry.getType()){ - case kDouble: - return (T)Double.valueOf(ntEntry.getDouble(0)); - case kBoolean: - return (T)Boolean.valueOf(ntEntry.getBoolean(false)); - case kString: - return (T)ntEntry.getString(""); - default: - throw new IllegalArgumentException("DynamicNTVal with name "+name+" has invalid type"); - } - } - // this is a magic function that allows you to put any type of value into the network table - public void MagicPut(T val){ - if(val instanceof Double) - ntEntry.setDouble((double)val); - else if(val instanceof Boolean) - ntEntry.setBoolean((boolean)val); - else if(val instanceof String) - ntEntry.setString((String)val); - else - throw new IllegalArgumentException("DynamicNTVal with name "+name+" has invalid type"); - - } - public DynamicNTVal(String name, T val, Consumer update){ - this.name=name; - this.val=val; - this.update=update; - this.ntEntry=NetworkTableInstance.getDefault().getEntry(name); - MagicPut(val); - } -} diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 1901fc7..d51ceee 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -4,27 +4,15 @@ package frc.robot.util; -import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Rotation; -import java.util.Arrays; -import java.util.HashMap; import java.util.Map; -import java.util.stream.Stream; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Unit; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.HumanControls; /** * A class contianing all the positions needed to move the superstructure into a @@ -61,14 +49,14 @@ public enum ReefSide { Left } - /* Starts from 3 O'clock on field render */ + /* Labels For Blue Side Of Field, Relative To Center Of Hex */ public enum Index { - One, - Two, - Three, - Four, - Five, - Six + One, // -X + Two, // -X, +Y + Three, // +X, +Y + Four, // +X + Five, // +X, -Y + Six // -X, -Y } public final static Distance fieldLengthMeters = Feet.of(57.53); diff --git a/src/main/java/frc/robot/util/PoseUtil.java b/src/main/java/frc/robot/util/PoseUtil.java index 66a344a..4dcb949 100644 --- a/src/main/java/frc/robot/util/PoseUtil.java +++ b/src/main/java/frc/robot/util/PoseUtil.java @@ -97,7 +97,7 @@ public static Pose2d findClosestPose(Pose2d[] poseList, Pose2d referencePose) { * @return the mirrored pose */ public static Pose2d mirrorPoseX(Pose2d pose){ - double fieldX = 17.5387; // Field length in meters; TODO: get the real number + double fieldX = 17.5387; // Field length in meters; pose=new Pose2d(fieldX-pose.getX(), pose.getY(), pose.getRotation()); pose.rotateBy(Rotation2d.fromRotations(0)); return pose; From ee7c5d501124c3241463144ee7155cfc6001ae2c Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Thu, 6 Mar 2025 23:11:02 -0800 Subject: [PATCH 04/37] Removed Problems And Unused Code --- src/main/java/frc/team696 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 index a86a539..f4befdf 160000 --- a/src/main/java/frc/team696 +++ b/src/main/java/frc/team696 @@ -1 +1 @@ -Subproject commit a86a539b21530af6334ce673a082747c2039a4a8 +Subproject commit f4befdfa479a23a6232b29dba58aafa556207eca From 2d8deb949b8a6cdf25695d7a4bb504268a08401c Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Fri, 7 Mar 2025 22:52:20 -0800 Subject: [PATCH 05/37] Ground Coral + Other Stuff --- .Glass/glass.json | 43 +++++++ src/main/java/frc/robot/BotConstants.java | 19 ++- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/HumanControls.java | 37 +----- src/main/java/frc/robot/Robot.java | 99 ++++++++------- .../robot/commands/MoveSuperStructure.java | 4 +- src/main/java/frc/robot/subsystems/Arm.java | 5 - .../java/frc/robot/subsystems/Elevator.java | 26 ++-- .../frc/robot/subsystems/GroundCoral.java | 113 ++++++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 25 ++++ src/main/java/frc/team696 | 2 +- 11 files changed, 281 insertions(+), 102 deletions(-) create mode 100644 .Glass/glass.json create mode 100644 src/main/java/frc/robot/subsystems/GroundCoral.java diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..b1479f5 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,43 @@ +{ + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "GroundCoral": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/Pose": "Field2d", + "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/Arm": "Subsystem", + "/SmartDashboard/ArmMinus": "Command", + "/SmartDashboard/ArmPlus": "Command", + "/SmartDashboard/Auto Chooser": "String Chooser", + "/SmartDashboard/Elevator": "Subsystem", + "/SmartDashboard/Elevator duty cycle up": "Command", + "/SmartDashboard/EndEffector": "Subsystem", + "/SmartDashboard/Face Hex": "Command", + "/SmartDashboard/Face Net": "Command", + "/SmartDashboard/GroundCoral": "Subsystem", + "/SmartDashboard/Module 0": "Mechanism2d", + "/SmartDashboard/Module 1": "Mechanism2d", + "/SmartDashboard/Module 2": "Mechanism2d", + "/SmartDashboard/Module 3": "Mechanism2d", + "/SmartDashboard/Reset Gyro": "Command", + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/Wrist": "Subsystem", + "/SmartDashboard/Zero Arm": "Command", + "/SmartDashboard/Zero Wrist": "Command" + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "696" + } +} diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 5880960..e62332b 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -48,7 +49,6 @@ public static class Elevator { public static class Arm { public static int masterID = 13; - public static int slaveID=16; public static TalonFXConfiguration cfg = new TalonFXConfiguration(); static { cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -87,4 +87,21 @@ public static class EndEffector { } } + public static class GroundCoral{ + public static int angleId=17; + public static int rollerId=18; + public static TalonFXConfiguration angleCfg=new TalonFXConfiguration(); + public static TalonFXConfiguration rollerCfg=new TalonFXConfiguration(); + static{ + angleCfg.Slot0.kP=10.; + angleCfg.CurrentLimits.StatorCurrentLimit=120; + angleCfg.CurrentLimits.StatorCurrentLimitEnable=true; + angleCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + rollerCfg.CurrentLimits.StatorCurrentLimit=120; + rollerCfg.CurrentLimits.StatorCurrentLimitEnable=true; + } + + } + } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 42838bc..34739f5 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 38; - public static final String GIT_SHA = "741e097e31f38bde771122224987235c8a208c9c"; - public static final String GIT_DATE = "2025-03-03 22:12:52 PST"; + public static final int GIT_REVISION = 41; + public static final String GIT_SHA = "ee7c5d501124c3241463144ee7155cfc6001ae2c"; + public static final String GIT_DATE = "2025-03-06 23:11:02 PST"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-02-13 20:45:12 PST"; - public static final long BUILD_UNIX_TIME = 1739508312539L; + public static final String BUILD_DATE = "2025-03-07 20:46:29 PST"; + public static final long BUILD_UNIX_TIME = 1741409189601L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/HumanControls.java b/src/main/java/frc/robot/HumanControls.java index a5730e4..02f4934 100644 --- a/src/main/java/frc/robot/HumanControls.java +++ b/src/main/java/frc/robot/HumanControls.java @@ -11,35 +11,10 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -/** Add your docs here. */ public final class HumanControls { - /*public final class OperatorPanel2024{ - public static final Joystick operatorPanel = new Joystick(1); - public static final Joystick operatorPanelB = new Joystick(2); - - public static final JoystickButton Shoot = new JoystickButton(operatorPanel, 1); - public static final JoystickButton Amp = new JoystickButton(operatorPanel, 2); - public static final JoystickButton ExtraA = new JoystickButton(operatorPanel, 3); - public static final JoystickButton Trap = new JoystickButton(operatorPanel, 4); - public static final JoystickButton ExtraB = new JoystickButton(operatorPanel, 6); - public static final JoystickButton Ground = new JoystickButton(operatorPanel, 7); - public static final JoystickButton Source = new JoystickButton(operatorPanel, 8); - public static final JoystickButton Rollers = new JoystickButton(operatorPanel, 9); - public static final JoystickButton Drop = new JoystickButton(operatorPanel, 10); - public static final JoystickButton ExtraC = new JoystickButton(operatorPanel,11); - - public static final JoystickButton Climb = new JoystickButton(operatorPanelB, 2); - public static final JoystickButton OhShit = new JoystickButton(operatorPanelB, 3); - public static final JoystickButton Rightest = new JoystickButton(operatorPanelB, 4); - public static final JoystickButton Right = new JoystickButton(operatorPanelB, 5); - public static final JoystickButton Left = new JoystickButton(operatorPanelB, 6); - public static final JoystickButton Gyro = new JoystickButton(operatorPanelB, 7); - public static final JoystickButton Leftest = new JoystickButton(operatorPanelB, 8); - }*/ public final class OperatorPanel2025{ public static final Joystick OperatorPanel=new Joystick(1); - public static final JoystickButton L1=new JoystickButton(OperatorPanel, 9); public static final JoystickButton L2=new JoystickButton(OperatorPanel, 10); public static final JoystickButton L3=new JoystickButton(OperatorPanel, 11); @@ -52,12 +27,16 @@ public final class OperatorPanel2025{ public static final JoystickButton gyro=new JoystickButton(OperatorPanel, 13); public static final JoystickButton Climb1=new JoystickButton(OperatorPanel, 4); public static final JoystickButton Climb2=new JoystickButton(OperatorPanel, 3); - public static final JoystickButton SouceCoral=new JoystickButton(OperatorPanel, 6); - public static final JoystickButton GroundCoral=new JoystickButton(OperatorPanel, 0); + public static final JoystickButton SouceCoral=new JoystickButton(OperatorPanel, 5); + public static final JoystickButton GroundCoral=new JoystickButton(OperatorPanel, 6); public static final JoystickButton Barge=new JoystickButton(OperatorPanel,2); public static final JoystickButton Processor=new JoystickButton(OperatorPanel, 1); public static final JoystickButton leftOrRight=new JoystickButton(OperatorPanel, 14); + public static final JoystickButton unlabedSwitch=new JoystickButton(OperatorPanel, 15); + public static final JoystickButton deepOrSwitch=new JoystickButton(OperatorPanel, 16); + + // Use one of the switches on the driver station for this //public static final BooleanSupplier manualOverride=new JoystickButton(null, 0); } @@ -90,8 +69,4 @@ public final class SingleXboxController{ public static final Trigger RT = controller.rightTrigger(0.5); } - public final class SinglePS4Controller{ - - } - } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dfb230b..7cc7153 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,12 +8,10 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.Utils; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; @@ -30,10 +28,10 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; import frc.robot.util.GameInfo; -import frc.team696.lib.Camera.LimelightHelpers; import frc.team696.lib.Logging.BackupLogger; import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; +import frc.robot.subsystems.GroundCoral; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -86,6 +84,7 @@ public Robot() { SmartDashboard.putData(Arm.get()); SmartDashboard.putData(EndEffector.get()); SmartDashboard.putData(Wrist.get()); + SmartDashboard.putData(GroundCoral.get()); Swerve.get().registerTelemetry(m_SwerveTelemetry::telemeterize); @@ -109,13 +108,6 @@ public Robot() { .withRotationalRate( (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / -120 * MaxRotationalRate))); - SmartDashboard.putData("Face Net", Swerve.get().applyRequest( - () -> Swerve.fcDriveReq.withVelocityX( - applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) - .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) - .withRotationalRate( - (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceNet())).getDegrees() / 30 * MaxRotationalRate))); - SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); NamedCommands.registerCommand("hello", new InstantCommand( @@ -135,32 +127,74 @@ public Robot() { private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(()->Swerve.get().seedFieldCentric())); - HumanControls.OperatorPanel2025.releaseCoral.onTrue(new InstantCommand(()-> {EndEffector.get().idlePower = -0.6;})); + HumanControls.OperatorPanel2025.releaseCoral.whileTrue( + new ConditionalCommand( + new InstantCommand(()-> {EndEffector.get().idlePower = -0.6;}), + GroundCoral.get().Spit(), + HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean) + ); + + + + HumanControls.OperatorPanel2025.unlabedSwitch.onTrue( + new InstantCommand(() -> { + Elevator.get().getCurrentCommand().cancel(); + GroundCoral.get().getCurrentCommand().cancel(); + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); + GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); + }) + ); + + HumanControls.OperatorPanel2025.unlabedSwitch.onFalse( + new InstantCommand(() -> { + Elevator.get().getCurrentCommand().cancel(); + GroundCoral.get().getCurrentCommand().cancel(); + + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(35)); + GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); + EndEffector.get().idlePower = 0; + }) + ); + + HumanControls.OperatorPanel2025.GroundCoral.whileTrue( + new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean) + ); + + HumanControls.OperatorPanel2025.L1.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.4), HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean - )); + ).deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource ))); HumanControls.OperatorPanel2025.L2.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.L2Algae, -0.8, false, -0.8), new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Back), -0.6), HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean - )); + ).deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource ))); HumanControls.OperatorPanel2025.L3.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.), new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6), HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean - )); + ).deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource ))); + + HumanControls.OperatorPanel2025.L4.whileTrue( + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6) + .deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource )) + ); // + + HumanControls.OperatorPanel2025.Barge.whileTrue( + new MoveSuperStructure(GameInfo.Net, 1.) + .deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceNet, Swerve.get()::FaceSource )) + ); + - HumanControls.OperatorPanel2025.L4.whileTrue(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6)); - HumanControls.OperatorPanel2025.Climb2.whileTrue(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.5, false, 0.2)); - HumanControls.OperatorPanel2025.Barge.whileTrue(new MoveSuperStructure(GameInfo.Net, 1.)); + HumanControls.OperatorPanel2025.SouceCoral.whileTrue(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.5, false, 0.2)); HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6)); //HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); @@ -171,19 +205,9 @@ public void robotPeriodic() { long start = RobotController.getTime(); CommandScheduler.getInstance().run(); long elapsed = RobotController.getTime() - start; - BackupLogger.addToQueue("SchedulerTimeMS", elapsed); // Scheduler Time in Microseconds, anything over 20,000 should - // trigger the watchdog - updateVision("limelight-corner"); + BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler Time in Microseconds, anything over 20,000 should + BackupLogger.logSystemInformation(); - - /* - * int i = 0; - * for (SwerveModule mod : - * CommandSwerveDrivetrain.get().getModules()) { - * SmartDashboard.putNumber("Encoder" + ++i, - * mod.getEncoder().getPosition().getValueAsDouble()); - * } - */ } @Override @@ -225,23 +249,6 @@ public void teleopInit() { } } - public void updateVision(String limelightName) { - double heading = Swerve.get().getState().Pose.getRotation().getDegrees(); - - LimelightHelpers.SetRobotOrientation(limelightName, heading, 0.0, 0.0, 0.0, 0.0, 0.0); - var measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - if (measurement != null) { - if (measurement.tagCount > 0 && measurement.rawFiducials[0].ambiguity < 0.5 - && measurement.rawFiducials[0].distToCamera < 5) { - // Experimental - double trustMetric = (Math.pow(measurement.rawFiducials[0].distToCamera, 2) - * measurement.rawFiducials[0].ambiguity / 35); - Swerve.get().setVisionMeasurementStdDevs(VecBuilder.fill(trustMetric, trustMetric, trustMetric)); - Swerve.get().addVisionMeasurement(measurement.pose, Utils.fpgaToCurrentTime(measurement.timestampSeconds)); - } - } - } - @Override public void teleopPeriodic() { diff --git a/src/main/java/frc/robot/commands/MoveSuperStructure.java b/src/main/java/frc/robot/commands/MoveSuperStructure.java index 52fad78..7aedf62 100644 --- a/src/main/java/frc/robot/commands/MoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/MoveSuperStructure.java @@ -43,7 +43,9 @@ public MoveSuperStructure(CoralScoringPosition position, double runRollers) { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index ef62289..2e212df 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -12,7 +12,6 @@ import java.util.function.DoubleSupplier; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -40,7 +39,6 @@ public static synchronized Arm get() { } TalonFX master = new TalonFX(BotConstants.Arm.masterID, BotConstants.rioBus); - TalonFX slave = new TalonFX(BotConstants.Arm.slaveID, BotConstants.rioBus); StatusSignal velocitySignal; StatusSignal positionSignal; StatusSignal voltageSignal; @@ -54,14 +52,11 @@ public static synchronized Arm get() { /** Creates a new Arm. */ private Arm() { master.getConfigurator().apply(BotConstants.Arm.cfg); - slave.getConfigurator().apply(BotConstants.Arm.cfg); velocitySignal = master.getVelocity(); positionSignal = master.getPosition(); voltageSignal = master.getMotorVoltage(); currentSignal = master.getStatorCurrent(); - slave.setControl(new Follower(master.getDeviceID(), false)); - zeroArm(); // this.setDefaultCommand(Position(()->0)); new TriggerNTDouble("testing/armAngle", ntpos, (ev) -> ntpos = ev); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index ddf2c69..dce3e8b 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -13,7 +13,6 @@ import com.ctre.phoenix6.controls.MotionMagicVoltage; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -48,7 +47,6 @@ private Elevator() { .linearPosition(Meters.of(master.getPosition())) .linearVelocity(MetersPerSecond.of(master.getVelocity())); }, this)); - SmartDashboard.putData("Elevator duty cycle up", runDutyCycle()); } public void stop() { @@ -62,8 +60,15 @@ public void DriveVoltage(Voltage v){ master.VoltageOut(v); } + public void goToPosition(double position) { + if (GroundCoral.get().getPosition() > .1) { + position = Math.max(position, 30); + } + master.get().setControl(positionReq.withPosition(position)); + } + public void goToPosition(GameInfo.CoralScoringPosition position) { - master.get().setControl(positionReq.withPosition(position.height)); + goToPosition(position.height); } public double getPosition() { @@ -76,31 +81,28 @@ public double getPosition() { * @return the command which holds the elevator at position (requires this subsystem) */ public Command positionCommand(GameInfo.CoralScoringPosition position){ - return this.runEnd(()->master.get().setControl(positionReq.withPosition(position.height)), ()->master.get().set(0)); - } - public Command positionCommandRun(GameInfo.CoralScoringPosition position){ - return this.runEnd(()->master.setControl(positionReq.withPosition(position.height)), ()->master.VoltageOut(Volts.of(0))); + return this.runEnd(()->goToPosition(position), ()->master.get().set(0)); } public Command positionCommand(double position){ - return this.startEnd(()->master.setControl(positionReq.withPosition(position)), ()->master.VoltageOut(Volts.of(0))); + return this.runEnd(()->goToPosition(position), ()->master.VoltageOut(Volts.of(0))); } /** * Holds the current position * @return a command that holds the elevator at the position it was in at schedluing time */ public Command holdPosition(){ - return this.startEnd(()->master.setControl(positionReq.withPosition(master.getPosition())), ()->master.VoltageOut(Volts.of(0))); - } - public Command runDutyCycle(){ - return this.startEnd(()->master.PercentOutput(.4), ()->master.stop()); + return this.startEnd(()->goToPosition(master.getPosition()), ()->master.VoltageOut(Volts.of(0))); } + public void zero(){ resetPosition(0); } + public void resetPosition(double newPosition){ master.setPosition(newPosition); slave.setPosition(newPosition); } + @Override public void periodic() { BackupLogger.addToQueue("Elevator/MasterCurrent", master.getCurrent()); diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java new file mode 100644 index 0000000..ee37c9d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -0,0 +1,113 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amp; +import static edu.wpi.first.units.Units.Rotation; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.BotConstants; + +/** + * represents the other coral system that picks up from ground and can score L1 + */ +public class GroundCoral extends SubsystemBase { + private static GroundCoral m_GroundCoral = null; + + public static synchronized final GroundCoral get() { + if (m_GroundCoral == null) { + m_GroundCoral = new GroundCoral(); + } + return m_GroundCoral; + } + + TalonFX angleMotor = new TalonFX(BotConstants.GroundCoral.angleId, BotConstants.rioBus); + TalonFX rollerMotor = new TalonFX(BotConstants.GroundCoral.rollerId, BotConstants.rioBus); + PositionDutyCycle positionRequest = new PositionDutyCycle(0); + + private GroundCoral() { + angleMotor.getConfigurator().apply(BotConstants.GroundCoral.angleCfg); + rollerMotor.getConfigurator().apply(BotConstants.GroundCoral.rollerCfg); + zero(); + this.setDefaultCommand(this.runEnd(() -> { + angleMotor.setControl(positionRequest.withPosition(0)); + }, () -> { + angleMotor.stopMotor(); + })); + } + + public void resetPosition(double newPosition) { + angleMotor.setPosition(newPosition); + } + + public void zero() { + resetPosition(0); + } + + public void stop(){ + angleMotor.stopMotor(); + rollerMotor.stopMotor(); + } + + public boolean isStalling(){ + return rollerMotor.getStatorCurrent().getValue().in(Amp)>(BotConstants.GroundCoral.rollerCfg.CurrentLimits.StatorCurrentLimit-20); + } + + public double getPosition(){ + return angleMotor.getPosition().getValue().in(Rotation); + } + + public void position(double position) { + if (Elevator.get().getPosition() >= 30) { + angleMotor.setControl(positionRequest.withPosition(position)); + } else { + stop(); + } + } + + public Command Intake() { + return this.startEnd(() -> { + position(1.2); + rollerMotor.set(1.); + }, () -> { + angleMotor.stopMotor(); + rollerMotor.stopMotor(); + }); + } + + public Command Stowed() { + return this.runEnd( + ()->{position(0); rollerMotor.stopMotor();}, + this::stop + ); + } + + public Command Ready() { + return this.runEnd( + ()->{position(0.6); rollerMotor.stopMotor();}, + this::stop + ); + } + + public Command Spit() { + return this.runEnd(()-> { + position(0.6); + if (getPosition() > 30) { + rollerMotor.set(-0.6); + } + }, this::stop); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("GroundCoral/Position", getPosition()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 8d6162b..4ca22bf 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -27,12 +27,14 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.TunerConstants; import frc.robot.TunerConstants.TunerSwerveDrivetrain; import frc.robot.util.GameInfo; import frc.team696.lib.Util; +import frc.team696.lib.Camera.LimeLightCam; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements @@ -44,6 +46,25 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private double m_lastSimTime; private static Swerve m_CommandSwerveDrivetrain=null; + + public LimeLightCam CamA = new LimeLightCam("B"); + public LimeLightCam CamB = new LimeLightCam("A"); + + public Supplier goalRotation = () -> new Rotation2d(); + + public Command setGoalRotation(Supplier during, Supplier after) { + return Commands.startEnd( ()-> { + if (during != null) { + goalRotation = during; + } + }, ()-> { + if (after != null) { + goalRotation = after; + } + } ); + + } + public static synchronized Swerve get(){ if(m_CommandSwerveDrivetrain==null){ m_CommandSwerveDrivetrain=TunerConstants.createDrivetrain(); @@ -250,6 +271,10 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + + CamA.addVisionEstimate(this::addVisionMeasurement); + CamB.addVisionEstimate(this::addVisionMeasurement); + } private void startSimThread() { diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 index f4befdf..8c2d365 160000 --- a/src/main/java/frc/team696 +++ b/src/main/java/frc/team696 @@ -1 +1 @@ -Subproject commit f4befdfa479a23a6232b29dba58aafa556207eca +Subproject commit 8c2d365965048681ed6e20f4e4d8e7c2cdc7e6f0 From 50c3cfd6c8bbceb56863650aec9ee4f6828a8f8f Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Fri, 7 Mar 2025 22:55:00 -0800 Subject: [PATCH 06/37] Updated Library --- src/main/java/frc/team696 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 index 8c2d365..d2e9786 160000 --- a/src/main/java/frc/team696 +++ b/src/main/java/frc/team696 @@ -1 +1 @@ -Subproject commit 8c2d365965048681ed6e20f4e4d8e7c2cdc7e6f0 +Subproject commit d2e9786db4bb80ee08f9b2b038f0f10afe81889b From 0f683ed33a21a6605208f39623b4f2b3acc984a1 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Sun, 9 Mar 2025 00:30:03 -0800 Subject: [PATCH 07/37] Updated Ratio plus other changes --- src/main/java/frc/robot/BotConstants.java | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 129 ++-- .../java/frc/robot/subsystems/Elevator.java | 67 +- .../frc/robot/subsystems/GroundCoral.java | 6 +- .../java/frc/robot/subsystems/Swerve.java | 635 +++++++++--------- src/main/java/frc/robot/util/GameInfo.java | 5 +- src/main/java/frc/robot/util/PoseUtil.java | 105 --- 8 files changed, 444 insertions(+), 515 deletions(-) delete mode 100644 src/main/java/frc/robot/util/PoseUtil.java diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index e62332b..6e41767 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -93,7 +93,7 @@ public static class GroundCoral{ public static TalonFXConfiguration angleCfg=new TalonFXConfiguration(); public static TalonFXConfiguration rollerCfg=new TalonFXConfiguration(); static{ - angleCfg.Slot0.kP=10.; + angleCfg.Slot0.kP=1.; angleCfg.CurrentLimits.StatorCurrentLimit=120; angleCfg.CurrentLimits.StatorCurrentLimitEnable=true; angleCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 34739f5..6fc8317 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 41; - public static final String GIT_SHA = "ee7c5d501124c3241463144ee7155cfc6001ae2c"; - public static final String GIT_DATE = "2025-03-06 23:11:02 PST"; + public static final int GIT_REVISION = 43; + public static final String GIT_SHA = "50c3cfd6c8bbceb56863650aec9ee4f6828a8f8f"; + public static final String GIT_DATE = "2025-03-07 22:55:00 PST"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-07 20:46:29 PST"; - public static final long BUILD_UNIX_TIME = 1741409189601L; + public static final String BUILD_DATE = "2025-03-09 00:21:07 PST"; + public static final long BUILD_UNIX_TIME = 1741508467604L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7cc7153..dea458a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,7 +39,6 @@ public class Robot extends TimedRobot { private double MaxRotationalRate = RotationsPerSecond.of(10).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); - private void logBuildInfo() { BackupLogger.addToQueue("BuildConstants/ProjectName", BuildConstants.MAVEN_NAME); BackupLogger.addToQueue("BuildConstants/BuildDate", BuildConstants.BUILD_DATE); @@ -64,7 +63,6 @@ public double applyDeadband(double x, double deadband) { return Math.abs(x) < deadband ? 0 : x; } - public void putSwerveSysIDCalibrationButtons() { SmartDashboard.putData("CTRESwerveCalibrationc/DynamicForward", Swerve.get().sysIdDynamic(SysIdRoutine.Direction.kForward)); @@ -96,17 +94,21 @@ public Robot() { Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( - Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.08),2) * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) - .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.08),2) * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) + Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.08), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.08), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) .withRotationalRate( - Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.08), 2) * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); + Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.08), 2) + * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); SmartDashboard.putData("Face Hex", Swerve.get().applyRequest( - () -> Swerve.fcDriveReq.withVelocityX( - applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) - .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) - .withRotationalRate( - (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / -120 * MaxRotationalRate))); + () -> Swerve.fcDriveReq.withVelocityX( + applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) + .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) + .withRotationalRate( + (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / -120 + * MaxRotationalRate))); SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); @@ -121,83 +123,77 @@ public Robot() { Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); Wrist.get().setDefaultCommand(Wrist.get().Position(0)); - Arm.get().setDefaultCommand(Arm.get().Position(()->0)); - EndEffector.get().setDefaultCommand(EndEffector.get().spin(()->EndEffector.get().idlePower)); + Arm.get().setDefaultCommand(Arm.get().Position(() -> 0)); + EndEffector.get().setDefaultCommand(EndEffector.get().spin(() -> EndEffector.get().idlePower)); } private void configureDriverStationBinds() { - HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(()->Swerve.get().seedFieldCentric())); + HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(() -> Swerve.get().seedFieldCentric())); HumanControls.OperatorPanel2025.releaseCoral.whileTrue( - new ConditionalCommand( - new InstantCommand(()-> {EndEffector.get().idlePower = -0.6;}), - GroundCoral.get().Spit(), - HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean) - ); - - + new ConditionalCommand( + new InstantCommand(() -> { + EndEffector.get().idlePower = -0.6; + }), + GroundCoral.get().Spit(), + HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean)); HumanControls.OperatorPanel2025.unlabedSwitch.onTrue( - new InstantCommand(() -> { - Elevator.get().getCurrentCommand().cancel(); - GroundCoral.get().getCurrentCommand().cancel(); - Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); - GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); - }) - ); + new InstantCommand(() -> { + Elevator.get().getCurrentCommand().cancel(); + GroundCoral.get().getCurrentCommand().cancel(); + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); + GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); + })); HumanControls.OperatorPanel2025.unlabedSwitch.onFalse( - new InstantCommand(() -> { - Elevator.get().getCurrentCommand().cancel(); - GroundCoral.get().getCurrentCommand().cancel(); + new InstantCommand(() -> { + Elevator.get().getCurrentCommand().cancel(); + GroundCoral.get().getCurrentCommand().cancel(); - Elevator.get().setDefaultCommand(Elevator.get().positionCommand(35)); - GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); - EndEffector.get().idlePower = 0; - }) - ); + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(35)); + GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); + EndEffector.get().idlePower = 0; + })); HumanControls.OperatorPanel2025.GroundCoral.whileTrue( - new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean) - ); - - + new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), + HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean)); HumanControls.OperatorPanel2025.L1.whileTrue( - new ConditionalCommand( - new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.4), - HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean - ).deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource ))); - + new ConditionalCommand( + new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.4), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); + HumanControls.OperatorPanel2025.L2.whileTrue( - new ConditionalCommand( - new MoveSuperStructure(GameInfo.L2Algae, -0.8, false, -0.8), - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Back), -0.6), - HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean - ).deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource ))); + new ConditionalCommand( + new MoveSuperStructure(GameInfo.L2Algae, -0.8, false, -0.8), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Back), -0.6), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); HumanControls.OperatorPanel2025.L3.whileTrue( - new ConditionalCommand( - new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.), - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6), - HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean - ).deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource ))); + new ConditionalCommand( + new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); HumanControls.OperatorPanel2025.L4.whileTrue( - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6) - .deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceHexFace, Swerve.get()::FaceSource )) - ); // + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); // HumanControls.OperatorPanel2025.Barge.whileTrue( - new MoveSuperStructure(GameInfo.Net, 1.) - .deadlineFor(Swerve.get().setGoalRotation( Swerve.get()::FaceNet, Swerve.get()::FaceSource )) - ); + new MoveSuperStructure(GameInfo.Net, 1.) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceNet, Swerve.get()::FaceSource))); - - HumanControls.OperatorPanel2025.SouceCoral.whileTrue(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.5, false, 0.2)); + HumanControls.OperatorPanel2025.SouceCoral.whileTrue(new MoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.5, false, 0.2)); HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6)); - //HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); + // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new + // MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); } @Override @@ -205,8 +201,9 @@ public void robotPeriodic() { long start = RobotController.getTime(); CommandScheduler.getInstance().run(); long elapsed = RobotController.getTime() - start; - BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler Time in Microseconds, anything over 20,000 should - + BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler Time in Microseconds, anything over + // 20,000 should + BackupLogger.logSystemInformation(); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index dce3e8b..bfc58c3 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -22,46 +22,52 @@ import frc.team696.lib.Logging.BackupLogger; public class Elevator extends SubsystemBase { - private static Elevator m_Elevator=null; - public static synchronized Elevator get(){ - if(m_Elevator==null){ - m_Elevator=new Elevator(); + private static Elevator m_Elevator = null; + + public static synchronized Elevator get() { + if (m_Elevator == null) { + m_Elevator = new Elevator(); } return m_Elevator; } + private TalonFactory master, slave; private MotionMagicVoltage positionReq; public SysIdRoutine identificationRoutine; private Elevator() { - master=new TalonFactory(BotConstants.Elevator.masterID, BotConstants.rioBus, BotConstants.Elevator.cfg, "Elevator Master"); - slave=new TalonFactory(BotConstants.Elevator.slaveId,BotConstants.rioBus, BotConstants.Elevator.cfg, "Elevator Slave"); + master = new TalonFactory(BotConstants.Elevator.masterID, BotConstants.rioBus, BotConstants.Elevator.cfg, + "Elevator Master"); + slave = new TalonFactory(BotConstants.Elevator.slaveId, BotConstants.rioBus, BotConstants.Elevator.cfg, + "Elevator Slave"); slave.Follow(master, true); - - positionReq=new MotionMagicVoltage(0).withSlot(0); + + positionReq = new MotionMagicVoltage(0).withSlot(0); zero(); - identificationRoutine=new SysIdRoutine(new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(0.4),Seconds.of(3)), - new SysIdRoutine.Mechanism(this::DriveVoltage, (log)->{ - log.motor(master.getName()).voltage(master.get().getMotorVoltage().getValue()) - .linearPosition(Meters.of(master.getPosition())) - .linearVelocity(MetersPerSecond.of(master.getVelocity())); - }, this)); + identificationRoutine = new SysIdRoutine( + new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(0.4), Seconds.of(3)), + new SysIdRoutine.Mechanism(this::DriveVoltage, (log) -> { + log.motor(master.getName()).voltage(master.get().getMotorVoltage().getValue()) + .linearPosition(Meters.of(master.getPosition())) + .linearVelocity(MetersPerSecond.of(master.getVelocity())); + }, this)); } public void stop() { master.stop(); } + /** * * Use ONLY for SysID. Sets the elevator motors to a specific voltage */ - public void DriveVoltage(Voltage v){ + public void DriveVoltage(Voltage v) { master.VoltageOut(v); } public void goToPosition(double position) { - if (GroundCoral.get().getPosition() > .1) { + if (GroundCoral.get().getPosition() > .5) { position = Math.max(position, 30); } master.get().setControl(positionReq.withPosition(position)); @@ -69,36 +75,43 @@ public void goToPosition(double position) { public void goToPosition(GameInfo.CoralScoringPosition position) { goToPosition(position.height); - } + } public double getPosition() { return master.getPosition(); } + /** * * Moves to and holds a position + * * @param position The scoring position to hold - * @return the command which holds the elevator at position (requires this subsystem) + * @return the command which holds the elevator at position (requires this + * subsystem) */ - public Command positionCommand(GameInfo.CoralScoringPosition position){ - return this.runEnd(()->goToPosition(position), ()->master.get().set(0)); + public Command positionCommand(GameInfo.CoralScoringPosition position) { + return this.runEnd(() -> goToPosition(position), () -> master.get().set(0)); } - public Command positionCommand(double position){ - return this.runEnd(()->goToPosition(position), ()->master.VoltageOut(Volts.of(0))); + + public Command positionCommand(double position) { + return this.runEnd(() -> goToPosition(position), () -> master.VoltageOut(Volts.of(0))); } + /** * Holds the current position - * @return a command that holds the elevator at the position it was in at schedluing time + * + * @return a command that holds the elevator at the position it was in at + * schedluing time */ - public Command holdPosition(){ - return this.startEnd(()->goToPosition(master.getPosition()), ()->master.VoltageOut(Volts.of(0))); + public Command holdPosition() { + return this.startEnd(() -> goToPosition(master.getPosition()), () -> master.VoltageOut(Volts.of(0))); } - public void zero(){ + public void zero() { resetPosition(0); } - public void resetPosition(double newPosition){ + public void resetPosition(double newPosition) { master.setPosition(newPosition); slave.setPosition(newPosition); } diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index ee37c9d..a2dd52d 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -74,7 +74,7 @@ public void position(double position) { public Command Intake() { return this.startEnd(() -> { - position(1.2); + position(1.2 * 9.); rollerMotor.set(1.); }, () -> { angleMotor.stopMotor(); @@ -91,14 +91,14 @@ public Command Stowed() { public Command Ready() { return this.runEnd( - ()->{position(0.6); rollerMotor.stopMotor();}, + ()->{position(0.6 * 9.); rollerMotor.stopMotor();}, this::stop ); } public Command Spit() { return this.runEnd(()-> { - position(0.6); + position(0.6 * 9.); if (getPosition() > 30) { rollerMotor.set(-0.6); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4ca22bf..095d13c 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -41,324 +41,351 @@ * Subsystem so it can easily be used in command-based projects. */ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - private static Swerve m_CommandSwerveDrivetrain=null; - - public LimeLightCam CamA = new LimeLightCam("B"); - public LimeLightCam CamB = new LimeLightCam("A"); - - public Supplier goalRotation = () -> new Rotation2d(); - - public Command setGoalRotation(Supplier during, Supplier after) { - return Commands.startEnd( ()-> { - if (during != null) { - goalRotation = during; - } - }, ()-> { - if (after != null) { - goalRotation = after; - } - } ); + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; - } - - public static synchronized Swerve get(){ - if(m_CommandSwerveDrivetrain==null){ - m_CommandSwerveDrivetrain=TunerConstants.createDrivetrain(); - } - return m_CommandSwerveDrivetrain; - } - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - Seconds.of(10), // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public Swerve( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public Swerve( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } + private static Swerve m_CommandSwerveDrivetrain = null; - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]įµ€, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]įµ€, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public Swerve( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } + public LimeLightCam CamA = new LimeLightCam("B"); + public LimeLightCam CamB = new LimeLightCam("A"); - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } + public Supplier goalRotation = () -> new Rotation2d(); - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(12, 0, 0), - // PID constants for rotation - new PIDConstants(8, 0, 0) - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - public static SwerveRequest.ApplyRobotSpeeds rcDriveReq=new SwerveRequest.ApplyRobotSpeeds().withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage).withSteerRequestType(SteerRequestType.Position); - public static SwerveRequest.FieldCentric fcDriveReq=new SwerveRequest.FieldCentric().withForwardPerspective(ForwardPerspectiveValue.BlueAlliance).withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage).withSteerRequestType(SteerRequestType.Position); - public void Drive(ChassisSpeeds c){ - setControl(rcDriveReq.withSpeeds(c)); - } - public void Drive(ChassisSpeeds c, boolean fieldRelative){ - if(fieldRelative){ - this.setControl(fcDriveReq.withVelocityX(c.vxMetersPerSecond).withVelocityY(c.vyMetersPerSecond).withRotationalRate(c.omegaRadiansPerSecond)); - }else{ - Drive(c); - } - } + public Command setGoalRotation(Supplier during, Supplier after) { + return Commands.startEnd(() -> { + if (during != null) { + goalRotation = during; + } + }, () -> { + if (after != null) { + goalRotation = after; + } + }); - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - - CamA.addVisionEstimate(this::addVisionMeasurement); - CamB.addVisionEstimate(this::addVisionMeasurement); + } + public static synchronized Swerve get() { + if (m_CommandSwerveDrivetrain == null) { + m_CommandSwerveDrivetrain = TunerConstants.createDrivetrain(); } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); + return m_CommandSwerveDrivetrain; + } + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + /* + * SysId routine for characterizing translation. This is used to find PID gains + * for the drive motors. + */ + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + Seconds.of(10), // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this)); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public Swerve( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); } - public Pose2d getPose(){ - return getState().Pose; + configureAutoBuilder(); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public Swerve( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); } - /** - * - * @param position Position for distance from - * @return distance from position argument - */ - public double distTo(Translation2d position) { - return getPose().getTranslation().getDistance(position); + configureAutoBuilder(); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz + * on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation + * in the form [x, y, theta]įµ€, with units in + * meters + * and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation + * in the form [x, y, theta]įµ€, with units in + * meters + * and radians + * @param modules Constants for each specific module + */ + public Swerve( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + if (Utils.isSimulation()) { + startSimThread(); } - - /** - * - * @param position Position for distance from - * @return distance from position argument - */ - public double distTo(Pose2d position) { - return distTo(position.getTranslation()); + configureAutoBuilder(); + } + + /** + * Returns a command that applies the specified control request to this swerve + * drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); + + private void configureAutoBuilder() { + try { + var config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, // Supplier of current robot pose + this::resetPose, // Consumer for seeding pose against auto + () -> getState().Speeds, // Supplier of current robot speeds + // Consumer of ChassisSpeeds and feedforwards to drive the robot + (speeds, feedforwards) -> setControl( + m_pathApplyRobotSpeeds.withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(12, 0, 0), + // PID constants for rotation + new PIDConstants(8, 0, 0)), + config, + // Assume the path needs to be flipped for Red vs Blue, this is normally the + // case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this // Subsystem for requirements + ); + } catch (Exception ex) { + DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); } - - /** - * - * @param position Position for angle to - * @return Angle to Position - */ - public Rotation2d angleTo(Translation2d position) { - Translation2d delta = getPose().getTranslation().minus(position); - Rotation2d rot = Rotation2d.fromRadians(Math.atan2(delta.getY(), delta.getX())); - return rot; + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + public static SwerveRequest.ApplyRobotSpeeds rcDriveReq = new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.Position); + public static SwerveRequest.FieldCentric fcDriveReq = new SwerveRequest.FieldCentric() + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.Position); + + public void Drive(ChassisSpeeds c) { + setControl(rcDriveReq.withSpeeds(c)); + } + + public void Drive(ChassisSpeeds c, boolean fieldRelative) { + if (fieldRelative) { + this.setControl(fcDriveReq.withVelocityX(c.vxMetersPerSecond).withVelocityY(c.vyMetersPerSecond) + .withRotationalRate(c.omegaRadiansPerSecond)); + } else { + Drive(c); } - - /** - * - * @param position Position for angle to - * @return Angle to Position + } + + @Override + public void periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply + * it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts + * mid-match. + * Otherwise, only check and apply the operator perspective if the DS is + * disabled. + * This ensures driving behavior doesn't change until an explicit disable event + * occurs during testing. */ - public Rotation2d angleTo(Pose2d position) { - return angleTo(position.getTranslation()); - } - - public Rotation2d FaceHexFace() { - Translation2d reefPosition = Util.getAlliance() == Alliance.Blue ? GameInfo.blueReef : (new Translation2d(GameInfo.fieldLengthMeters.in(Meters) - GameInfo.blueReef.getX(),GameInfo.blueReef.getY())); - - Rotation2d angleToReef = getPose().getTranslation().minus(reefPosition).getAngle(); - - Rotation2d hexAngleToReef = Rotation2d.fromDegrees(((int)((angleToReef.getDegrees() + 30) / 60)) * 60.); - - return hexAngleToReef; - } - - public Rotation2d FaceNet() { - return Util.getAlliance() == Alliance.Blue ? Rotation2d.fromDegrees(-90) : Rotation2d.fromDegrees(90); + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); } - public Rotation2d FaceSource() { - if (Util.getAlliance() == Alliance.Blue) { - if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { - return Rotation2d.fromDegrees(-45); - } else { - return Rotation2d.fromDegrees(45); - } - } else { - if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { - return Rotation2d.fromDegrees(135); - } else { - return Rotation2d.fromDegrees(-135); - } - } + CamA.addVisionEstimate(this::addVisionMeasurement); + CamB.addVisionEstimate(this::addVisionMeasurement); + + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + public Pose2d getPose() { + return getState().Pose; + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Translation2d position) { + return getPose().getTranslation().getDistance(position); + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Pose2d position) { + return distTo(position.getTranslation()); + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Translation2d position) { + Translation2d delta = getPose().getTranslation().minus(position); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(delta.getY(), delta.getX())); + return rot; + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Pose2d position) { + return angleTo(position.getTranslation()); + } + + public Rotation2d FaceHexFace() { + Translation2d reefPosition = Util.getAlliance() == Alliance.Blue ? GameInfo.blueReef + : (new Translation2d(GameInfo.fieldLengthMeters.in(Meters) - GameInfo.blueReef.getX(), + GameInfo.blueReef.getY())); + + Rotation2d angleToReef = getPose().getTranslation().minus(reefPosition).getAngle(); + + Rotation2d hexAngleToReef = Rotation2d.fromDegrees(((int) ((angleToReef.getDegrees() + 30) / 60)) * 60.); + + return hexAngleToReef; + } + + public Rotation2d FaceNet() { + return Util.getAlliance() == Alliance.Blue ? Rotation2d.fromDegrees(-90) : Rotation2d.fromDegrees(90); + } + + public Rotation2d FaceSource() { + if (Util.getAlliance() == Alliance.Blue) { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(-45); + } else { + return Rotation2d.fromDegrees(45); + } + } else { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(135); + } else { + return Rotation2d.fromDegrees(-135); + } } + } } diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index d51ceee..dec454b 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -139,17 +139,14 @@ RobotSide.Back, new CoralScoringPosition(62, -1.1, -8.3) RobotSide.Front, new CoralScoringPosition(6., 1., -0.9), RobotSide.Back, new CoralScoringPosition(0, 0, 0) ) - ); - L2Algae = new CoralScoringPosition (18., -3., 1.); L3Algae = new CoralScoringPosition (44., -3., 1.); - Net = new CoralScoringPosition(67., 0., 5.8); ClimbUp=new CoralScoringPosition(27, 0, 0); ClimbDown=new CoralScoringPosition(2, 0, 0); ground=new CoralScoringPosition(5., -6.3, -.5); - Processor = new CoralScoringPosition(0, 7., 11.9); + Processor = new CoralScoringPosition(0, -5., 1.); } } diff --git a/src/main/java/frc/robot/util/PoseUtil.java b/src/main/java/frc/robot/util/PoseUtil.java deleted file mode 100644 index 4dcb949..0000000 --- a/src/main/java/frc/robot/util/PoseUtil.java +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.List; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** Add your docs here. */ -public class PoseUtil { - - /** - * Finds the closest Pose2d to a reference Pose2d from a list of Pose2ds. - * "Closest" is determined by the Euclidean distance between the translation components (x, y) of the poses. - * Rotation is not considered in the distance calculation. - * - * @param poseList The list of Pose2ds to search within. Must not be null or empty. - * @param referencePose The Pose2d to find the closest Pose2d to. Must not be null. - * @return The Pose2d from the list that is closest to the referencePose, or null if the list is null or empty. - * @throws IllegalArgumentException if poseList or referencePose is null. - * @throws IllegalStateException if poseList is empty. - */ - public static Pose2d findClosestPose(List poseList, Pose2d referencePose) { - if (poseList == null) { - throw new IllegalArgumentException("poseList cannot be null"); - } - if (referencePose == null) { - throw new IllegalArgumentException("referencePose cannot be null"); - } - if (poseList.isEmpty()) { - return null; // Or throw an exception: throw new IllegalStateException("poseList cannot be empty"); - } - - Pose2d closestPose = null; - double minDistance = Double.MAX_VALUE; // Initialize with a very large value - - for (Pose2d pose : poseList) { - // Calculate the squared Euclidean distance between the translations - Translation2d poseTranslation = pose.getTranslation(); - Translation2d referenceTranslation = referencePose.getTranslation(); - double distance = referenceTranslation.getDistance(poseTranslation); - - if (distance < minDistance) { - minDistance = distance; - closestPose = pose; - } - } - - return closestPose; - } - /** - * Finds the closest Pose2d to a reference Pose2d from an array of Pose2ds. - * "Closest" is determined by the Euclidean distance between the translation components (x, y) of the poses. - * Rotation is not considered in the distance calculation. - * - * @param poseList The array of Pose2ds to search within. Must not be null or empty. - * @param referencePose The Pose2d to find the closest Pose2d to. Must not be null. - * @return The Pose2d from the array that is closest to the referencePose, or null if the array is null or empty. - * @throws IllegalArgumentException if poseList or referencePose is null. - * @throws IllegalStateException if poseList is empty. - */ - public static Pose2d findClosestPose(Pose2d[] poseList, Pose2d referencePose) { - if (poseList == null) { - throw new IllegalArgumentException("poseList cannot be null"); - } - if (referencePose == null) { - throw new IllegalArgumentException("referencePose cannot be null"); - } - if (poseList.length==0) { - return null; // Or throw an exception: throw new IllegalStateException("poseList cannot be empty"); - } - - Pose2d closestPose = null; - double minDistance = Double.MAX_VALUE; // Initialize with a very large value - - for (Pose2d pose : poseList) { - // Calculate the squared Euclidean distance between the translations - Translation2d poseTranslation = pose.getTranslation(); - Translation2d referenceTranslation = referencePose.getTranslation(); - double distance = referenceTranslation.getDistance(poseTranslation); - - if (distance < minDistance) { - minDistance = distance; - closestPose = pose; - } - } - - return closestPose; - } - /** - * Mirrors the pose across the alliance divider line - * @param pose the pose to mirror - * @return the mirrored pose - */ - public static Pose2d mirrorPoseX(Pose2d pose){ - double fieldX = 17.5387; // Field length in meters; - pose=new Pose2d(fieldX-pose.getX(), pose.getY(), pose.getRotation()); - pose.rotateBy(Rotation2d.fromRotations(0)); - return pose; - } -} From 67310794249e354938811afe4d6092355f89763d Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Sun, 9 Mar 2025 00:30:21 -0800 Subject: [PATCH 08/37] update submodule --- src/main/java/frc/team696 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 index d2e9786..43ea082 160000 --- a/src/main/java/frc/team696 +++ b/src/main/java/frc/team696 @@ -1 +1 @@ -Subproject commit d2e9786db4bb80ee08f9b2b038f0f10afe81889b +Subproject commit 43ea0823fca2ea2f9bc4f01dbfd6bb3514cac33e From afb86b5286e0cd32db74848fc49ba74e92b3990c Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Sun, 9 Mar 2025 13:03:59 -0700 Subject: [PATCH 09/37] Limelight Values --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- src/main/java/frc/team696 | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6fc8317..eff0323 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 43; - public static final String GIT_SHA = "50c3cfd6c8bbceb56863650aec9ee4f6828a8f8f"; - public static final String GIT_DATE = "2025-03-07 22:55:00 PST"; + public static final int GIT_REVISION = 45; + public static final String GIT_SHA = "67310794249e354938811afe4d6092355f89763d"; + public static final String GIT_DATE = "2025-03-09 00:30:21 PST"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-09 00:21:07 PST"; - public static final long BUILD_UNIX_TIME = 1741508467604L; + public static final String BUILD_DATE = "2025-03-09 12:52:40 PDT"; + public static final long BUILD_UNIX_TIME = 1741549960294L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 095d13c..dbb8325 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -47,7 +47,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private static Swerve m_CommandSwerveDrivetrain = null; - public LimeLightCam CamA = new LimeLightCam("B"); + public LimeLightCam CamA = new LimeLightCam("limelight-right"); public LimeLightCam CamB = new LimeLightCam("A"); public Supplier goalRotation = () -> new Rotation2d(); diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 index 43ea082..e86806b 160000 --- a/src/main/java/frc/team696 +++ b/src/main/java/frc/team696 @@ -1 +1 @@ -Subproject commit 43ea0823fca2ea2f9bc4f01dbfd6bb3514cac33e +Subproject commit e86806b12610b19054a842c1333b7b87c656e8c5 From 0a75d019cbe3709a1cd4453e87938f2fd91dd752 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Sun, 9 Mar 2025 23:41:29 -0700 Subject: [PATCH 10/37] Beginning Auto And Localization --- .../pathplanner/autos/NearCenterTruss.auto | 49 ---- .../pathplanner/autos/NearCenterTruss2.auto | 43 ---- .../deploy/pathplanner/autos/New Auto.auto | 22 +- .../autos/Test Auto (Simulation).auto | 49 ---- .../deploy/pathplanner/autos/centermove.auto | 19 -- src/main/deploy/pathplanner/autos/edge2.auto | 100 -------- .../deploy/pathplanner/autos/edgemove.auto | 19 -- .../deploy/pathplanner/autos/edgetour.auto | 133 ---------- .../deploy/pathplanner/autos/middlemove.auto | 19 -- .../deploy/pathplanner/autos/middletour.auto | 73 ------ .../pathplanner/paths/CenterTrussMove.path | 54 ---- .../paths/CenterTrussToReefER.path | 54 ---- .../paths/CenterTrussToSource.path | 54 ---- .../deploy/pathplanner/paths/EdgeMove.path | 54 ---- .../pathplanner/paths/EdgeToReefCL.path | 54 ---- .../pathplanner/paths/EdgeToReefCR.path | 54 ---- .../pathplanner/paths/EdgeToReefDL.path | 54 ---- .../{Example Path.path => First Test.path} | 24 +- src/main/deploy/pathplanner/paths/L4.path | 54 ---- .../deploy/pathplanner/paths/MiddleMove.path | 54 ---- .../pathplanner/paths/MiddleToReefCR.path | 54 ---- .../pathplanner/paths/New New New Path.path | 54 ---- .../pathplanner/paths/ReefALToSourceB.path | 54 ---- .../pathplanner/paths/ReefALToSourceR.path | 54 ---- .../pathplanner/paths/ReefARToSourceB.path | 54 ---- .../pathplanner/paths/ReefARToSourceR.path | 54 ---- .../pathplanner/paths/ReefBLToSourceB.path | 54 ---- .../pathplanner/paths/ReefBRToSourceB.path | 54 ---- .../pathplanner/paths/ReefCLToSourceB.path | 54 ---- .../pathplanner/paths/ReefCRToSourceB.path | 54 ---- .../pathplanner/paths/ReefELToSourceR.path | 54 ---- .../pathplanner/paths/ReefERToSourceB.path | 54 ---- .../pathplanner/paths/ReefERToSourceR.path | 54 ---- .../pathplanner/paths/ReefFLToSourceB.path | 54 ---- .../pathplanner/paths/ReefFLToSourceR.path | 54 ---- .../pathplanner/paths/ReefFRToSouceR.path | 54 ---- .../pathplanner/paths/ReefFRToSourceR.path | 54 ---- .../pathplanner/paths/ReefToSource.path | 54 ---- .../paths/{New Path.path => Second Test.path} | 26 +- .../pathplanner/paths/SourceBToReefAL.path | 54 ---- .../pathplanner/paths/SourceBToReefAR.path | 54 ---- .../pathplanner/paths/SourceBToReefBL.path | 54 ---- .../pathplanner/paths/SourceBToReefBR.path | 54 ---- .../pathplanner/paths/SourceBToReefCL.path | 54 ---- .../pathplanner/paths/SourceBToReefCR.path | 54 ---- .../pathplanner/paths/SourceBToReefFL.path | 54 ---- .../pathplanner/paths/SourceRToReefAL.path | 54 ---- .../pathplanner/paths/SourceRToReefAR.path | 54 ---- .../pathplanner/paths/SourceRToReefEL.path | 54 ---- .../pathplanner/paths/SourceRToReefER.path | 54 ---- .../pathplanner/paths/SourceRToReefFL.path | 54 ---- .../pathplanner/paths/SourceRToReefFR.path | 54 ---- .../pathplanner/paths/SourceToReef.path | 54 ---- .../pathplanner/paths/SourceToReef2.path | 54 ---- .../pathplanner/paths/backtoSource.path | 54 ---- src/main/deploy/pathplanner/settings.json | 36 ++- src/main/java/frc/robot/BotConstants.java | 152 +++++------ src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 50 ++-- .../commands/AutoMoveSuperStructure.java | 81 ++++++ .../robot/commands/MoveSuperStructure.java | 12 +- src/main/java/frc/robot/subsystems/Arm.java | 10 - .../java/frc/robot/subsystems/Elevator.java | 11 +- .../frc/robot/subsystems/EndEffector.java | 40 ++- .../frc/robot/subsystems/GroundCoral.java | 41 +-- .../java/frc/robot/subsystems/Swerve.java | 22 +- src/main/java/frc/robot/subsystems/Wrist.java | 15 +- src/main/java/frc/robot/util/GameInfo.java | 240 +++++++++--------- .../java/frc/robot/util/TriggerNTDouble.java | 37 --- 69 files changed, 425 insertions(+), 3230 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/NearCenterTruss.auto delete mode 100644 src/main/deploy/pathplanner/autos/NearCenterTruss2.auto delete mode 100644 src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto delete mode 100644 src/main/deploy/pathplanner/autos/centermove.auto delete mode 100644 src/main/deploy/pathplanner/autos/edge2.auto delete mode 100644 src/main/deploy/pathplanner/autos/edgemove.auto delete mode 100644 src/main/deploy/pathplanner/autos/edgetour.auto delete mode 100644 src/main/deploy/pathplanner/autos/middlemove.auto delete mode 100644 src/main/deploy/pathplanner/autos/middletour.auto delete mode 100644 src/main/deploy/pathplanner/paths/CenterTrussMove.path delete mode 100644 src/main/deploy/pathplanner/paths/CenterTrussToReefER.path delete mode 100644 src/main/deploy/pathplanner/paths/CenterTrussToSource.path delete mode 100644 src/main/deploy/pathplanner/paths/EdgeMove.path delete mode 100644 src/main/deploy/pathplanner/paths/EdgeToReefCL.path delete mode 100644 src/main/deploy/pathplanner/paths/EdgeToReefCR.path delete mode 100644 src/main/deploy/pathplanner/paths/EdgeToReefDL.path rename src/main/deploy/pathplanner/paths/{Example Path.path => First Test.path} (67%) delete mode 100644 src/main/deploy/pathplanner/paths/L4.path delete mode 100644 src/main/deploy/pathplanner/paths/MiddleMove.path delete mode 100644 src/main/deploy/pathplanner/paths/MiddleToReefCR.path delete mode 100644 src/main/deploy/pathplanner/paths/New New New Path.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefALToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefALToSourceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefARToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefARToSourceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefBLToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefBRToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefCLToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefCRToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefELToSourceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefERToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefERToSourceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefFLToSourceB.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefFLToSourceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefFRToSouceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefFRToSourceR.path delete mode 100644 src/main/deploy/pathplanner/paths/ReefToSource.path rename src/main/deploy/pathplanner/paths/{New Path.path => Second Test.path} (65%) delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefAL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefAR.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefBL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefBR.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefCL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefCR.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceBToReefFL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceRToReefAL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceRToReefAR.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceRToReefEL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceRToReefER.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceRToReefFL.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceRToReefFR.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceToReef.path delete mode 100644 src/main/deploy/pathplanner/paths/SourceToReef2.path delete mode 100644 src/main/deploy/pathplanner/paths/backtoSource.path create mode 100644 src/main/java/frc/robot/commands/AutoMoveSuperStructure.java delete mode 100644 src/main/java/frc/robot/util/TriggerNTDouble.java diff --git a/src/main/deploy/pathplanner/autos/NearCenterTruss.auto b/src/main/deploy/pathplanner/autos/NearCenterTruss.auto deleted file mode 100644 index b27f2ac..0000000 --- a/src/main/deploy/pathplanner/autos/NearCenterTruss.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterTrussToSource" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceToReef" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToSource" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceToReef2" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearCenterTruss2.auto b/src/main/deploy/pathplanner/autos/NearCenterTruss2.auto deleted file mode 100644 index c280e82..0000000 --- a/src/main/deploy/pathplanner/autos/NearCenterTruss2.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterTrussToReefER" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefERToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index e51a5ee..79184cd 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -7,13 +7,31 @@ { "type": "path", "data": { - "pathName": null + "pathName": "First Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Second Test" + } + }, + { + "type": "named", + "data": { + "name": "Intake" } } ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto b/src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto deleted file mode 100644 index 7caa292..0000000 --- a/src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Example Path" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "New Path" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/centermove.auto b/src/main/deploy/pathplanner/autos/centermove.auto deleted file mode 100644 index 0b097e3..0000000 --- a/src/main/deploy/pathplanner/autos/centermove.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterTrussMove" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/edge2.auto b/src/main/deploy/pathplanner/autos/edge2.auto deleted file mode 100644 index 45cac44..0000000 --- a/src/main/deploy/pathplanner/autos/edge2.auto +++ /dev/null @@ -1,100 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "EdgeToReefCL" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "named", - "data": { - "name": "ScoreL4" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCLToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefBR" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "named", - "data": { - "name": "ScoreL4" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "ReefBRToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefAR" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "named", - "data": { - "name": "ScoreL4" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/edgemove.auto b/src/main/deploy/pathplanner/autos/edgemove.auto deleted file mode 100644 index b9a251b..0000000 --- a/src/main/deploy/pathplanner/autos/edgemove.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "EdgeMove" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/edgetour.auto b/src/main/deploy/pathplanner/autos/edgetour.auto deleted file mode 100644 index e7ccc3d..0000000 --- a/src/main/deploy/pathplanner/autos/edgetour.auto +++ /dev/null @@ -1,133 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "EdgeToReefCR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCRToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefCL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCLToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefBL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefBLToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefAR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefARToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefAL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefALToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefFR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefFRToSouceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefFL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefFLToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefELToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefELToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefER" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefERToSourceR" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/middlemove.auto b/src/main/deploy/pathplanner/autos/middlemove.auto deleted file mode 100644 index 600be2c..0000000 --- a/src/main/deploy/pathplanner/autos/middlemove.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleMove" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/middletour.auto b/src/main/deploy/pathplanner/autos/middletour.auto deleted file mode 100644 index 89b07ad..0000000 --- a/src/main/deploy/pathplanner/autos/middletour.auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleToReefCR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCRToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefAL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefALToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefFR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefFRToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefELToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefAR" - } - }, - { - "type": "named", - "data": { - "name": "hello" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterTrussMove.path b/src/main/deploy/pathplanner/paths/CenterTrussMove.path deleted file mode 100644 index 2efe92c..0000000 --- a/src/main/deploy/pathplanner/paths/CenterTrussMove.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.53675, - "y": 5.0584999999999996 - }, - "prevControl": null, - "nextControl": { - "x": 7.120696721311476, - "y": 5.019979508196721 - }, - "isLocked": false, - "linkedName": "CenterTruss" - }, - { - "anchor": { - "x": 6.605225409836065, - "y": 5.0584999999999996 - }, - "prevControl": { - "x": 7.432377049180327, - "y": 5.05594262295082 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterTrussToReefER.path b/src/main/deploy/pathplanner/paths/CenterTrussToReefER.path deleted file mode 100644 index 881c488..0000000 --- a/src/main/deploy/pathplanner/paths/CenterTrussToReefER.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.53675, - "y": 5.0584999999999996 - }, - "prevControl": null, - "nextControl": { - "x": 7.02, - "y": 4.5417499999999995 - }, - "isLocked": false, - "linkedName": "CenterTruss" - }, - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": { - "x": 6.015749999999996, - "y": 3.313249999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefER" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterTrussToSource.path b/src/main/deploy/pathplanner/paths/CenterTrussToSource.path deleted file mode 100644 index 770cc7a..0000000 --- a/src/main/deploy/pathplanner/paths/CenterTrussToSource.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.516290983606557, - "y": 4.984016393442623 - }, - "prevControl": null, - "nextControl": { - "x": 6.617213114754098, - "y": 5.295696721311476 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.7861680327868852, - "y": 7.03391393442623 - }, - "prevControl": { - "x": 2.505430327868852, - "y": 6.362602459016394 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ex1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -51.48307369289725 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeMove.path b/src/main/deploy/pathplanner/paths/EdgeMove.path deleted file mode 100644 index b45499d..0000000 --- a/src/main/deploy/pathplanner/paths/EdgeMove.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595250000000001, - "y": 7.25225 - }, - "prevControl": null, - "nextControl": { - "x": 6.192276987840646, - "y": 7.251727329722346 - }, - "isLocked": false, - "linkedName": "Edge" - }, - { - "anchor": { - "x": 5.897950819672132, - "y": 7.25225 - }, - "prevControl": { - "x": 7.2699005723947545, - "y": 7.2338036884881465 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -179.70897632112784 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeToReefCL.path b/src/main/deploy/pathplanner/paths/EdgeToReefCL.path deleted file mode 100644 index 4353444..0000000 --- a/src/main/deploy/pathplanner/paths/EdgeToReefCL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595250000000001, - "y": 7.25225 - }, - "prevControl": null, - "nextControl": { - "x": 6.5325, - "y": 6.7452499999999995 - }, - "isLocked": false, - "linkedName": "Edge" - }, - { - "anchor": { - "x": 4.9822500000000005, - "y": 5.41925 - }, - "prevControl": { - "x": 5.46975, - "y": 6.140750000000001 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 57.680383491819796 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeToReefCR.path b/src/main/deploy/pathplanner/paths/EdgeToReefCR.path deleted file mode 100644 index 41c6f49..0000000 --- a/src/main/deploy/pathplanner/paths/EdgeToReefCR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595250000000001, - "y": 7.25225 - }, - "prevControl": null, - "nextControl": { - "x": 5.9475, - "y": 6.80375 - }, - "isLocked": false, - "linkedName": "Edge" - }, - { - "anchor": { - "x": 5.46975, - "y": 4.922 - }, - "prevControl": { - "x": 7.02725, - "y": 5.764749999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 58.172553423326704 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeToReefDL.path b/src/main/deploy/pathplanner/paths/EdgeToReefDL.path deleted file mode 100644 index 3ce9a60..0000000 --- a/src/main/deploy/pathplanner/paths/EdgeToReefDL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595250000000001, - "y": 7.25225 - }, - "prevControl": null, - "nextControl": { - "x": 7.991290801914323, - "y": 5.379697812362297 - }, - "isLocked": false, - "linkedName": "Edge" - }, - { - "anchor": { - "x": 6.221618852459016, - "y": 4.288729508196721 - }, - "prevControl": { - "x": 6.750602379014676, - "y": 5.379215035375417 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/First Test.path similarity index 67% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/First Test.path index b6074a8..e625933 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.60875, - "y": 6.8915 + "x": 7.576790942430968, + "y": 1.9549432938856013 }, "prevControl": null, "nextControl": { - "x": 1.8743422906562406, - "y": 6.845967803712558 + "x": 7.042790942430968, + "y": 1.9549432938856013 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6802254098360656, - "y": 5.343647540983607 + "x": 5.992617033407298, + "y": 1.9549432938856013 }, "prevControl": { - "x": 1.1203436468772154, - "y": 5.79078396275844 + "x": 6.593551220414204, + "y": 1.9294521233974358 }, "nextControl": null, "isLocked": false, - "linkedName": "ep1" + "linkedName": "Nuts" } ], "rotationTargets": [], @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 118.44292862436343 + "rotation": -90.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -155.6375469846878 + "rotation": -90.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L4.path b/src/main/deploy/pathplanner/paths/L4.path deleted file mode 100644 index 589685b..0000000 --- a/src/main/deploy/pathplanner/paths/L4.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.03483606557377, - "y": 5.40358606557377 - }, - "prevControl": { - "x": 4.03483606557377, - "y": 5.40358606557377 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ep2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 61.38954033403478 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleMove.path b/src/main/deploy/pathplanner/paths/MiddleMove.path deleted file mode 100644 index 96185dd..0000000 --- a/src/main/deploy/pathplanner/paths/MiddleMove.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.566, - "y": 6.17975 - }, - "prevControl": null, - "nextControl": { - "x": 6.58125, - "y": 6.158811475409837 - }, - "isLocked": false, - "linkedName": "MiddleStart" - }, - { - "anchor": { - "x": 5.993852459016393, - "y": 6.17975 - }, - "prevControl": { - "x": 6.988831967213113, - "y": 6.182786885245902 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleToReefCR.path b/src/main/deploy/pathplanner/paths/MiddleToReefCR.path deleted file mode 100644 index 088ed83..0000000 --- a/src/main/deploy/pathplanner/paths/MiddleToReefCR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.566, - "y": 6.17975 - }, - "prevControl": null, - "nextControl": { - "x": 6.579700584987288, - "y": 5.806952241681442 - }, - "isLocked": false, - "linkedName": "MiddleStart" - }, - { - "anchor": { - "x": 5.46975, - "y": 4.922 - }, - "prevControl": { - "x": 5.71019443267824, - "y": 4.990457832232982 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 58.172553423326704 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path deleted file mode 100644 index e1ab9b1..0000000 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.790250000000001, - "y": 4.960999999999999 - }, - "prevControl": { - "x": 6.790250000000001, - "y": 4.960999999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefALToSourceB.path b/src/main/deploy/pathplanner/paths/ReefALToSourceB.path deleted file mode 100644 index 6db7fa1..0000000 --- a/src/main/deploy/pathplanner/paths/ReefALToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": null, - "nextControl": { - "x": 2.7963988482503943, - "y": 4.814238006000046 - }, - "isLocked": false, - "linkedName": "ReefAL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.376469896774239, - "y": 5.438651880522935 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefALToSourceR.path b/src/main/deploy/pathplanner/paths/ReefALToSourceR.path deleted file mode 100644 index d5ee177..0000000 --- a/src/main/deploy/pathplanner/paths/ReefALToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": null, - "nextControl": { - "x": 2.9054999999999995, - "y": 3.3522499999999997 - }, - "isLocked": false, - "linkedName": "ReefAL" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 1.364999999997346, - "y": 3.556999999994158 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefARToSourceB.path b/src/main/deploy/pathplanner/paths/ReefARToSourceB.path deleted file mode 100644 index 973d33e..0000000 --- a/src/main/deploy/pathplanner/paths/ReefARToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": null, - "nextControl": { - "x": 4.003, - "y": 4.366249999999999 - }, - "isLocked": false, - "linkedName": "ReefAR" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 0.4722500000000003, - "y": 7.07675 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefARToSourceR.path b/src/main/deploy/pathplanner/paths/ReefARToSourceR.path deleted file mode 100644 index db29e1c..0000000 --- a/src/main/deploy/pathplanner/paths/ReefARToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": null, - "nextControl": { - "x": 2.7637177827401294, - "y": 3.528590662447068 - }, - "isLocked": false, - "linkedName": "ReefAR" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.242802121627151, - "y": 2.596402623347232 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefBLToSourceB.path b/src/main/deploy/pathplanner/paths/ReefBLToSourceB.path deleted file mode 100644 index 4b71cf0..0000000 --- a/src/main/deploy/pathplanner/paths/ReefBLToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.50025, - "y": 5.0584999999999996 - }, - "prevControl": null, - "nextControl": { - "x": 3.071038680859481, - "y": 5.479570082428542 - }, - "isLocked": false, - "linkedName": "ReefBL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.4611942558252036, - "y": 6.10867838534708 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -58.49573328079569 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefBRToSourceB.path b/src/main/deploy/pathplanner/paths/ReefBRToSourceB.path deleted file mode 100644 index 6b44e56..0000000 --- a/src/main/deploy/pathplanner/paths/ReefBRToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.8600409836065577, - "y": 5.475512295081967 - }, - "prevControl": null, - "nextControl": { - "x": 2.3419037208153863, - "y": 5.603040389952021 - }, - "isLocked": false, - "linkedName": "ReefBR" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.6688446379516346, - "y": 5.521446137788735 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 122.85572195043298 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefCLToSourceB.path b/src/main/deploy/pathplanner/paths/ReefCLToSourceB.path deleted file mode 100644 index 5ba7994..0000000 --- a/src/main/deploy/pathplanner/paths/ReefCLToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.9822500000000005, - "y": 5.41925 - }, - "prevControl": null, - "nextControl": { - "x": 3.7245000000000004, - "y": 5.858 - }, - "isLocked": false, - "linkedName": "ReefCL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.301, - "y": 6.735499999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 57.680383491819796 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefCRToSourceB.path b/src/main/deploy/pathplanner/paths/ReefCRToSourceB.path deleted file mode 100644 index 3e52ee6..0000000 --- a/src/main/deploy/pathplanner/paths/ReefCRToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.46975, - "y": 4.922 - }, - "prevControl": null, - "nextControl": { - "x": 4.5142500000000005, - "y": 5.94575 - }, - "isLocked": false, - "linkedName": "ReefCR" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 1.9305, - "y": 6.60875 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 58.172553423326704 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefELToSourceR.path b/src/main/deploy/pathplanner/paths/ReefELToSourceR.path deleted file mode 100644 index 670a576..0000000 --- a/src/main/deploy/pathplanner/paths/ReefELToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.92375, - "y": 2.61125 - }, - "prevControl": null, - "nextControl": { - "x": 3.4840908373840813, - "y": 2.328051729177785 - }, - "isLocked": false, - "linkedName": "ReefEL" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.0263976800287327, - "y": 1.4771932976546012 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -60.52411099675415 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefERToSourceB.path b/src/main/deploy/pathplanner/paths/ReefERToSourceB.path deleted file mode 100644 index c869ef4..0000000 --- a/src/main/deploy/pathplanner/paths/ReefERToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 4.894500000000001, - "y": 1.85075 - }, - "isLocked": false, - "linkedName": "ReefER" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.5545000000000004, - "y": 1.0415 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefERToSourceR.path b/src/main/deploy/pathplanner/paths/ReefERToSourceR.path deleted file mode 100644 index 30e2c34..0000000 --- a/src/main/deploy/pathplanner/paths/ReefERToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 5.21625, - "y": 2.4259999999999993 - }, - "isLocked": false, - "linkedName": "ReefER" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.2035, - "y": 1.694749999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFLToSourceB.path b/src/main/deploy/pathplanner/paths/ReefFLToSourceB.path deleted file mode 100644 index 9b4285c..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFLToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 2.807999999999999, - "y": 3.19625 - }, - "isLocked": false, - "linkedName": "ReefFL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 1.7945649676983555, - "y": 6.61128607889577 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFLToSourceR.path b/src/main/deploy/pathplanner/paths/ReefFLToSourceR.path deleted file mode 100644 index e2eff28..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFLToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 4.92925, - "y": 2.6014999999999997 - }, - "isLocked": false, - "linkedName": "ReefFL" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 0.20899999999999963, - "y": 1.1389999999999991 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFRToSouceR.path b/src/main/deploy/pathplanner/paths/ReefFRToSouceR.path deleted file mode 100644 index e48756e..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFRToSouceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.3735000000000004, - "y": 2.9817499999999995 - }, - "prevControl": null, - "nextControl": { - "x": 2.6908909470734246, - "y": 2.43103298536486 - }, - "isLocked": false, - "linkedName": "ReefFR" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.216165124821639, - "y": 1.9947836705032231 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 59.036243467926546 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFRToSourceR.path b/src/main/deploy/pathplanner/paths/ReefFRToSourceR.path deleted file mode 100644 index 1e6e819..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFRToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.3735000000000004, - "y": 2.9817499999999995 - }, - "prevControl": null, - "nextControl": { - "x": 2.52525, - "y": 3.332749999999999 - }, - "isLocked": false, - "linkedName": "ReefFR" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 0.9457499999999999, - "y": 1.938499999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 59.036243467926546 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToSource.path b/src/main/deploy/pathplanner/paths/ReefToSource.path deleted file mode 100644 index 0a33f0f..0000000 --- a/src/main/deploy/pathplanner/paths/ReefToSource.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.728176229508197, - "y": 5.18780737704918 - }, - "prevControl": null, - "nextControl": { - "x": 3.4356762295081973, - "y": 6.41630737704918 - }, - "isLocked": false, - "linkedName": "ex2" - }, - { - "anchor": { - "x": 1.64775, - "y": 7.0865 - }, - "prevControl": { - "x": 2.40825, - "y": 6.8135 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -53.13010235415593 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 120.96375653207352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/Second Test.path similarity index 65% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/Second Test.path index 117bf66..d824dde 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6802254098360656, - "y": 5.343647540983607 + "x": 5.992617033407298, + "y": 1.9549432938856013 }, "prevControl": null, "nextControl": { - "x": 4.680225409836065, - "y": 5.343647540983607 + "x": 4.595626463880671, + "y": 1.7228558000493093 }, "isLocked": false, - "linkedName": "ep1" + "linkedName": "Nuts" }, { "anchor": { - "x": 1.8580942622950818, - "y": 6.95 + "x": 1.4531419501972382, + "y": 0.8238423785749501 }, "prevControl": { - "x": 3.0688524590163935, - "y": 7.129815573770491 + "x": 2.946701260478304, + "y": 1.447724821252465 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -52.125016348901745 + "rotation": 143.60051234648324 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 118.44292862436343 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefAL.path b/src/main/deploy/pathplanner/paths/SourceBToReefAL.path deleted file mode 100644 index 396fba0..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefAL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.4722500000000003, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": { - "x": 1.9835000000000003, - "y": 3.6447499999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefAR.path b/src/main/deploy/pathplanner/paths/SourceBToReefAR.path deleted file mode 100644 index afa41c8..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefAR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.2853469493415943, - "y": 6.363039664386715 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": { - "x": 2.8409604971839766, - "y": 4.5566264678922686 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefBL.path b/src/main/deploy/pathplanner/paths/SourceBToReefBL.path deleted file mode 100644 index 3ee988c..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefBL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 1.9967451896648978, - "y": 6.562537101862314 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.50025, - "y": 5.0584999999999996 - }, - "prevControl": { - "x": 3.1039096610105723, - "y": 5.437154570087065 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefBL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -58.49573328079569 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefBR.path b/src/main/deploy/pathplanner/paths/SourceBToReefBR.path deleted file mode 100644 index 6c87932..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefBR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 1.818380083850317, - "y": 6.640740137529289 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.8600409836065577, - "y": 5.475512295081967 - }, - "prevControl": { - "x": 2.0097226066154796, - "y": 5.915347603812001 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefBR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 122.85572195043298 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefCL.path b/src/main/deploy/pathplanner/paths/SourceBToReefCL.path deleted file mode 100644 index 6c17455..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefCL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.4722500000000003, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 4.9822500000000005, - "y": 5.41925 - }, - "prevControl": { - "x": 3.9822500000000005, - "y": 5.41925 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 57.680383491819796 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefCR.path b/src/main/deploy/pathplanner/paths/SourceBToReefCR.path deleted file mode 100644 index 0d1c9df..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefCR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.4722500000000003, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 5.46975, - "y": 4.922 - }, - "prevControl": { - "x": 4.795081967213115, - "y": 5.727254098360657 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 58.172553423326704 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefFL.path b/src/main/deploy/pathplanner/paths/SourceBToReefFL.path deleted file mode 100644 index c5ef2f5..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefFL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.47225, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": { - "x": 2.92925, - "y": 2.6014999999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefFL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefAL.path b/src/main/deploy/pathplanner/paths/SourceRToReefAL.path deleted file mode 100644 index 6d56988..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefAL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": { - "x": 1.9835000000000003, - "y": 3.6447499999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefAR.path b/src/main/deploy/pathplanner/paths/SourceRToReefAR.path deleted file mode 100644 index 1a9f0d9..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefAR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": { - "x": 2.0029999999999997, - "y": 4.366249999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefEL.path b/src/main/deploy/pathplanner/paths/SourceRToReefEL.path deleted file mode 100644 index 8c75b7d..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefEL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2090000000000005, - "y": 1.1389999999999998 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 4.92375, - "y": 2.61125 - }, - "prevControl": { - "x": 3.92375, - "y": 2.61125 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefEL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -60.52411099675415 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefER.path b/src/main/deploy/pathplanner/paths/SourceRToReefER.path deleted file mode 100644 index 78d9d43..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefER.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2096508560167436, - "y": 1.7839970668582361 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": { - "x": 4.70942146615192, - "y": 2.6005634886412072 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefER" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefFL.path b/src/main/deploy/pathplanner/paths/SourceRToReefFL.path deleted file mode 100644 index 7a8f0ba..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefFL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": { - "x": 2.92925, - "y": 2.6014999999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefFL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefFR.path b/src/main/deploy/pathplanner/paths/SourceRToReefFR.path deleted file mode 100644 index 1fc95d0..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefFR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 3.3735000000000004, - "y": 2.9817499999999995 - }, - "prevControl": { - "x": 2.3735000000000004, - "y": 2.9817499999999995 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefFR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 59.036243467926546 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToReef.path b/src/main/deploy/pathplanner/paths/SourceToReef.path deleted file mode 100644 index 3153cf6..0000000 --- a/src/main/deploy/pathplanner/paths/SourceToReef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.7861680327868852, - "y": 7.03391393442623 - }, - "prevControl": null, - "nextControl": { - "x": 2.786168032786885, - "y": 7.03391393442623 - }, - "isLocked": false, - "linkedName": "ex1" - }, - { - "anchor": { - "x": 3.728176229508197, - "y": 5.18780737704918 - }, - "prevControl": { - "x": 2.728176229508197, - "y": 5.18780737704918 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ex2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.96375653207352 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -51.48307369289725 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToReef2.path b/src/main/deploy/pathplanner/paths/SourceToReef2.path deleted file mode 100644 index b27fab6..0000000 --- a/src/main/deploy/pathplanner/paths/SourceToReef2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.7861680327868852, - "y": 7.03391393442623 - }, - "prevControl": null, - "nextControl": { - "x": 2.3136270491803277, - "y": 7.669262295081967 - }, - "isLocked": false, - "linkedName": "ex1" - }, - { - "anchor": { - "x": 3.728176229508197, - "y": 5.18780737704918 - }, - "prevControl": { - "x": 3.467163934426229, - "y": 5.784315573770492 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ex2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.96375653207352 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -51.48307369289725 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backtoSource.path b/src/main/deploy/pathplanner/paths/backtoSource.path deleted file mode 100644 index 9dac851..0000000 --- a/src/main/deploy/pathplanner/paths/backtoSource.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.03483606557377, - "y": 5.40358606557377 - }, - "prevControl": null, - "nextControl": { - "x": 6.03483606557377, - "y": 5.40358606557377 - }, - "isLocked": false, - "linkedName": "ep2" - }, - { - "anchor": { - "x": 2.0139344262295085, - "y": 6.973975409836065 - }, - "prevControl": { - "x": 1.0139344262295085, - "y": 6.973975409836065 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -37.87498365109819 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 61.38954033403478 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 344420b..3d81038 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,36 +1,32 @@ { - "robotWidth": 1.0, - "robotLength": 1.0, + "robotWidth": 0.762, + "robotLength": 0.762, "holonomicMode": true, - "pathFolders": [ - "ComponentPaths" - ], + "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 2.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 110.0, + "robotMass": 115.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.044, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", - "driveCurrentLimit": 40.0, + "driveCurrentLimit": 120.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.254, + "flModuleY": 0.254, + "frModuleX": 0.254, + "frModuleY": -0.254, + "blModuleX": -0.254, + "blModuleY": 0.254, + "brModuleX": -0.254, + "brModuleY": -0.254, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [ - "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.3,\"y\":0.0},\"size\":{\"width\":0.4,\"length\":0.05},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" - ] + "robotFeatures": [] } \ No newline at end of file diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 6e41767..93a9ab4 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -15,93 +15,95 @@ * Constants for robot mechanisms unrelated to the drive train */ public class BotConstants { - // The CAN Bus attached directly to the RoboRIO, used for devices controlling - // non-drivetrain mechanisms - public static CANBus rioBus; - // Used for drivetrain devices (motors, encoders, IMU, etc.). Supports CAN FD. - public static CANBus canivoreBus; + // The CAN Bus attached directly to the RoboRIO, used for devices controlling + // non-drivetrain mechanisms + public static CANBus rioBus; + // Used for drivetrain devices (motors, encoders, IMU, etc.). Supports CAN FD. + public static CANBus canivoreBus; + static { + rioBus = new CANBus("rio"); + canivoreBus = new CANBus("cv"); + } + + public static class Elevator { + public static int masterID = 11; + public static int slaveId = 10; + public static TalonFXConfiguration cfg; static { - rioBus = new CANBus("rio"); - canivoreBus = new CANBus("cv"); + cfg = new TalonFXConfiguration(); + cfg.Slot0.kP = 6.; + cfg.Slot0.kG = 0.05; + // cfg.Slot0.kS = 0.02; + cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; + cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; + cfg.MotionMagic.MotionMagicAcceleration = 120.; + cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; + cfg.CurrentLimits.StatorCurrentLimit = 120.; + cfg.CurrentLimits.StatorCurrentLimitEnable = true; + } - public static class Elevator { - public static int masterID = 11; - public static int slaveId = 10; - public static TalonFXConfiguration cfg; - static { - cfg = new TalonFXConfiguration(); - cfg.Slot0.kP = 6.; - cfg.Slot0.kG = 0.05; - //cfg.Slot0.kS = 0.02; - cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; - cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - - cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; - cfg.MotionMagic.MotionMagicAcceleration = 120.; - cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - - } + } + public static class Arm { + public static int masterID = 13; + public static TalonFXConfiguration cfg = new TalonFXConfiguration(); + static { + cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; + cfg.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + cfg.Slot0.kP = 8.; + cfg.CurrentLimits.StatorCurrentLimitEnable = true; + cfg.CurrentLimits.StatorCurrentLimit = 100.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 80.; + cfg.MotionMagic.MotionMagicAcceleration = 65.; + + cfg.Slot0.kG = 0; } + } - public static class Arm { - public static int masterID = 13; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; - cfg.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - cfg.Slot0.kP = 8.; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 100.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 80.; - cfg.MotionMagic.MotionMagicAcceleration = 65.; - - cfg.Slot0.kG = 0; - } - } + public static class Wrist { + public static int motorID = 14; + public static TalonFXConfiguration cfg = new TalonFXConfiguration(); + static { + cfg.Slot0.kP = 32.; - public static class Wrist { - public static int motorID = 14; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.Slot0.kP = 32.; + cfg.MotionMagic.MotionMagicAcceleration = 40.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; + cfg.CurrentLimits.StatorCurrentLimitEnable = false; + cfg.CurrentLimits.SupplyCurrentLimitEnable = false; + cfg.CurrentLimits.StatorCurrentLimit = 120.; + } - cfg.MotionMagic.MotionMagicAcceleration = 40.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; - cfg.CurrentLimits.StatorCurrentLimitEnable = false; - cfg.CurrentLimits.SupplyCurrentLimitEnable=false; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - } + } + public static class EndEffector { + public static int motorID = 15; + public static TalonFXConfiguration cfg = new TalonFXConfiguration(); + static { + cfg.CurrentLimits.StatorCurrentLimitEnable = true; + cfg.CurrentLimits.StatorCurrentLimit = 80.; } + } - public static class EndEffector { - public static int motorID = 15; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 80.; - } + public static class GroundCoral { + public static int angleId = 17; + public static int rollerId = 18; + public static TalonFXConfiguration angleCfg = new TalonFXConfiguration(); + public static TalonFXConfiguration rollerCfg = new TalonFXConfiguration(); + static { + angleCfg.Slot0.kP = 1.; + angleCfg.CurrentLimits.StatorCurrentLimit = 120; + angleCfg.CurrentLimits.StatorCurrentLimitEnable = true; + angleCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + angleCfg.MotionMagic.MotionMagicCruiseVelocity = 40; + angleCfg.MotionMagic.MotionMagicAcceleration = 40; + + rollerCfg.CurrentLimits.StatorCurrentLimit = 120; + rollerCfg.CurrentLimits.StatorCurrentLimitEnable = true; } - public static class GroundCoral{ - public static int angleId=17; - public static int rollerId=18; - public static TalonFXConfiguration angleCfg=new TalonFXConfiguration(); - public static TalonFXConfiguration rollerCfg=new TalonFXConfiguration(); - static{ - angleCfg.Slot0.kP=1.; - angleCfg.CurrentLimits.StatorCurrentLimit=120; - angleCfg.CurrentLimits.StatorCurrentLimitEnable=true; - angleCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - rollerCfg.CurrentLimits.StatorCurrentLimit=120; - rollerCfg.CurrentLimits.StatorCurrentLimitEnable=true; - } - - } + } } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index eff0323..7f5a47d 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 45; - public static final String GIT_SHA = "67310794249e354938811afe4d6092355f89763d"; - public static final String GIT_DATE = "2025-03-09 00:30:21 PST"; + public static final int GIT_REVISION = 46; + public static final String GIT_SHA = "afb86b5286e0cd32db74848fc49ba74e92b3990c"; + public static final String GIT_DATE = "2025-03-09 13:03:59 PDT"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-09 12:52:40 PDT"; - public static final long BUILD_UNIX_TIME = 1741549960294L; + public static final String BUILD_DATE = "2025-03-09 19:33:56 PDT"; + public static final long BUILD_UNIX_TIME = 1741574036237L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dea458a..8feb9e9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Swerve; @@ -32,6 +33,7 @@ import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; import frc.robot.subsystems.GroundCoral; +import frc.robot.subsystems.LED; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -77,21 +79,15 @@ public void putSwerveSysIDCalibrationButtons() { private final SendableChooser autoChooser; public Robot() { - SmartDashboard.putData(CommandScheduler.getInstance()); - SmartDashboard.putData(Elevator.get()); - SmartDashboard.putData(Arm.get()); - SmartDashboard.putData(EndEffector.get()); - SmartDashboard.putData(Wrist.get()); - SmartDashboard.putData(GroundCoral.get()); - - Swerve.get().registerTelemetry(m_SwerveTelemetry::telemeterize); - + Arm.get(); + Elevator.get(); + EndEffector.get(); + GroundCoral.get(); + LED.get(); + Swerve.get(); + Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); configureDriverStationBinds(); - - // Log Build information - logBuildInfo(); - Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.08), 2) @@ -102,19 +98,18 @@ public Robot() { Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.08), 2) * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); - SmartDashboard.putData("Face Hex", Swerve.get().applyRequest( + HumanControls.DriverPanel.OtherButton.whileTrue(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) .withRotationalRate( - (Swerve.get().getPose().getRotation().minus(Swerve.get().FaceHexFace())).getDegrees() / -120 + (Swerve.get().getPose().getRotation().minus(Swerve.get().goalRotation.get())).getDegrees() / -300 * MaxRotationalRate))); - SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); - - NamedCommands.registerCommand("hello", new InstantCommand( - () -> SmartDashboard.putBoolean("auto", true))); - + NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); + NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -150,7 +145,7 @@ private void configureDriverStationBinds() { Elevator.get().getCurrentCommand().cancel(); GroundCoral.get().getCurrentCommand().cancel(); - Elevator.get().setDefaultCommand(Elevator.get().positionCommand(35)); + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(20)); GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); EndEffector.get().idlePower = 0; })); @@ -188,8 +183,9 @@ private void configureDriverStationBinds() { new MoveSuperStructure(GameInfo.Net, 1.) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceNet, Swerve.get()::FaceSource))); - HumanControls.OperatorPanel2025.SouceCoral.whileTrue(new MoveSuperStructure( - GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.5, false, 0.2)); + HumanControls.OperatorPanel2025.SouceCoral.whileTrue((new MoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.6, false, 0.1)) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceSource, Swerve.get()::FaceHexFace))); HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6)); // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new @@ -201,10 +197,12 @@ public void robotPeriodic() { long start = RobotController.getTime(); CommandScheduler.getInstance().run(); long elapsed = RobotController.getTime() - start; - BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler Time in Microseconds, anything over - // 20,000 should + // BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler + // Time in Microseconds, anything over + // 20,000 should - BackupLogger.logSystemInformation(); + // BackupLogger.logSystemInformation(); + // } @Override diff --git a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java new file mode 100644 index 0000000..988ee2a --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java @@ -0,0 +1,81 @@ + +package frc.robot.commands; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.EndEffector; +import frc.robot.util.GameInfo.CoralScoringPosition; + +public class AutoMoveSuperStructure extends Command { + + CoralScoringPosition position; + + double runRollers = 0; + + double postRollerState = 0; + + double readyToShoot; + + boolean waitForStall = false; + + public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, + double postRollerState, boolean waitForStall) { + this.position = position; + + this.runRollers = runRollers; + + this.postRollerState = postRollerState; + + this.waitForStall = waitForStall; + + addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); + } + + public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, double postRollerState) { + this(position, runRollers, postRollerState, false); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + readyToShoot = 999999999; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Arm.get().goToPosition(position); + Wrist.get().goToPosition(position); + Elevator.get().goToPosition(position); + if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 + && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 + && Math.abs(Elevator.get().getPosition() - position.height) < .5) { + EndEffector.get().run(runRollers); + if (readyToShoot > 99999) { + readyToShoot = Timer.getFPGATimestamp(); + } + } else { + EndEffector.get().run(EndEffector.get().idlePower); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Arm.get().stop(); + Wrist.get().stop(); + Elevator.get().stop(); + EndEffector.get().idlePower = postRollerState; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (Timer.getFPGATimestamp() - readyToShoot > 1) + && (!waitForStall || EndEffector.get().isStalling()); + } +} diff --git a/src/main/java/frc/robot/commands/MoveSuperStructure.java b/src/main/java/frc/robot/commands/MoveSuperStructure.java index 7aedf62..f5dc50e 100644 --- a/src/main/java/frc/robot/commands/MoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/MoveSuperStructure.java @@ -23,7 +23,8 @@ public class MoveSuperStructure extends Command { boolean requirePress = true; - public MoveSuperStructure(CoralScoringPosition position, double runRollers, boolean requirePress, double postRollerState) { + public MoveSuperStructure(CoralScoringPosition position, double runRollers, boolean requirePress, + double postRollerState) { this.position = position; this.runRollers = runRollers; @@ -35,8 +36,6 @@ public MoveSuperStructure(CoralScoringPosition position, double runRollers, bool addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); } - - public MoveSuperStructure(CoralScoringPosition position, double runRollers) { this(position, runRollers, true, 0.); } @@ -54,9 +53,12 @@ public void execute() { Wrist.get().goToPosition(position); Elevator.get().goToPosition(position); - if ((!requirePress || HumanControls.OperatorPanel2025.releaseCoral.getAsBoolean()) && Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 && Math.abs(Elevator.get().getPosition() - position.height) < .5 ) + if ((!requirePress || HumanControls.OperatorPanel2025.releaseCoral.getAsBoolean()) + && Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 + && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 + && Math.abs(Elevator.get().getPosition() - position.height) < .5) EndEffector.get().run(runRollers); - else + else EndEffector.get().run(EndEffector.get().idlePower); } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 2e212df..03c97d4 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -25,7 +25,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; import frc.robot.util.GameInfo; -import frc.robot.util.TriggerNTDouble; import frc.team696.lib.Logging.BackupLogger; public class Arm extends SubsystemBase { @@ -59,11 +58,7 @@ private Arm() { zeroArm(); // this.setDefaultCommand(Position(()->0)); - new TriggerNTDouble("testing/armAngle", ntpos, (ev) -> ntpos = ev); - SmartDashboard.putData("ArmPlus", Spin(0.1)); - SmartDashboard.putData("ArmMinus", Spin(-0.1)); - SmartDashboard.putData("Zero Arm", this.runOnce(() -> zeroArm()).ignoringDisable(true)); } public void stop() { @@ -112,10 +107,5 @@ public Command Spin(double speed) { @Override public void periodic() { - // This method will be called once per scheduler run - BackupLogger.addToQueue("Arm/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); - BackupLogger.addToQueue("Arm/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); - BackupLogger.addToQueue("Arm/VoltageVolts", voltageSignal.refresh().getValue().in(Volts)); - BackupLogger.addToQueue("Arm/PositionRot", positionSignal.refresh().getValue().in(Rotations)); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index bfc58c3..767e8e3 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -67,8 +67,8 @@ public void DriveVoltage(Voltage v) { } public void goToPosition(double position) { - if (GroundCoral.get().getPosition() > .5) { - position = Math.max(position, 30); + if (GroundCoral.get().getPosition() > 2.) { + position = Math.max(position, 16); } master.get().setControl(positionReq.withPosition(position)); } @@ -118,12 +118,5 @@ public void resetPosition(double newPosition) { @Override public void periodic() { - BackupLogger.addToQueue("Elevator/MasterCurrent", master.getCurrent()); - BackupLogger.addToQueue("Elevator/SlaveCurrent", slave.getCurrent()); - BackupLogger.addToQueue("Elevator/MasterPosition", master.getPosition()); - BackupLogger.addToQueue("Elevator/SlavePosition", slave.getPosition()); - BackupLogger.addToQueue("Elevator/SlaveVelocity", slave.getVelocity()); - BackupLogger.addToQueue("Elevator/MasterVelocity", master.getVelocity()); - } } diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java index 052f5b3..0439294 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector.java @@ -80,12 +80,10 @@ public Command spin(double power) { return this.startEnd(() -> run(power), () -> motor.set(0)); } - public boolean isStalling(){ - - return motor.getStatorCurrent().getValue().in(Amps) >= 40 && velocitySignal.getValue().in(RotationsPerSecond) < 5; - } + public boolean isStalling() { - + return motor.getStatorCurrent().getValue().in(Amps) >= 75 && velocitySignal.getValue().in(RotationsPerSecond) < 5; + } public Command spin(DoubleSupplier power) { return this.runEnd(() -> run(power.getAsDouble()), () -> motor.set(0)); @@ -97,26 +95,22 @@ public Command spinVelocity(double velocity) { @Override public void periodic() { - - // if (isStalling() && BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity == 10. ) { - // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 6.; - // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 6.; - // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); - // // idlePower = -0.8; + + // if (isStalling() && + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity == 10. ) { + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 6.; + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 6.; + // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); + // // idlePower = -0.8; // } else if (!isStalling()) { - // if (BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity < 16.) { - // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 10.; - // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 16.; - // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); - // } - // //idlePower = 0.; + // if (BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity < 16.) { + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 10.; + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 16.; + // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); // } - - + // //idlePower = 0.; + // } + // This method will be called once per scheduler run - BackupLogger.addToQueue("EndEffector/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); - BackupLogger.addToQueue("EndEffector/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); - BackupLogger.addToQueue("EndEffector/VoltageVolts", voltageSignal.refresh().getValue().in(Volts)); - BackupLogger.addToQueue("EndEffector/PositionRot", positionSignal.refresh().getValue().in(Rotations)); } } diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index a2dd52d..532beb9 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -7,10 +7,9 @@ import static edu.wpi.first.units.Units.Amp; import static edu.wpi.first.units.Units.Rotation; -import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; @@ -30,7 +29,7 @@ public static synchronized final GroundCoral get() { TalonFX angleMotor = new TalonFX(BotConstants.GroundCoral.angleId, BotConstants.rioBus); TalonFX rollerMotor = new TalonFX(BotConstants.GroundCoral.rollerId, BotConstants.rioBus); - PositionDutyCycle positionRequest = new PositionDutyCycle(0); + MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0); private GroundCoral() { angleMotor.getConfigurator().apply(BotConstants.GroundCoral.angleCfg); @@ -51,21 +50,22 @@ public void zero() { resetPosition(0); } - public void stop(){ + public void stop() { angleMotor.stopMotor(); rollerMotor.stopMotor(); } - public boolean isStalling(){ - return rollerMotor.getStatorCurrent().getValue().in(Amp)>(BotConstants.GroundCoral.rollerCfg.CurrentLimits.StatorCurrentLimit-20); + public boolean isStalling() { + return rollerMotor.getStatorCurrent().getValue() + .in(Amp) > (BotConstants.GroundCoral.rollerCfg.CurrentLimits.StatorCurrentLimit - 20); } - public double getPosition(){ + public double getPosition() { return angleMotor.getPosition().getValue().in(Rotation); } public void position(double position) { - if (Elevator.get().getPosition() >= 30) { + if (Elevator.get().getPosition() >= 16) { angleMotor.setControl(positionRequest.withPosition(position)); } else { stop(); @@ -84,30 +84,31 @@ public Command Intake() { public Command Stowed() { return this.runEnd( - ()->{position(0); rollerMotor.stopMotor();}, - this::stop - ); + () -> { + position(0); + rollerMotor.stopMotor(); + }, + this::stop); } public Command Ready() { return this.runEnd( - ()->{position(0.6 * 9.); rollerMotor.stopMotor();}, - this::stop - ); + () -> { + position(0.6 * 9.); + rollerMotor.set(0.3); + }, + this::stop); } public Command Spit() { - return this.runEnd(()-> { + return this.runEnd(() -> { position(0.6 * 9.); - if (getPosition() > 30) { - rollerMotor.set(-0.6); - } + rollerMotor.set(-0.6); }, this::stop); } @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("GroundCoral/Position", getPosition()); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index dbb8325..42a8cb5 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -255,7 +255,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { .withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage) .withSteerRequestType(SteerRequestType.Position); public static SwerveRequest.FieldCentric fcDriveReq = new SwerveRequest.FieldCentric() - .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) .withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage) .withSteerRequestType(SteerRequestType.Position); @@ -295,8 +295,11 @@ public void periodic() { }); } - CamA.addVisionEstimate(this::addVisionMeasurement); - CamB.addVisionEstimate(this::addVisionMeasurement); + CamA.addVisionEstimate(this::addVisionMeasurement, (Estimate) -> { + if (Estimate.distToTag > 4) + return false; + return true; + }); } @@ -363,8 +366,9 @@ public Rotation2d FaceHexFace() { GameInfo.blueReef.getY())); Rotation2d angleToReef = getPose().getTranslation().minus(reefPosition).getAngle(); - - Rotation2d hexAngleToReef = Rotation2d.fromDegrees(((int) ((angleToReef.getDegrees() + 30) / 60)) * 60.); + Rotation2d hexAngleToReef = Rotation2d + .fromDegrees( + ((int) ((angleToReef.getDegrees() + Math.signum(angleToReef.getDegrees()) * 30) / 60)) * 60. - 90.); return hexAngleToReef; } @@ -376,15 +380,15 @@ public Rotation2d FaceNet() { public Rotation2d FaceSource() { if (Util.getAlliance() == Alliance.Blue) { if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { - return Rotation2d.fromDegrees(-45); + return Rotation2d.fromDegrees(37); } else { - return Rotation2d.fromDegrees(45); + return Rotation2d.fromDegrees(127); } } else { if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { - return Rotation2d.fromDegrees(135); + return Rotation2d.fromDegrees(-37); } else { - return Rotation2d.fromDegrees(-135); + return Rotation2d.fromDegrees(-127); } } } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 047afdd..3ca62e4 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -53,7 +53,6 @@ private Wrist() { positionSignal = motor.getPosition(); voltageSignal = motor.getMotorVoltage(); currentSignal = motor.getStatorCurrent(); - SmartDashboard.putData("Zero Wrist", this.runOnce(() -> zero()).ignoringDisable(true)); } public void resetPosition(double newPosition) { @@ -69,11 +68,11 @@ public void stop() { } public double getPosition() { - return positionSignal.getValueAsDouble(); + return motor.getPosition().getValueAsDouble(); } - public Command Position(double position){ - return this.runEnd(()->goToPosition(position), ()->motor.set(0.0)); + public Command Position(double position) { + return this.runEnd(() -> goToPosition(position), () -> motor.set(0.0)); } public void goToPosition(double position) { @@ -81,19 +80,13 @@ public void goToPosition(double position) { } - public void goToPosition(CoralScoringPosition position){ + public void goToPosition(CoralScoringPosition position) { goToPosition(position.wristRot.in(Rotation)); } - @Override public void periodic() { - // This method will be called once per scheduler run - BackupLogger.addToQueue("Wrist/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); - BackupLogger.addToQueue("Wrist/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); - BackupLogger.addToQueue("Wrist/VoltageVolts", voltageSignal.refresh().getValue().in(Volts)); - BackupLogger.addToQueue("Wrist/PositionRot", positionSignal.refresh().getValue().in(Rotation)); } } diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index dec454b..bba1f95 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -19,134 +19,120 @@ * scoring position */ public class GameInfo { - public static class CoralScoringPosition { - /** - * Creates a new CoralScoringPosition - * - * @param height The height of the elevator (zero represents the elevator's - * lowest position), positive=up - * @param armRot The rotation of the arm - * @param wristRot The rotation of the wrist - */ - public CoralScoringPosition(double height, double armRot, double wristRot) { - this.height = height; - this.armRot = Rotation.of(armRot); - this.wristRot = Rotation.of(wristRot); - } - - public double height; - // Rotations of the Motor, Im too lazy to change off angle now - public Angle armRot; - public Angle wristRot; + public static class CoralScoringPosition { + /** + * Creates a new CoralScoringPosition + * + * @param height The height of the elevator (zero represents the elevator's + * lowest position), positive=up + * @param armRot The rotation of the arm + * @param wristRot The rotation of the wrist + */ + public CoralScoringPosition(double height, double armRot, double wristRot) { + this.height = height; + this.armRot = Rotation.of(armRot); + this.wristRot = Rotation.of(wristRot); } - public static CoralScoringPosition Net, ground, Processor, ClimbUp, ClimbDown, L2Algae, L3Algae; - - - /* Looking at the Index Dead On */ - public enum ReefSide { - Right, - Left - } - - /* Labels For Blue Side Of Field, Relative To Center Of Hex */ - public enum Index { - One, // -X - Two, // -X, +Y - Three, // +X, +Y - Four, // +X - Five, // +X, -Y - Six // -X, -Y - } - - public final static Distance fieldLengthMeters = Feet.of(57.53); - public final static Distance fieldWidthMeters = Feet.of(26.75); - - public static Translation2d mirrorTranslation(Translation2d starting) { - return new Translation2d(fieldLengthMeters.in(Meters) - starting.getY(), starting.getY()); - } - - public final static Translation2d blueReef = new Translation2d(4.5,4.); - - public final static Map> ScoringPosesBlue; - - public enum Position { - L1, - L2, - L3, - L4, - Intake - } - - public enum RobotSide { - Front, - Back - } - - public final static Map> RobotState; - - static { - ScoringPosesBlue = Map.of( - Index.One, Map.of( - ReefSide.Right, new Translation2d(4.046, 5.322), - ReefSide.Left, new Translation2d(3.539, 4.912) - ), - - Index.Two, Map.of( - ReefSide.Right, new Translation2d(3.188, 4.473), - ReefSide.Left, new Translation2d(3.179, 3.752) - ), - - Index.Three, Map.of( - ReefSide.Right, new Translation2d(3.481, 3.050), - ReefSide.Left, new Translation2d(4.056, 2.689) - ), - - Index.Four, Map.of( - ReefSide.Right, new Translation2d(4.836, 2.679), - ReefSide.Left, new Translation2d(5.450, 2.992) - ), - - Index.Five, Map.of( - ReefSide.Right, new Translation2d(5.821, 3.762), - ReefSide.Left, new Translation2d(5.850, 4.376) - ), - - Index.Six, Map.of( - ReefSide.Right, new Translation2d(5.489, 4.932), - ReefSide.Left, new Translation2d(4.924, 5.351) - ) - ); - - RobotState = Map.of( - Position.L1, Map.of( - RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1), - RobotSide.Back, new CoralScoringPosition(0, -1., -8.) - ), - Position.L2, Map.of( - RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56), - RobotSide.Back, new CoralScoringPosition(7., -1., -7.6) - ), - Position.L3, Map.of( - RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1), - RobotSide.Back, new CoralScoringPosition(25., -1., -7.9) - ), - Position.L4, Map.of( - RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6), - RobotSide.Back, new CoralScoringPosition(62, -1.1, -8.3) - ), - Position.Intake, Map.of( - RobotSide.Front, new CoralScoringPosition(6., 1., -0.9), - RobotSide.Back, new CoralScoringPosition(0, 0, 0) - ) - ); - L2Algae = new CoralScoringPosition (18., -3., 1.); - L3Algae = new CoralScoringPosition (44., -3., 1.); - Net = new CoralScoringPosition(67., 0., 5.8); - ClimbUp=new CoralScoringPosition(27, 0, 0); - ClimbDown=new CoralScoringPosition(2, 0, 0); - ground=new CoralScoringPosition(5., -6.3, -.5); - Processor = new CoralScoringPosition(0, -5., 1.); - } + public double height; + // Rotations of the Motor, Im too lazy to change off angle now + public Angle armRot; + public Angle wristRot; + } + + public static CoralScoringPosition Net, ground, Processor, ClimbUp, ClimbDown, L2Algae, L3Algae; + + /* Looking at the Index Dead On */ + public enum ReefSide { + Right, + Left + } + + /* Labels For Blue Side Of Field, Relative To Center Of Hex */ + public enum Index { + One, // -X + Two, // -X, +Y + Three, // +X, +Y + Four, // +X + Five, // +X, -Y + Six // -X, -Y + } + + public final static Distance fieldLengthMeters = Feet.of(57.53); + public final static Distance fieldWidthMeters = Feet.of(26.75); + + public static Translation2d mirrorTranslation(Translation2d starting) { + return new Translation2d(fieldLengthMeters.in(Meters) - starting.getY(), starting.getY()); + } + + public final static Translation2d blueReef = new Translation2d(4.5, 4.); + + public final static Map> ScoringPosesBlue; + + public enum Position { + L1, + L2, + L3, + L4, + Intake + } + + public enum RobotSide { + Front, + Back + } + + public final static Map> RobotState; + + static { + ScoringPosesBlue = Map.of( + Index.One, Map.of( + ReefSide.Right, new Translation2d(4.046, 5.322), + ReefSide.Left, new Translation2d(3.539, 4.912)), + + Index.Two, Map.of( + ReefSide.Right, new Translation2d(3.188, 4.473), + ReefSide.Left, new Translation2d(3.179, 3.752)), + + Index.Three, Map.of( + ReefSide.Right, new Translation2d(3.481, 3.050), + ReefSide.Left, new Translation2d(4.056, 2.689)), + + Index.Four, Map.of( + ReefSide.Right, new Translation2d(4.836, 2.679), + ReefSide.Left, new Translation2d(5.450, 2.992)), + + Index.Five, Map.of( + ReefSide.Right, new Translation2d(5.821, 3.762), + ReefSide.Left, new Translation2d(5.850, 4.376)), + + Index.Six, Map.of( + ReefSide.Right, new Translation2d(5.489, 4.932), + ReefSide.Left, new Translation2d(4.924, 5.351))); + + RobotState = Map.of( + Position.L1, Map.of( + RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1), + RobotSide.Back, new CoralScoringPosition(0, -1., -8.)), + Position.L2, Map.of( + RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56), + RobotSide.Back, new CoralScoringPosition(4., -1., -7.6)), + Position.L3, Map.of( + RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1), + RobotSide.Back, new CoralScoringPosition(25., -1., -7.9)), + Position.L4, Map.of( + RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6), + RobotSide.Back, new CoralScoringPosition(62, -1.1, -8.3)), + Position.Intake, Map.of( + RobotSide.Front, new CoralScoringPosition(6., 1., -0.9), + RobotSide.Back, new CoralScoringPosition(0, 0, 0))); + L2Algae = new CoralScoringPosition(18., -3., 1.); + L3Algae = new CoralScoringPosition(44., -3., 1.); + Net = new CoralScoringPosition(67., 0., 5.8); + ClimbUp = new CoralScoringPosition(27, 0, 0); + ClimbDown = new CoralScoringPosition(2, 0, 0); + ground = new CoralScoringPosition(5., -6.3, -.5); + Processor = new CoralScoringPosition(0, -5., 1.); + } } diff --git a/src/main/java/frc/robot/util/TriggerNTDouble.java b/src/main/java/frc/robot/util/TriggerNTDouble.java deleted file mode 100644 index 17ecf56..0000000 --- a/src/main/java/frc/robot/util/TriggerNTDouble.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.function.DoubleConsumer; - -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** Add your docs here. */ -public class TriggerNTDouble { - String name; - double val; - Trigger trigger; - DoubleEntry ntEntry; - DoubleConsumer update; - - private boolean hasChanged() { - double prev = val; - val = ntEntry.getAsDouble(); - return prev != val; - } - - public TriggerNTDouble(String name, double val, DoubleConsumer update) { - this.name = name; - this.val = val; - this.update = update; - this.ntEntry = NetworkTableInstance.getDefault().getDoubleTopic(name).getEntry(0); - this.ntEntry.set(val); - trigger = new Trigger(() -> hasChanged()); - trigger.onTrue(new InstantCommand(() -> update.accept(this.val)).ignoringDisable(true)); - } -} From 1c17e9a55140b5bdb9cbb79b123b3db03c0e153b Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Mon, 10 Mar 2025 13:28:01 -0700 Subject: [PATCH 11/37] Limelights working --- src/main/java/frc/robot/BotConstants.java | 4 +-- src/main/java/frc/robot/BuildConstants.java | 10 +++---- src/main/java/frc/robot/Robot.java | 29 ++++++++++++++----- src/main/java/frc/robot/subsystems/Arm.java | 5 ---- .../java/frc/robot/subsystems/Elevator.java | 1 - .../frc/robot/subsystems/EndEffector.java | 4 --- .../java/frc/robot/subsystems/Swerve.java | 8 ++++- src/main/java/frc/robot/subsystems/Wrist.java | 5 ---- 8 files changed, 35 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 93a9ab4..c620d21 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -68,10 +68,10 @@ public static class Wrist { public static TalonFXConfiguration cfg = new TalonFXConfiguration(); static { cfg.Slot0.kP = 32.; - + cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.MotionMagic.MotionMagicAcceleration = 40.; cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; - cfg.CurrentLimits.StatorCurrentLimitEnable = false; + cfg.CurrentLimits.StatorCurrentLimitEnable = true; cfg.CurrentLimits.SupplyCurrentLimitEnable = false; cfg.CurrentLimits.StatorCurrentLimit = 120.; } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7f5a47d..757fb1c 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 46; - public static final String GIT_SHA = "afb86b5286e0cd32db74848fc49ba74e92b3990c"; - public static final String GIT_DATE = "2025-03-09 13:03:59 PDT"; + public static final int GIT_REVISION = 47; + public static final String GIT_SHA = "0a75d019cbe3709a1cd4453e87938f2fd91dd752"; + public static final String GIT_DATE = "2025-03-09 23:41:29 PDT"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-09 19:33:56 PDT"; - public static final long BUILD_UNIX_TIME = 1741574036237L; + public static final String BUILD_DATE = "2025-03-10 13:14:13 PDT"; + public static final long BUILD_UNIX_TIME = 1741637653112L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8feb9e9..ae3c35c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,7 +11,10 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.path.ConstraintsZone; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; @@ -30,6 +33,7 @@ import frc.robot.subsystems.Wrist; import frc.robot.util.GameInfo; import frc.team696.lib.Logging.BackupLogger; +import frc.team696.lib.Swerve.SwerveConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; import frc.robot.subsystems.GroundCoral; @@ -37,10 +41,13 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxSpeed = SwerveConstants.THEORETICAL_MAX_SPEED.in(MetersPerSecond);// aTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxRotationalRate = RotationsPerSecond.of(10).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); + private ProfiledPIDController thetaController = new ProfiledPIDController(1 / 60, 0, 0, + new TrapezoidProfile.Constraints(60, 40)); + private void logBuildInfo() { BackupLogger.addToQueue("BuildConstants/ProjectName", BuildConstants.MAVEN_NAME); BackupLogger.addToQueue("BuildConstants/BuildDate", BuildConstants.BUILD_DATE); @@ -90,21 +97,26 @@ public Robot() { configureDriverStationBinds(); Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( - Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.08), 2) + Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.09), 2) * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) - .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.08), 2) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.07), 2) * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) .withRotationalRate( - Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.08), 2) + Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.09), 2) * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); HumanControls.DriverPanel.OtherButton.whileTrue(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( - applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) - .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) + Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) + .withRotationalRate( - (Swerve.get().getPose().getRotation().minus(Swerve.get().goalRotation.get())).getDegrees() / -300 - * MaxRotationalRate))); + (thetaController.calculate(Swerve.get().getPose().getRotation().getDegrees(), + Swerve.get().goalRotation.get().getDegrees())) + * MaxRotationalRate)) + .alongWith(new InstantCommand(() -> thetaController.reset(Swerve.get().getPose().getRotation().getDegrees())))); NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); @@ -201,6 +213,7 @@ public void robotPeriodic() { // Time in Microseconds, anything over // 20,000 should + m_SwerveTelemetry.telemeterize(Swerve.get().getState()); // BackupLogger.logSystemInformation(); // } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 03c97d4..21eb754 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -4,10 +4,7 @@ package frc.robot.subsystems; -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 static edu.wpi.first.units.Units.Volts; import java.util.function.DoubleSupplier; @@ -20,12 +17,10 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; import frc.robot.util.GameInfo; -import frc.team696.lib.Logging.BackupLogger; public class Arm extends SubsystemBase { private static Arm arm = null; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 767e8e3..0cbda20 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -19,7 +19,6 @@ import frc.robot.BotConstants; import frc.robot.util.GameInfo; import frc.team696.lib.HardwareDevices.TalonFactory; -import frc.team696.lib.Logging.BackupLogger; public class Elevator extends SubsystemBase { private static Elevator m_Elevator = null; diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java index 0439294..26b113b 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector.java @@ -5,10 +5,7 @@ package frc.robot.subsystems; 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 static edu.wpi.first.units.Units.Volts; - import java.util.function.DoubleSupplier; import com.ctre.phoenix6.StatusSignal; @@ -23,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; -import frc.team696.lib.Logging.BackupLogger; /** * This class represents the Coral scoring mechanism attached to the arm of the diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 42a8cb5..0c85f4a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -48,7 +49,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private static Swerve m_CommandSwerveDrivetrain = null; public LimeLightCam CamA = new LimeLightCam("limelight-right"); - public LimeLightCam CamB = new LimeLightCam("A"); + public LimeLightCam CamB = new LimeLightCam("limelight-left"); public Supplier goalRotation = () -> new Rotation2d(); @@ -300,6 +301,11 @@ public void periodic() { return false; return true; }); + CamB.addVisionEstimate(this::addVisionMeasurement, (Estimate) -> { + if (Estimate.distToTag > 4) + return false; + return true; + }); } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 3ca62e4..d133fad 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -4,10 +4,7 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -18,12 +15,10 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; import frc.robot.util.GameInfo.CoralScoringPosition; -import frc.team696.lib.Logging.BackupLogger; public class Wrist extends SubsystemBase { private static Wrist m_Wrist = null; From 2feb56cb18d3734f714f241a9e2efb015d40fcac Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Mon, 10 Mar 2025 16:35:38 -0700 Subject: [PATCH 12/37] Minor Position Adjustments --- src/main/java/frc/robot/BotConstants.java | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 8 +++++--- src/main/java/frc/robot/util/GameInfo.java | 6 +++--- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index c620d21..c6ffcc9 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -71,7 +71,7 @@ public static class Wrist { cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.MotionMagic.MotionMagicAcceleration = 40.; cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; + cfg.CurrentLimits.StatorCurrentLimitEnable = false; cfg.CurrentLimits.SupplyCurrentLimitEnable = false; cfg.CurrentLimits.StatorCurrentLimit = 120.; } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 757fb1c..244707f 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 47; - public static final String GIT_SHA = "0a75d019cbe3709a1cd4453e87938f2fd91dd752"; - public static final String GIT_DATE = "2025-03-09 23:41:29 PDT"; + public static final int GIT_REVISION = 48; + public static final String GIT_SHA = "1c17e9a55140b5bdb9cbb79b123b3db03c0e153b"; + public static final String GIT_DATE = "2025-03-10 13:28:01 PDT"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-10 13:14:13 PDT"; - public static final long BUILD_UNIX_TIME = 1741637653112L; + public static final String BUILD_DATE = "2025-03-10 14:26:09 PDT"; + public static final long BUILD_UNIX_TIME = 1741641969533L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ae3c35c..4babe20 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,8 +45,8 @@ public class Robot extends TimedRobot { private double MaxRotationalRate = RotationsPerSecond.of(10).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); - private ProfiledPIDController thetaController = new ProfiledPIDController(1 / 60, 0, 0, - new TrapezoidProfile.Constraints(60, 40)); + private ProfiledPIDController thetaController = new ProfiledPIDController(1. / 200., 0, 0., + new TrapezoidProfile.Constraints(360, 480)); private void logBuildInfo() { BackupLogger.addToQueue("BuildConstants/ProjectName", BuildConstants.MAVEN_NAME); @@ -116,7 +116,9 @@ public Robot() { (thetaController.calculate(Swerve.get().getPose().getRotation().getDegrees(), Swerve.get().goalRotation.get().getDegrees())) * MaxRotationalRate)) - .alongWith(new InstantCommand(() -> thetaController.reset(Swerve.get().getPose().getRotation().getDegrees())))); + .alongWith( + Commands.startEnd(() -> thetaController.reset(Swerve.get().getPose().getRotation().getDegrees()), () -> { + }))); NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index bba1f95..d41e9c2 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -116,19 +116,19 @@ RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1), RobotSide.Back, new CoralScoringPosition(0, -1., -8.)), Position.L2, Map.of( RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56), - RobotSide.Back, new CoralScoringPosition(4., -1., -7.6)), + RobotSide.Back, new CoralScoringPosition(4., -1., -8.1)), Position.L3, Map.of( RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1), RobotSide.Back, new CoralScoringPosition(25., -1., -7.9)), Position.L4, Map.of( RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6), - RobotSide.Back, new CoralScoringPosition(62, -1.1, -8.3)), + RobotSide.Back, new CoralScoringPosition(62, -1.1, -9.)), Position.Intake, Map.of( RobotSide.Front, new CoralScoringPosition(6., 1., -0.9), RobotSide.Back, new CoralScoringPosition(0, 0, 0))); L2Algae = new CoralScoringPosition(18., -3., 1.); L3Algae = new CoralScoringPosition(44., -3., 1.); - Net = new CoralScoringPosition(67., 0., 5.8); + Net = new CoralScoringPosition(67., 0., 6.5); ClimbUp = new CoralScoringPosition(27, 0, 0); ClimbDown = new CoralScoringPosition(2, 0, 0); ground = new CoralScoringPosition(5., -6.3, -.5); From bf6de5c769187e5350473b5057fdd550c51cf887 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Tue, 11 Mar 2025 22:31:41 -0700 Subject: [PATCH 13/37] Initial Fixes --- src/main/java/frc/robot/Robot.java | 9 +-- src/main/java/frc/robot/TODO.md | 11 +++ src/main/java/frc/robot/subsystems/Arm.java | 67 +++++++++---------- .../java/frc/robot/subsystems/Elevator.java | 3 +- .../frc/robot/subsystems/GroundCoral.java | 25 +++++-- 5 files changed, 70 insertions(+), 45 deletions(-) create mode 100644 src/main/java/frc/robot/TODO.md diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4babe20..dc3f7c1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -99,7 +99,7 @@ public Robot() { () -> Swerve.fcDriveReq.withVelocityX( Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.09), 2) * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) - .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.07), 2) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.09), 2) * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) .withRotationalRate( Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.09), 2) @@ -134,6 +134,7 @@ public Robot() { Wrist.get().setDefaultCommand(Wrist.get().Position(0)); Arm.get().setDefaultCommand(Arm.get().Position(() -> 0)); EndEffector.get().setDefaultCommand(EndEffector.get().spin(() -> EndEffector.get().idlePower)); + GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); } private void configureDriverStationBinds() { @@ -152,7 +153,7 @@ private void configureDriverStationBinds() { GroundCoral.get().getCurrentCommand().cancel(); Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); - })); + }).ignoringDisable(true)); HumanControls.OperatorPanel2025.unlabedSwitch.onFalse( new InstantCommand(() -> { @@ -162,7 +163,7 @@ private void configureDriverStationBinds() { Elevator.get().setDefaultCommand(Elevator.get().positionCommand(20)); GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); EndEffector.get().idlePower = 0; - })); + }).ignoringDisable(true)); HumanControls.OperatorPanel2025.GroundCoral.whileTrue( new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), @@ -171,7 +172,7 @@ private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.L1.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.4), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.3), HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); diff --git a/src/main/java/frc/robot/TODO.md b/src/main/java/frc/robot/TODO.md new file mode 100644 index 0000000..ecc702d --- /dev/null +++ b/src/main/java/frc/robot/TODO.md @@ -0,0 +1,11 @@ +- [x] Ground Intake Rollers Too fast intaking and way too fast spitting +- [x] Incorrect Position for ground intake when idling +- [x?] Elevator goes down when ready for intaking +- [] Middle Auto +- [] Left Auto +- [] Right Auto +- [???] Dead zones in auto align? +- [] Picking up algae it returns too fast both reef and ground - Rotate Intake To Scoring Position?? +- [todo there] Processor +- [x] shoots too hard for l1 + diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 21eb754..75c5bf3 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -8,15 +8,12 @@ import java.util.function.DoubleSupplier; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; @@ -33,27 +30,32 @@ public static synchronized Arm get() { } TalonFX master = new TalonFX(BotConstants.Arm.masterID, BotConstants.rioBus); - StatusSignal velocitySignal; - StatusSignal positionSignal; - StatusSignal voltageSignal; - StatusSignal currentSignal; MotionMagicVoltage positionRequest = new MotionMagicVoltage(0); VoltageOut voltageRequest = new VoltageOut(0); - double ntpos = 0; + boolean slowMode = false; + + ProfiledPIDController pidController = new ProfiledPIDController(8., 0, 0, new TrapezoidProfile.Constraints(85., 65.)); // Need + // to + // use + // this + // because + // CTR SHITTYTRONICS IS + // GATEKEEPING BASIC + // FUNCTIONALITY + // + ProfiledPIDController slowPidController = new ProfiledPIDController(8., 0, 0, + new TrapezoidProfile.Constraints(60., 35.)); /** Creates a new Arm. */ private Arm() { master.getConfigurator().apply(BotConstants.Arm.cfg); - velocitySignal = master.getVelocity(); - positionSignal = master.getPosition(); - voltageSignal = master.getMotorVoltage(); - currentSignal = master.getStatorCurrent(); zeroArm(); - // this.setDefaultCommand(Position(()->0)); + pidController.reset(0); + slowPidController.reset(0); } public void stop() { @@ -68,39 +70,36 @@ public void zeroArm() { resetArmPosition(0); } + public void goToPosition(double position) { + master.setControl(voltageRequest.withOutput(pidController.calculate(getPosition(), position))); + } + public void goToPosition(GameInfo.CoralScoringPosition position) { - master.setControl(positionRequest.withPosition(position.armRot.in(Rotations))); + goToPosition(position.armRot.in(Rotations)); + } + + public void goToPosition(DoubleSupplier position) { + goToPosition(position.getAsDouble()); } public Command Position(DoubleSupplier position) { - return this.runEnd(() -> master.setControl(positionRequest.withPosition(position.getAsDouble())), - () -> master.set(0)); + return this.runEnd(() -> goToPosition(position), + () -> stop()); } public Command Position(GameInfo.CoralScoringPosition position) { - return this.startEnd(() -> master.setControl(positionRequest.withPosition(position.armRot.in(Rotations))), - () -> master.set(0)); + return this.startEnd(() -> goToPosition(position), + () -> stop()); } public double getPosition() { return master.getPosition().getValueAsDouble(); } - public Command ArmWithNTPosition() { - return this.runEnd(() -> master.setControl(positionRequest.withPosition(ntpos)), () -> master.stopMotor()); - } - - /** - * Spins the arm at a certain fraction of the motors - * - * @param speed [-1, 1] - * @return the command that spins the arm - */ - public Command Spin(double speed) { - return this.runEnd(() -> master.setControl(voltageRequest.withOutput(speed * 12)), () -> master.set(0)); - } - @Override public void periodic() { + pidController.calculate(getPosition()); + slowPidController.calculate(getPosition()); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 0cbda20..f3319ca 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -66,7 +66,8 @@ public void DriveVoltage(Voltage v) { } public void goToPosition(double position) { - if (GroundCoral.get().getPosition() > 2.) { + if (GroundCoral.get().getPosition() > GroundCoral.Positions.Stowed.value + 2. + && GroundCoral.get().getPosition() < GroundCoral.Positions.Ready.value - 1) { position = Math.max(position, 16); } master.get().setControl(positionReq.withPosition(position)); diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index 532beb9..9ed26d5 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -20,6 +20,19 @@ public class GroundCoral extends SubsystemBase { private static GroundCoral m_GroundCoral = null; + public static enum Positions { + Stowed(0), + Ready(6.), + Spit(6.), + Intake(10.8); + + Positions(double value) { + this.value = value; + } + + double value = 0; + } + public static synchronized final GroundCoral get() { if (m_GroundCoral == null) { m_GroundCoral = new GroundCoral(); @@ -74,8 +87,8 @@ public void position(double position) { public Command Intake() { return this.startEnd(() -> { - position(1.2 * 9.); - rollerMotor.set(1.); + position(Positions.Intake.value); + rollerMotor.set(0.7); }, () -> { angleMotor.stopMotor(); rollerMotor.stopMotor(); @@ -85,7 +98,7 @@ public Command Intake() { public Command Stowed() { return this.runEnd( () -> { - position(0); + position(Positions.Stowed.value); rollerMotor.stopMotor(); }, this::stop); @@ -94,7 +107,7 @@ public Command Stowed() { public Command Ready() { return this.runEnd( () -> { - position(0.6 * 9.); + position(Positions.Ready.value); rollerMotor.set(0.3); }, this::stop); @@ -102,8 +115,8 @@ public Command Ready() { public Command Spit() { return this.runEnd(() -> { - position(0.6 * 9.); - rollerMotor.set(-0.6); + position(Positions.Spit.value); + rollerMotor.set(-0.4); }, this::stop); } From 42ea68710b5a15cbbeb9efa455f0e873f1b61257 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Wed, 12 Mar 2025 16:59:56 -0700 Subject: [PATCH 14/37] Minor changes --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/TODO.md | 2 +- src/main/java/frc/robot/subsystems/GroundCoral.java | 6 +++--- src/main/java/frc/robot/subsystems/Swerve.java | 8 +++++--- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dc3f7c1..4201e3b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,6 +86,7 @@ public void putSwerveSysIDCalibrationButtons() { private final SendableChooser autoChooser; public Robot() { + thetaController.enableContinuousInput(-180, 180); Arm.get(); Elevator.get(); EndEffector.get(); diff --git a/src/main/java/frc/robot/TODO.md b/src/main/java/frc/robot/TODO.md index ecc702d..f757eb3 100644 --- a/src/main/java/frc/robot/TODO.md +++ b/src/main/java/frc/robot/TODO.md @@ -5,7 +5,7 @@ - [] Left Auto - [] Right Auto - [???] Dead zones in auto align? -- [] Picking up algae it returns too fast both reef and ground - Rotate Intake To Scoring Position?? +- [foundation layed] Picking up algae it returns too fast both reef and ground - Rotate Intake To Scoring Position?? - [todo there] Processor - [x] shoots too hard for l1 diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index 9ed26d5..5846582 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -22,9 +22,9 @@ public class GroundCoral extends SubsystemBase { public static enum Positions { Stowed(0), - Ready(6.), - Spit(6.), - Intake(10.8); + Ready(7.), + Spit(7.), + Intake(14.); Positions(double value) { this.value = value; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0c85f4a..f5451e4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -15,6 +15,8 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -24,7 +26,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -374,8 +375,9 @@ public Rotation2d FaceHexFace() { Rotation2d angleToReef = getPose().getTranslation().minus(reefPosition).getAngle(); Rotation2d hexAngleToReef = Rotation2d .fromDegrees( - ((int) ((angleToReef.getDegrees() + Math.signum(angleToReef.getDegrees()) * 30) / 60)) * 60. - 90.); - + MathUtil.inputModulus( + ((int) ((angleToReef.getDegrees() + Math.signum(angleToReef.getDegrees()) * 30) / 60)) * 60. - 90., + -180., 180)); return hexAngleToReef; } From 642f55b83ab448bccee1edd7fe49bd8280edc380 Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Sat, 15 Mar 2025 22:01:27 -0700 Subject: [PATCH 15/37] LA Regional --- 1 | 88 +++ .../autos/{New Auto.auto => Middle.auto} | 14 +- .../deploy/pathplanner/autos/RightSide.auto | 73 +++ .../deploy/pathplanner/paths/Fifth Test.path | 54 ++ .../deploy/pathplanner/paths/First Test.path | 28 +- .../deploy/pathplanner/paths/Fourth Test.path | 54 ++ .../deploy/pathplanner/paths/MiddleOne.path | 54 ++ .../deploy/pathplanner/paths/Second Test.path | 30 +- .../deploy/pathplanner/paths/Third Test.path | 54 ++ src/main/deploy/pathplanner/settings.json | 14 +- src/main/java/frc/robot/BotConstants.java | 4 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 49 +- src/main/java/frc/robot/SwerveTelemetry.java | 181 +++--- src/main/java/frc/robot/TunerConstants.java | 514 +++++++++--------- .../commands/AutoMoveSuperStructure.java | 19 +- .../robot/commands/MoveSuperStructure.java | 21 +- src/main/java/frc/robot/subsystems/Arm.java | 11 +- .../java/frc/robot/subsystems/Elevator.java | 8 +- .../frc/robot/subsystems/GroundCoral.java | 27 +- .../java/frc/robot/subsystems/Swerve.java | 48 +- src/main/java/frc/robot/subsystems/Wrist.java | 19 +- src/main/java/frc/robot/util/GameInfo.java | 36 +- 23 files changed, 922 insertions(+), 488 deletions(-) create mode 100644 1 rename src/main/deploy/pathplanner/autos/{New Auto.auto => Middle.auto} (58%) create mode 100644 src/main/deploy/pathplanner/autos/RightSide.auto create mode 100644 src/main/deploy/pathplanner/paths/Fifth Test.path create mode 100644 src/main/deploy/pathplanner/paths/Fourth Test.path create mode 100644 src/main/deploy/pathplanner/paths/MiddleOne.path create mode 100644 src/main/deploy/pathplanner/paths/Third Test.path diff --git a/1 b/1 new file mode 100644 index 0000000..a11abbd --- /dev/null +++ b/1 @@ -0,0 +1,88 @@ + +package frc.robot.commands; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.EndEffector; +import frc.robot.util.GameInfo.CoralScoringPosition; + +public class AutoMoveSuperStructure extends Command { + + CoralScoringPosition position; + + double runRollers = 0; + + double postRollerState = 0; + + double readyToShoot; + + boolean waitForStall = false; + + public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, + double postRollerState, boolean waitForStall) { + this.position = position; + + this.runRollers = runRollers; + + this.postRollerState = postRollerState; + + this.waitForStall = waitForStall; + + addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); + } + + public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, double postRollerState) { + this(position, runRollers, postRollerState, false); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + readyToShoot = 999999999; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Wrist.get().goToPosition(position); + Elevator.get().goToPosition(position); + if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 + && Math.abs(Elevator.get().getPosition() - position.height) < .5) { + + Arm.get().goToPosition(position); + if (Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5) { + + EndEffector.get().run(runRollers); + if (readyToShoot > 99999) { + readyToShoot = Timer.getFPGATimestamp(); + } + } else { + EndEffector.get().run(EndEffector.get().idlePower); + } + } else { + EndEffector.get().run(EndEffector.get().idlePower); + + Arm.get().goToPosition(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Arm.get().stop(); + Wrist.get().stop(); + Elevator.get().stop(); + EndEffector.get().idlePower = postRollerState; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (Timer.getFPGATimestamp() - readyToShoot > 1) + && (!waitForStall || EndEffector.get().isStalling()); + } +} diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Middle.auto similarity index 58% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Middle.auto index 79184cd..8e4a60a 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/Middle.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "First Test" + "pathName": "MiddleOne" } }, { @@ -15,18 +15,6 @@ "data": { "name": "L4" } - }, - { - "type": "path", - "data": { - "pathName": "Second Test" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/RightSide.auto b/src/main/deploy/pathplanner/autos/RightSide.auto new file mode 100644 index 0000000..decbbb1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RightSide.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "First Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Second Test" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Third Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Fourth Test" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Fifth Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path new file mode 100644 index 0000000..3b16609 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1514654215976328, + "y": 0.8693047337278097 + }, + "prevControl": null, + "nextControl": { + "x": 1.9153322238658776, + "y": 1.5428670488165666 + }, + "isLocked": false, + "linkedName": "balls" + }, + { + "anchor": { + "x": 3.781769677638067, + "y": 3.0356200690335298 + }, + "prevControl": { + "x": 2.5871579142011836, + "y": 1.9796281743096635 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.85153165654307 + }, + "reversed": false, + "folder": "RightSide", + "idealStartingState": { + "velocity": 0, + "rotation": 146.1455719493309 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path index e625933..b98f9cd 100644 --- a/src/main/deploy/pathplanner/paths/First Test.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.576790942430968, - "y": 1.9549432938856013 + "x": 6.974554286858974, + "y": 1.814462755793885 }, "prevControl": null, "nextControl": { - "x": 7.042790942430968, - "y": 1.9549432938856013 + "x": 6.02238774346647, + "y": 2.2398737980769226 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.992617033407298, - "y": 1.9549432938856013 + "x": 4.91479328464004, + "y": 2.9057453464003937 }, "prevControl": { - "x": 6.593551220414204, - "y": 1.9294521233974358 + "x": 5.7920988966962526, + "y": 2.5072519877958577 }, "nextControl": null, "isLocked": false, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 4.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": -149.5631867906827 }, "reversed": false, - "folder": null, + "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": -144.38773779361557 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path new file mode 100644 index 0000000..2bf929d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.1040376140285995, + "y": 2.868407914201183 + }, + "prevControl": null, + "nextControl": { + "x": 3.115277906188363, + "y": 1.8700347479043384 + }, + "isLocked": false, + "linkedName": "third" + }, + { + "anchor": { + "x": 1.1514654215976328, + "y": 0.8693047337278097 + }, + "prevControl": { + "x": 2.232266241370808, + "y": 1.5271133814102564 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "balls" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 146.1455719493309 + }, + "reversed": false, + "folder": "RightSide", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleOne.path b/src/main/deploy/pathplanner/paths/MiddleOne.path new file mode 100644 index 0000000..e80e72a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.05, + "y": 3.875 + }, + "prevControl": null, + "nextControl": { + "x": 6.389948058621888, + "y": 3.875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.675, + "y": 3.875 + }, + "prevControl": { + "x": 6.010450938895257, + "y": 3.875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index d824dde..713341a 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 5.992617033407298, - "y": 1.9549432938856013 + "x": 4.91479328464004, + "y": 2.9057453464003937 }, "prevControl": null, "nextControl": { - "x": 4.595626463880671, - "y": 1.7228558000493093 + "x": 4.050264268984221, + "y": 1.7830794656065079 }, "isLocked": false, "linkedName": "Nuts" }, { "anchor": { - "x": 1.4531419501972382, - "y": 0.8238423785749501 + "x": 1.1514654215976328, + "y": 0.8693047337278097 }, "prevControl": { - "x": 2.946701260478304, - "y": 1.447724821252465 + "x": 2.645024731878695, + "y": 1.4931871764053244 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "balls" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 4.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 143.60051234648324 + "rotation": 146.1455719493309 }, "reversed": false, - "folder": null, + "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": -149.5631867906827 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path new file mode 100644 index 0000000..da6cda3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1514654215976328, + "y": 0.8693047337278097 + }, + "prevControl": null, + "nextControl": { + "x": 2.12943324704142, + "y": 1.3645529000246541 + }, + "isLocked": false, + "linkedName": "balls" + }, + { + "anchor": { + "x": 4.1040376140285995, + "y": 2.868407914201183 + }, + "prevControl": { + "x": 3.011638621794871, + "y": 2.025462663338264 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "third" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "RightSide", + "idealStartingState": { + "velocity": 0, + "rotation": 146.1455719493309 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 3d81038..3fea087 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,13 +1,15 @@ { - "robotWidth": 0.762, - "robotLength": 0.762, + "robotWidth": 0.736, + "robotLength": 0.736, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "RightSide" + ], "autoFolders": [], - "defaultMaxVel": 1.0, - "defaultMaxAccel": 1.0, + "defaultMaxVel": 4.5, + "defaultMaxAccel": 3.5, "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, "robotMass": 115.0, "robotMOI": 6.883, diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index c6ffcc9..2cd882d 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -97,8 +97,8 @@ public static class GroundCoral { angleCfg.CurrentLimits.StatorCurrentLimit = 120; angleCfg.CurrentLimits.StatorCurrentLimitEnable = true; angleCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - angleCfg.MotionMagic.MotionMagicCruiseVelocity = 40; - angleCfg.MotionMagic.MotionMagicAcceleration = 40; + angleCfg.MotionMagic.MotionMagicCruiseVelocity = 60; + angleCfg.MotionMagic.MotionMagicAcceleration = 30; rollerCfg.CurrentLimits.StatorCurrentLimit = 120; rollerCfg.CurrentLimits.StatorCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 244707f..8a0b0a3 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 48; - public static final String GIT_SHA = "1c17e9a55140b5bdb9cbb79b123b3db03c0e153b"; - public static final String GIT_DATE = "2025-03-10 13:28:01 PDT"; + public static final int GIT_REVISION = 51; + public static final String GIT_SHA = "42ea68710b5a15cbbeb9efa455f0e873f1b61257"; + public static final String GIT_DATE = "2025-03-12 16:59:56 PDT"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-10 14:26:09 PDT"; - public static final long BUILD_UNIX_TIME = 1741641969533L; + public static final String BUILD_DATE = "2025-03-15 16:16:44 PDT"; + public static final long BUILD_UNIX_TIME = 1742080604759L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4201e3b..7fb985c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,7 +11,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; -import com.pathplanner.lib.path.ConstraintsZone; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -122,7 +121,7 @@ public Robot() { }))); NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( - GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); + GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); @@ -131,9 +130,23 @@ public Robot() { // Warmup Commands for PathPlanner PathfindingCommand.warmupCommand().schedule(); - Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); - Wrist.get().setDefaultCommand(Wrist.get().Position(0)); - Arm.get().setDefaultCommand(Arm.get().Position(() -> 0)); + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(() -> { + if (!HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean()) { + if (GroundCoral.get().getPosition() > 3.) { + return 25; + } else { + return 0; + } + } else { + if (GroundCoral.get().getPosition() < 5.) { + return 25.; + } else { + return 0; + } + } + })); + Wrist.get().setDefaultCommand(Wrist.get().Position(-0.3)); + Arm.get().setDefaultCommand(Arm.get().Position(() -> .7)); EndEffector.get().setDefaultCommand(EndEffector.get().spin(() -> EndEffector.get().idlePower)); GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); } @@ -146,34 +159,31 @@ private void configureDriverStationBinds() { EndEffector.get().idlePower = -0.6; }), GroundCoral.get().Spit(), - HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean)); + () -> !HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean())); - HumanControls.OperatorPanel2025.unlabedSwitch.onTrue( + HumanControls.OperatorPanel2025.unlabedSwitch.onFalse( new InstantCommand(() -> { - Elevator.get().getCurrentCommand().cancel(); - GroundCoral.get().getCurrentCommand().cancel(); - Elevator.get().setDefaultCommand(Elevator.get().positionCommand(0)); + if (GroundCoral.get().getCurrentCommand() != null) + GroundCoral.get().getCurrentCommand().cancel(); GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); }).ignoringDisable(true)); - - HumanControls.OperatorPanel2025.unlabedSwitch.onFalse( + HumanControls.OperatorPanel2025.unlabedSwitch.onTrue( new InstantCommand(() -> { - Elevator.get().getCurrentCommand().cancel(); - GroundCoral.get().getCurrentCommand().cancel(); - - Elevator.get().setDefaultCommand(Elevator.get().positionCommand(20)); + if (GroundCoral.get().getCurrentCommand() != null) + GroundCoral.get().getCurrentCommand().cancel(); + Swerve.get().goalRotation = Swerve.get()::FaceHexFace; GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); EndEffector.get().idlePower = 0; }).ignoringDisable(true)); HumanControls.OperatorPanel2025.GroundCoral.whileTrue( new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), - HumanControls.OperatorPanel2025.unlabedSwitch::getAsBoolean)); + () -> !HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean())); HumanControls.OperatorPanel2025.L1.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.3), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.15), HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); @@ -203,7 +213,8 @@ private void configureDriverStationBinds() { GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.6, false, 0.1)) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceSource, Swerve.get()::FaceHexFace))); HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); - HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6)); + HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceProcessor, Swerve.get()::FaceSource))); // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new // MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); } diff --git a/src/main/java/frc/robot/SwerveTelemetry.java b/src/main/java/frc/robot/SwerveTelemetry.java index 6422fe1..257fff8 100644 --- a/src/main/java/frc/robot/SwerveTelemetry.java +++ b/src/main/java/frc/robot/SwerveTelemetry.java @@ -21,104 +21,111 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class SwerveTelemetry { - private final double MaxSpeed; + private final double MaxSpeed; - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public SwerveTelemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public SwerveTelemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = driveStateTable + .getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable + .getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable + .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable + .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + /** + * Accept the swerve drive state and telemeterize it to SmartDashboard and + * SignalLogger. + */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); - /* Telemeterize the module states to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + /* Telemeterize the module states to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } + } } diff --git a/src/main/java/frc/robot/TunerConstants.java b/src/main/java/frc/robot/TunerConstants.java index ae17c2f..df8872d 100644 --- a/src/main/java/frc/robot/TunerConstants.java +++ b/src/main/java/frc/robot/TunerConstants.java @@ -19,268 +19,270 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(1.79).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("cv", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.06); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; - - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 18.75; - private static final Distance kWheelRadius = Inches.of(3.36 / 2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 0; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 0; - private static final int kFrontLeftEncoderId = 0; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.229736328125); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(9); - private static final Distance kFrontLeftYPos = Inches.of(9); - - // Front Right - private static final int kFrontRightDriveMotorId = 3; - private static final int kFrontRightSteerMotorId = 2; - private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.415283203125); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(9); - private static final Distance kFrontRightYPos = Inches.of(-9); - - // Back Left - private static final int kBackLeftDriveMotorId = 5; - private static final int kBackLeftSteerMotorId = 4; - private static final int kBackLeftEncoderId = 2; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.448974609375); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-9); - private static final Distance kBackLeftYPos = Inches.of(9); - - // Back Right - private static final int kBackRightDriveMotorId = 7; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 3; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.455078125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-9); - private static final Distance kBackRightYPos = Inches.of(-9); - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.79).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("cv", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.06); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 0; + + private static final double kDriveGearRatio = 5.357142857142857; + private static final double kSteerGearRatio = 18.75; + private static final Distance kWheelRadius = Inches.of(3.2 / 2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 0; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 0; + private static final int kFrontLeftEncoderId = 0; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15771484375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(9); + private static final Distance kFrontLeftYPos = Inches.of(9); + + // Front Right + private static final int kFrontRightDriveMotorId = 3; + private static final int kFrontRightSteerMotorId = 2; + private static final int kFrontRightEncoderId = 1; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.415283203125); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(9); + private static final Distance kFrontRightYPos = Inches.of(-9); + + // Back Left + private static final int kBackLeftDriveMotorId = 5; + private static final int kBackLeftSteerMotorId = 4; + private static final int kBackLeftEncoderId = 2; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.448974609375); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-9); + private static final Distance kBackLeftYPos = Inches.of(9); + + // Back Right + private static final int kBackRightDriveMotorId = 7; + private static final int kBackRightSteerMotorId = 6; + private static final int kBackRightEncoderId = 3; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.455078125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-9); + private static final Distance kBackRightYPos = Inches.of(-9); + + public static final SwerveModuleConstants FrontLeft = ConstantCreator + .createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = ConstantCreator + .createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = ConstantCreator + .createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = ConstantCreator + .createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static Swerve createDrivetrain() { + return new Swerve( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + } + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected + * device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ - public static Swerve createDrivetrain() { - return new Swerve( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules); } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules); + } /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz + * on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation + * in the form [x, y, theta]įµ€, with units in + * meters + * and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation + * in the form [x, y, theta]įµ€, with units in + * meters + * and radians + * @param modules Constants for each specific module */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]įµ€, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]įµ€, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules); } + } } diff --git a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java index 988ee2a..33351ff 100644 --- a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java @@ -48,18 +48,25 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Arm.get().goToPosition(position); Wrist.get().goToPosition(position); Elevator.get().goToPosition(position); if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 - && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 && Math.abs(Elevator.get().getPosition() - position.height) < .5) { - EndEffector.get().run(runRollers); - if (readyToShoot > 99999) { - readyToShoot = Timer.getFPGATimestamp(); + + Arm.get().goToPosition(position); + if (Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5) { + + EndEffector.get().run(runRollers); + if (readyToShoot > 99999) { + readyToShoot = Timer.getFPGATimestamp(); + } + } else { + EndEffector.get().run(EndEffector.get().idlePower); } } else { EndEffector.get().run(EndEffector.get().idlePower); + + Arm.get().goToPosition(0); } } @@ -75,7 +82,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return (Timer.getFPGATimestamp() - readyToShoot > 1) + return (Timer.getFPGATimestamp() - readyToShoot > 0.6) && (!waitForStall || EndEffector.get().isStalling()); } } diff --git a/src/main/java/frc/robot/commands/MoveSuperStructure.java b/src/main/java/frc/robot/commands/MoveSuperStructure.java index f5dc50e..9e56a46 100644 --- a/src/main/java/frc/robot/commands/MoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/MoveSuperStructure.java @@ -49,17 +49,24 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Arm.get().goToPosition(position); Wrist.get().goToPosition(position); Elevator.get().goToPosition(position); - if ((!requirePress || HumanControls.OperatorPanel2025.releaseCoral.getAsBoolean()) - && Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 - && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 - && Math.abs(Elevator.get().getPosition() - position.height) < .5) - EndEffector.get().run(runRollers); - else + if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 + && Math.abs(Elevator.get().getPosition() - position.height) < .5) { + + Arm.get().goToPosition(position); + if (Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 + && (!requirePress || HumanControls.OperatorPanel2025.releaseCoral.getAsBoolean())) { + + EndEffector.get().run(runRollers); + } else { + EndEffector.get().run(EndEffector.get().idlePower); + } + } else { + Arm.get().goToPosition(0); EndEffector.get().run(EndEffector.get().idlePower); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 75c5bf3..9734223 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -36,7 +36,6 @@ public static synchronized Arm get() { boolean slowMode = false; - ProfiledPIDController pidController = new ProfiledPIDController(8., 0, 0, new TrapezoidProfile.Constraints(85., 65.)); // Need // to // use // this @@ -45,7 +44,7 @@ public static synchronized Arm get() { // GATEKEEPING BASIC // FUNCTIONALITY // - ProfiledPIDController slowPidController = new ProfiledPIDController(8., 0, 0, + ProfiledPIDController slowPidController = new ProfiledPIDController(1., 0, 0, new TrapezoidProfile.Constraints(60., 35.)); /** Creates a new Arm. */ @@ -54,7 +53,6 @@ private Arm() { zeroArm(); - pidController.reset(0); slowPidController.reset(0); } @@ -71,7 +69,11 @@ public void zeroArm() { } public void goToPosition(double position) { - master.setControl(voltageRequest.withOutput(pidController.calculate(getPosition(), position))); + if (!slowMode) { + master.setControl(positionRequest.withPosition(position)); + } else { + master.setControl(voltageRequest.withOutput(slowPidController.calculate(getPosition(), position))); + } } public void goToPosition(GameInfo.CoralScoringPosition position) { @@ -98,7 +100,6 @@ public double getPosition() { @Override public void periodic() { - pidController.calculate(getPosition()); slowPidController.calculate(getPosition()); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index f3319ca..1209252 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -10,6 +10,8 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.controls.MotionMagicVoltage; import edu.wpi.first.units.measure.Voltage; @@ -68,7 +70,7 @@ public void DriveVoltage(Voltage v) { public void goToPosition(double position) { if (GroundCoral.get().getPosition() > GroundCoral.Positions.Stowed.value + 2. && GroundCoral.get().getPosition() < GroundCoral.Positions.Ready.value - 1) { - position = Math.max(position, 16); + position = Math.max(position, 25); } master.get().setControl(positionReq.withPosition(position)); } @@ -97,6 +99,10 @@ public Command positionCommand(double position) { return this.runEnd(() -> goToPosition(position), () -> master.VoltageOut(Volts.of(0))); } + public Command positionCommand(DoubleSupplier position) { + return this.runEnd(() -> goToPosition(position.getAsDouble()), () -> stop()); + } + /** * Holds the current position * diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index 5846582..ef599e5 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; @@ -22,9 +23,9 @@ public class GroundCoral extends SubsystemBase { public static enum Positions { Stowed(0), - Ready(7.), - Spit(7.), - Intake(14.); + Ready(7.3), + Spit(7.3), + Intake(12.5); Positions(double value) { this.value = value; @@ -78,11 +79,7 @@ public double getPosition() { } public void position(double position) { - if (Elevator.get().getPosition() >= 16) { - angleMotor.setControl(positionRequest.withPosition(position)); - } else { - stop(); - } + angleMotor.setControl(positionRequest.withPosition(position)); } public Command Intake() { @@ -98,7 +95,12 @@ public Command Intake() { public Command Stowed() { return this.runEnd( () -> { - position(Positions.Stowed.value); + if (Elevator.get().getPosition() > 20 + || this.getPosition() < 4) { + position(Positions.Stowed.value); + } else { + position(Positions.Ready.value); + } rollerMotor.stopMotor(); }, this::stop); @@ -107,7 +109,11 @@ public Command Stowed() { public Command Ready() { return this.runEnd( () -> { - position(Positions.Ready.value); + if (Elevator.get().getPosition() > 20 || this.getPosition() > 6.) { + position(Positions.Ready.value); + } else { + position(Positions.Stowed.value); + } rollerMotor.set(0.3); }, this::stop); @@ -122,6 +128,7 @@ public Command Spit() { @Override public void periodic() { + SmartDashboard.putNumber("GroundCoral/Position", getPosition()); // This method will be called once per scheduler run } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f5451e4..b7c9eba 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -36,6 +37,7 @@ import frc.robot.TunerConstants.TunerSwerveDrivetrain; import frc.robot.util.GameInfo; import frc.team696.lib.Util; +import frc.team696.lib.Camera.BaseCam; import frc.team696.lib.Camera.LimeLightCam; /** @@ -52,6 +54,8 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { public LimeLightCam CamA = new LimeLightCam("limelight-right"); public LimeLightCam CamB = new LimeLightCam("limelight-left"); + Rotation2d yawOffset = new Rotation2d(0); + public Supplier goalRotation = () -> new Rotation2d(); public Command setGoalRotation(Supplier during, Supplier after) { @@ -67,6 +71,10 @@ public Command setGoalRotation(Supplier during, Supplier } + public void updateYawOffset() { + yawOffset = getPose().getRotation().minus(getPigeon2().getRotation2d()); + } + public static synchronized Swerve get() { if (m_CommandSwerveDrivetrain == null) { m_CommandSwerveDrivetrain = TunerConstants.createDrivetrain(); @@ -276,6 +284,8 @@ public void Drive(ChassisSpeeds c, boolean fieldRelative) { @Override public void periodic() { + if (DriverStation.isDisabled()) + updateYawOffset(); /* * Periodically try to apply the operator perspective. * If we haven't applied the operator perspective before, then we should apply @@ -297,16 +307,30 @@ public void periodic() { }); } - CamA.addVisionEstimate(this::addVisionMeasurement, (Estimate) -> { - if (Estimate.distToTag > 4) - return false; - return true; - }); - CamB.addVisionEstimate(this::addVisionMeasurement, (Estimate) -> { - if (Estimate.distToTag > 4) + BaseCam.acceptEstimate estimate = (Estimate) -> { + if (Estimate.distToTag > 3.5) return false; + + if (Estimate.ambiguity > 0.4) + return false; // Too Ambiguous, Ignore + if (getState().Speeds.omegaRadiansPerSecond > 2.5) + return false; // Rotating too fast, ignore + + if (Estimate.distToTag < 1) { + setVisionMeasurementStdDevs(VecBuilder.fill(0.001, 0.001, 0.001)); + } else { + setVisionMeasurementStdDevs( + VecBuilder.fill(Estimate.ambiguity * Math.pow(Estimate.distToTag, 2), + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2), + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2))); + } return true; - }); + }; + CamA.SetRobotOrientation(yawOffset); + CamB.SetRobotOrientation(yawOffset); + + CamA.addVisionEstimate(this::addVisionMeasurement, estimate); + CamB.addVisionEstimate(this::addVisionMeasurement, estimate); } @@ -400,4 +424,12 @@ public Rotation2d FaceSource() { } } } + + public Rotation2d FaceProcessor() { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(180); + } else { + return Rotation2d.fromDegrees(0); + } + } } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index d133fad..3d72929 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -6,15 +6,11 @@ import static edu.wpi.first.units.Units.Rotation; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; @@ -30,12 +26,7 @@ public static final synchronized Wrist get() { return m_Wrist; } - public TalonFX motor = new TalonFX(BotConstants.Wrist.motorID, BotConstants.rioBus); - - StatusSignal velocitySignal; - StatusSignal positionSignal; - StatusSignal voltageSignal; - StatusSignal currentSignal; + private TalonFX motor = new TalonFX(BotConstants.Wrist.motorID, BotConstants.rioBus); MotionMagicVoltage WristPoistionRequest = new MotionMagicVoltage(0); VoltageOut WristVoltageRequest = new VoltageOut(0); @@ -44,10 +35,6 @@ public static final synchronized Wrist get() { private Wrist() { motor.getConfigurator().apply(BotConstants.Wrist.cfg); zero(); - velocitySignal = motor.getVelocity(); - positionSignal = motor.getPosition(); - voltageSignal = motor.getMotorVoltage(); - currentSignal = motor.getStatorCurrent(); } public void resetPosition(double newPosition) { @@ -81,7 +68,7 @@ public void goToPosition(CoralScoringPosition position) { @Override public void periodic() { - + SmartDashboard.putNumber("Wrist/Position", getPosition()); // This method will be called once per scheduler run } } diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index d41e9c2..c9eb7bd 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -83,7 +83,7 @@ public enum RobotSide { } public final static Map> RobotState; - + public final static double wristOffset = 1.15; static { ScoringPosesBlue = Map.of( Index.One, Map.of( @@ -112,27 +112,27 @@ ReefSide.Right, new Translation2d(5.489, 4.932), RobotState = Map.of( Position.L1, Map.of( - RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1), - RobotSide.Back, new CoralScoringPosition(0, -1., -8.)), + RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1 - wristOffset), + RobotSide.Back, new CoralScoringPosition(0, -1., -8. - wristOffset)), Position.L2, Map.of( - RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56), - RobotSide.Back, new CoralScoringPosition(4., -1., -8.1)), + RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56 - wristOffset), + RobotSide.Back, new CoralScoringPosition(4., -1., -8.1 - wristOffset)), Position.L3, Map.of( - RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1), - RobotSide.Back, new CoralScoringPosition(25., -1., -7.9)), + RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1 - wristOffset), + RobotSide.Back, new CoralScoringPosition(25., -1., -8.1 - wristOffset)), Position.L4, Map.of( - RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6), - RobotSide.Back, new CoralScoringPosition(62, -1.1, -9.)), + RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6 - wristOffset), + RobotSide.Back, new CoralScoringPosition(64, -1.1, -9. - wristOffset)), Position.Intake, Map.of( - RobotSide.Front, new CoralScoringPosition(6., 1., -0.9), - RobotSide.Back, new CoralScoringPosition(0, 0, 0))); - L2Algae = new CoralScoringPosition(18., -3., 1.); - L3Algae = new CoralScoringPosition(44., -3., 1.); - Net = new CoralScoringPosition(67., 0., 6.5); - ClimbUp = new CoralScoringPosition(27, 0, 0); - ClimbDown = new CoralScoringPosition(2, 0, 0); - ground = new CoralScoringPosition(5., -6.3, -.5); - Processor = new CoralScoringPosition(0, -5., 1.); + RobotSide.Front, new CoralScoringPosition(6., 1., -0.9 - wristOffset), + RobotSide.Back, new CoralScoringPosition(0, 0, 0 - wristOffset))); + L2Algae = new CoralScoringPosition(17., -3., 1. - wristOffset); + L3Algae = new CoralScoringPosition(38., -3., 1. - wristOffset); + Net = new CoralScoringPosition(67., 0., 6.5 - wristOffset); + ClimbUp = new CoralScoringPosition(27, 0, 0 - wristOffset); + ClimbDown = new CoralScoringPosition(2, 0, 0 - wristOffset); + ground = new CoralScoringPosition(5., -6.3, -.5 - wristOffset); + Processor = new CoralScoringPosition(0, -5., 1. - wristOffset); } } From 241e930a9f2ff98af09404eee993b4351002961a Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Sat, 15 Mar 2025 22:01:40 -0700 Subject: [PATCH 16/37] deleted extra file --- 1 | 88 --------------------------------------------------------------- 1 file changed, 88 deletions(-) delete mode 100644 1 diff --git a/1 b/1 deleted file mode 100644 index a11abbd..0000000 --- a/1 +++ /dev/null @@ -1,88 +0,0 @@ - -package frc.robot.commands; - -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Wrist; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.EndEffector; -import frc.robot.util.GameInfo.CoralScoringPosition; - -public class AutoMoveSuperStructure extends Command { - - CoralScoringPosition position; - - double runRollers = 0; - - double postRollerState = 0; - - double readyToShoot; - - boolean waitForStall = false; - - public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, - double postRollerState, boolean waitForStall) { - this.position = position; - - this.runRollers = runRollers; - - this.postRollerState = postRollerState; - - this.waitForStall = waitForStall; - - addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); - } - - public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, double postRollerState) { - this(position, runRollers, postRollerState, false); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - readyToShoot = 999999999; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - Wrist.get().goToPosition(position); - Elevator.get().goToPosition(position); - if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 - && Math.abs(Elevator.get().getPosition() - position.height) < .5) { - - Arm.get().goToPosition(position); - if (Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5) { - - EndEffector.get().run(runRollers); - if (readyToShoot > 99999) { - readyToShoot = Timer.getFPGATimestamp(); - } - } else { - EndEffector.get().run(EndEffector.get().idlePower); - } - } else { - EndEffector.get().run(EndEffector.get().idlePower); - - Arm.get().goToPosition(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Arm.get().stop(); - Wrist.get().stop(); - Elevator.get().stop(); - EndEffector.get().idlePower = postRollerState; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (Timer.getFPGATimestamp() - readyToShoot > 1) - && (!waitForStall || EndEffector.get().isStalling()); - } -} From 12dead605185ae200ea91b50f339b731a035771c Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Mon, 17 Mar 2025 09:31:13 -0700 Subject: [PATCH 17/37] Post LA Regional --- src/main/deploy/pathplanner/paths/Fifth Test.path | 4 ++-- src/main/deploy/pathplanner/paths/First Test.path | 10 +++++----- src/main/deploy/pathplanner/paths/Fourth Test.path | 8 ++++---- src/main/deploy/pathplanner/paths/MiddleOne.path | 2 +- src/main/deploy/pathplanner/paths/Second Test.path | 2 +- src/main/deploy/pathplanner/paths/Third Test.path | 2 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../frc/robot/commands/AutoMoveSuperStructure.java | 2 +- 9 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path index 3b16609..6b8aaa6 100644 --- a/src/main/deploy/pathplanner/paths/Fifth Test.path +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 2.5871579142011836, - "y": 1.9796281743096635 + "y": 1.9796281743096638 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path index b98f9cd..4f8c661 100644 --- a/src/main/deploy/pathplanner/paths/First Test.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.974554286858974, - "y": 1.814462755793885 + "x": 6.896282128328403, + "y": 1.8521102995562126 }, "prevControl": null, "nextControl": { - "x": 6.02238774346647, - "y": 2.2398737980769226 + "x": 5.944115584935899, + "y": 2.27752134183925 }, "isLocked": false, "linkedName": null @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path index 2bf929d..4d608c3 100644 --- a/src/main/deploy/pathplanner/paths/Fourth Test.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.115277906188363, - "y": 1.8700347479043384 + "x": 3.1152779061883633, + "y": 1.8700347479043382 }, "isLocked": false, "linkedName": "third" @@ -21,7 +21,7 @@ }, "prevControl": { "x": 2.232266241370808, - "y": 1.5271133814102564 + "y": 1.5271133814102562 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleOne.path b/src/main/deploy/pathplanner/paths/MiddleOne.path index e80e72a..9d98c10 100644 --- a/src/main/deploy/pathplanner/paths/MiddleOne.path +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index 713341a..d20ffc8 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path index da6cda3..938bd35 100644 --- a/src/main/deploy/pathplanner/paths/Third Test.path +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 3fea087..f3ab840 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -7,7 +7,7 @@ ], "autoFolders": [], "defaultMaxVel": 4.5, - "defaultMaxAccel": 3.5, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8a0b0a3..3e8147a 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 51; - public static final String GIT_SHA = "42ea68710b5a15cbbeb9efa455f0e873f1b61257"; - public static final String GIT_DATE = "2025-03-12 16:59:56 PDT"; + public static final int GIT_REVISION = 53; + public static final String GIT_SHA = "241e930a9f2ff98af09404eee993b4351002961a"; + public static final String GIT_DATE = "2025-03-15 22:01:40 PDT"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-15 16:16:44 PDT"; - public static final long BUILD_UNIX_TIME = 1742080604759L; + public static final String BUILD_DATE = "2025-03-16 09:35:24 PDT"; + public static final long BUILD_UNIX_TIME = 1742142924311L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java index 33351ff..ca92f6f 100644 --- a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java @@ -82,7 +82,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return (Timer.getFPGATimestamp() - readyToShoot > 0.6) + return (Timer.getFPGATimestamp() - readyToShoot > 0.4) && (!waitForStall || EndEffector.get().isStalling()); } } From c6dabfed26c00a5524f50eebce2067592d306665 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Tue, 18 Mar 2025 16:46:24 -0700 Subject: [PATCH 18/37] monday --- simgui-ds.json | 5 + src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 9 +- .../java/frc/robot/commands/PIDtoNearest.java | 152 ++++++++++++++++++ .../frc/robot/commands/PIDtoPosition.java | 13 +- .../java/frc/robot/subsystems/Swerve.java | 5 +- src/main/java/frc/robot/util/GameInfo.java | 43 +++-- ....2.4.json => PathplannerLib-2025.2.6.json} | 8 +- 8 files changed, 215 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/commands/PIDtoNearest.java rename vendordeps/{PathplannerLib-2025.2.4.json => PathplannerLib-2025.2.6.json} (87%) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 3e8147a..3c03024 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 53; - public static final String GIT_SHA = "241e930a9f2ff98af09404eee993b4351002961a"; - public static final String GIT_DATE = "2025-03-15 22:01:40 PDT"; + public static final int GIT_REVISION = 54; + public static final String GIT_SHA = "12dead605185ae200ea91b50f339b731a035771c"; + public static final String GIT_DATE = "2025-03-17 09:31:13 PDT"; public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-16 09:35:24 PDT"; - public static final long BUILD_UNIX_TIME = 1742142924311L; + public static final String BUILD_DATE = "2025-03-18 15:22:12 PDT"; + public static final long BUILD_UNIX_TIME = 1742336532651L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7fb985c..5bb7744 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,8 @@ import com.pathplanner.lib.commands.PathfindingCommand; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; @@ -27,6 +29,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; +import frc.robot.commands.PIDtoNearest; +import frc.robot.commands.PIDtoPosition; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; @@ -126,6 +130,7 @@ public Robot() { GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); + SmartDashboard.putData("posey posey", new PIDtoNearest(false)); // Warmup Commands for PathPlanner PathfindingCommand.warmupCommand().schedule(); @@ -212,7 +217,9 @@ private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.SouceCoral.whileTrue((new MoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.6, false, 0.1)) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceSource, Swerve.get()::FaceHexFace))); - HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); + HumanControls.OperatorPanel2025.SouceCoral.onFalse(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1)); + //HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); + HumanControls.OperatorPanel2025.Climb1.whileTrue(new PIDtoNearest(false)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceProcessor, Swerve.get()::FaceSource))); // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new diff --git a/src/main/java/frc/robot/commands/PIDtoNearest.java b/src/main/java/frc/robot/commands/PIDtoNearest.java new file mode 100644 index 0000000..f53c2b5 --- /dev/null +++ b/src/main/java/frc/robot/commands/PIDtoNearest.java @@ -0,0 +1,152 @@ + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.HumanControls; +import frc.robot.HumanControls.OperatorPanel2025; +import frc.robot.subsystems.Swerve; +import frc.robot.util.GameInfo; +//import frc.robot.util.PoseUtil; +import frc.robot.util.GameInfo.ReefSide; +import frc.team696.lib.Util; +import frc.team696.lib.Logging.BackupLogger; + +/** + * PIDToPosition but it takes goes to the nearest scoring pose defined in + * GameInfo. + * + * @see PIDToPosition + * @see GameInfo + */ +public class PIDtoNearest extends Command { + private ProfiledPIDController xController, yController, omegaController; + private Pose2d goalPose; + private boolean ignoreLR; + + public static Pose2d findClosestPose(Pose2d referencePose) { + if (referencePose == null) { + throw new IllegalArgumentException("referencePose cannot be null"); + } + + Pose2d closestPose = null; + double minDistance = Double.MAX_VALUE; // Initialize with a very large value + + for (var entry : GameInfo.getScoringPoses().entrySet()) { + // Calculate the squared Euclidean distance between the translations + for (var poseEntry : entry.getValue().entrySet()) { + Pose2d pose = poseEntry.getValue(); + Translation2d referenceTranslation = referencePose.getTranslation(); + double distance = referenceTranslation.getDistance(pose.getTranslation()); + + if (distance < minDistance) { + minDistance = distance; + closestPose = pose; + } + } + } + + return closestPose; + } + + public static Pose2d findClosestPose(Pose2d referencePose, ReefSide side) { + if (referencePose == null) { + throw new IllegalArgumentException("referencePose cannot be null"); + } + + Pose2d closestPose = null; + double minDistance = Double.MAX_VALUE; // Initialize with a very large value + + for (var entry : GameInfo.getScoringPoses().entrySet()) { + // Calculate the squared Euclidean distance between the translations + Pose2d pose = entry.getValue().get(side); + Translation2d referenceTranslation = referencePose.getTranslation(); + double distance = referenceTranslation.getDistance(pose.getTranslation()); + + if (distance < minDistance) { + minDistance = distance; + closestPose = pose; + } + + } + + return closestPose; + } + + public PIDtoNearest() { + this(false); + } + + /** + * Creates a new PIDToNearest command + * + * @param ignoreLR If enabled, the robot will go to the nearest scoring pose + * regardless if the operator panel selected left or right. + */ + public PIDtoNearest(boolean ignoreLR) { + addRequirements(Swerve.get()); + xController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(1.3, 1.4)); + yController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(1.3, 1.4)); + xController.setTolerance(0.01); + yController.setTolerance(0.01); + + omegaController = new ProfiledPIDController(10, 0, 0, new TrapezoidProfile.Constraints(5, 2)); + omegaController.enableContinuousInput(-Math.PI, Math.PI); + omegaController.setTolerance(0.08); + this.ignoreLR = ignoreLR; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + Pose2d currPose = Swerve.get().getState().Pose; + goalPose = ignoreLR?findClosestPose(currPose):findClosestPose(currPose, HumanControls.OperatorPanel2025.leftOrRight.getAsBoolean()?ReefSide.Right:ReefSide.Left); + System.out.println("voy a ir a " + goalPose.getX() + " y " + goalPose.getY()); + xController.reset(currPose.getX()); + yController.reset(currPose.getY()); + omegaController.reset(currPose.getRotation().getRadians()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + BackupLogger.addToQueue("wantogo", goalPose); + + Pose2d currPose = Swerve.get().getState().Pose; + Swerve.get().Drive(new ChassisSpeeds( + xController.calculate(currPose.getX(), goalPose.getX()), + yController.calculate(currPose.getY(), goalPose.getY()), + omegaController.calculate(currPose.getRotation().getRadians(), goalPose.getRotation().getRadians())), true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + // System.out.println("There!"); + Swerve.get().Drive(new ChassisSpeeds(0, 0, 0)); + } + + public boolean atGoalPose(Pose2d goal, Pose2d curr) { + return (Math.abs(goal.getX() - curr.getX()) <= 0.02) && + (Math.abs(goal.getY() - curr.getY()) <= 0.02) && + (Math.abs(goal.getRotation().minus(curr.getRotation()).getDegrees())) < 2; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return atGoalPose(goalPose, Swerve.get().getState().Pose); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/PIDtoPosition.java b/src/main/java/frc/robot/commands/PIDtoPosition.java index aca382b..60a4ede 100644 --- a/src/main/java/frc/robot/commands/PIDtoPosition.java +++ b/src/main/java/frc/robot/commands/PIDtoPosition.java @@ -23,17 +23,22 @@ public class PIDtoPosition extends Command { private ProfiledPIDController xController, yController, omegaController; private Pose2d goalPose; + private double calculateWithTolerance(ProfiledPIDController controller, double measurement, double goal){ + double tolerance=controller.getPositionTolerance(); + double error=goal-measurement; + return Math.abs(error)> getScoringPoses(){ + return (Util.getAlliance()==Alliance.Red)?ScoringPosesRed:ScoringPosesBlue; } public final static Translation2d blueReef = new Translation2d(4.5, 4.); - public final static Map> ScoringPosesBlue; + public final static Map> ScoringPosesBlue, ScoringPosesRed; public enum Position { L1, @@ -87,29 +98,33 @@ public enum RobotSide { static { ScoringPosesBlue = Map.of( Index.One, Map.of( - ReefSide.Right, new Translation2d(4.046, 5.322), - ReefSide.Left, new Translation2d(3.539, 4.912)), + ReefSide.Right, new Pose2d(3.29, 3.76, Rotation2d.fromDegrees(90)), + ReefSide.Left, new Pose2d(3.3, 4.10, Rotation2d.fromDegrees(90))), Index.Two, Map.of( - ReefSide.Right, new Translation2d(3.188, 4.473), - ReefSide.Left, new Translation2d(3.179, 3.752)), + ReefSide.Right, new Pose2d(3.66, 4.96, Rotation2d.fromDegrees(30)), + ReefSide.Left, new Pose2d(4.02, 5.1, Rotation2d.fromDegrees(30))), Index.Three, Map.of( - ReefSide.Right, new Translation2d(3.481, 3.050), - ReefSide.Left, new Translation2d(4.056, 2.689)), + ReefSide.Right, new Pose2d(4.87, 5.21, Rotation2d.fromDegrees(-30)), + ReefSide.Left, new Pose2d(5.16, 5.02, Rotation2d.fromDegrees(-30))), Index.Four, Map.of( - ReefSide.Right, new Translation2d(4.836, 2.679), - ReefSide.Left, new Translation2d(5.450, 2.992)), + ReefSide.Right, new Pose2d(5.66, 4.28, Rotation2d.fromDegrees(-90)), + ReefSide.Left, new Pose2d(5.68, 3.91, Rotation2d.fromDegrees(-90))), Index.Five, Map.of( - ReefSide.Right, new Translation2d(5.821, 3.762), - ReefSide.Left, new Translation2d(5.850, 4.376)), + ReefSide.Right, new Pose2d(5.30, 3.10, Rotation2d.fromDegrees(-150)), + ReefSide.Left, new Pose2d(5, 2.94, Rotation2d.fromDegrees(-150))), Index.Six, Map.of( - ReefSide.Right, new Translation2d(5.489, 4.932), - ReefSide.Left, new Translation2d(4.924, 5.351))); + ReefSide.Right, new Pose2d(4.09, 2.87, Rotation2d.fromDegrees(150)), + ReefSide.Left, new Pose2d(3.78, 3.05, Rotation2d.fromDegrees(150)))); + ScoringPosesRed = Util.transformNestedMap(ScoringPosesBlue, (p2d) -> { + return new Pose2d(mirrorTranslationXY(p2d.getTranslation()), + p2d.getRotation().rotateBy(Rotation2d.fromDegrees(180))); + }); RobotState = Map.of( Position.L1, Map.of( RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1 - wristOffset), diff --git a/vendordeps/PathplannerLib-2025.2.4.json b/vendordeps/PathplannerLib-2025.2.6.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.4.json rename to vendordeps/PathplannerLib-2025.2.6.json index 24add57..95ba203 100644 --- a/vendordeps/PathplannerLib-2025.2.4.json +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.4.json", + "fileName": "PathplannerLib-2025.2.6.json", "name": "PathplannerLib", - "version": "2025.2.4", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.4" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.4", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 47650a0c4f0865317e72e3720194156da9ff54f5 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Wed, 19 Mar 2025 17:02:23 -0700 Subject: [PATCH 19/37] left side auto working --- .../pathplanner/autos/Copy of RightSide.auto | 139 ++++++++++++++++++ .../deploy/pathplanner/autos/LeftSide.auto | 127 ++++++++++++++++ .../deploy/pathplanner/paths/Fifth Test.path | 32 ++-- .../deploy/pathplanner/paths/First Test.path | 20 ++- .../deploy/pathplanner/paths/Fourth Test.path | 18 +-- .../deploy/pathplanner/paths/Second Test.path | 16 +- .../deploy/pathplanner/paths/Third Test.path | 32 ++-- .../deploy/pathplanner/paths/backtoreef.path | 54 +++++++ src/main/deploy/pathplanner/paths/end.path | 54 +++++++ .../deploy/pathplanner/paths/gotoreef.path | 54 +++++++ .../pathplanner/paths/thentosource.path | 54 +++++++ .../deploy/pathplanner/paths/tosource2.path | 54 +++++++ src/main/deploy/pathplanner/settings.json | 2 + src/main/java/frc/robot/BuildConstants.java | 12 +- src/main/java/frc/robot/Robot.java | 5 +- .../commands/AutoMoveSuperStructure.java | 2 +- .../java/frc/robot/commands/PIDtoNearest.java | 2 +- 17 files changed, 624 insertions(+), 53 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Copy of RightSide.auto create mode 100644 src/main/deploy/pathplanner/autos/LeftSide.auto create mode 100644 src/main/deploy/pathplanner/paths/backtoreef.path create mode 100644 src/main/deploy/pathplanner/paths/end.path create mode 100644 src/main/deploy/pathplanner/paths/gotoreef.path create mode 100644 src/main/deploy/pathplanner/paths/thentosource.path create mode 100644 src/main/deploy/pathplanner/paths/tosource2.path diff --git a/src/main/deploy/pathplanner/autos/Copy of RightSide.auto b/src/main/deploy/pathplanner/autos/Copy of RightSide.auto new file mode 100644 index 0000000..c29613b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of RightSide.auto @@ -0,0 +1,139 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "First Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Second Test" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Third Test" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fourth Test" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fifth Test" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftSide.auto b/src/main/deploy/pathplanner/autos/LeftSide.auto new file mode 100644 index 0000000..3ff0f9a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftSide.auto @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "gotoreef" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "thentosource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "backtoreef" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "tosource2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "end" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path index 6b8aaa6..4caf860 100644 --- a/src/main/deploy/pathplanner/paths/Fifth Test.path +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.1514654215976328, - "y": 0.8693047337278097 + "x": 1.2123355263157893, + "y": 0.9075657894736844 }, "prevControl": null, "nextControl": { - "x": 1.9153322238658776, - "y": 1.5428670488165666 + "x": 1.9762023285840344, + "y": 1.5811281045624415 }, "isLocked": false, "linkedName": "balls" }, { "anchor": { - "x": 3.781769677638067, - "y": 3.0356200690335298 + "x": 3.7139802631578944, + "y": 3.0435855263157894 }, "prevControl": { - "x": 2.5871579142011836, - "y": 1.9796281743096638 + "x": 2.519368499721011, + "y": 1.9875936315919234 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.6, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], "globalConstraints": { "maxVelocity": 4.5, "maxAcceleration": 3.0, @@ -48,7 +60,7 @@ "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": 146.1455719493309 + "rotation": 145.0079798014414 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path index 4f8c661..2806cd5 100644 --- a/src/main/deploy/pathplanner/paths/First Test.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 4.91479328464004, + "x": 4.91, "y": 2.9057453464003937 }, "prevControl": { - "x": 5.7920988966962526, + "x": 5.787305612056213, "y": 2.5072519877958577 }, "nextControl": null, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.26190476190476186, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], "globalConstraints": { "maxVelocity": 4.5, "maxAcceleration": 3.0, @@ -42,7 +54,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -149.5631867906827 + "rotation": -150.0 }, "reversed": false, "folder": "RightSide", diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path index 4d608c3..a32206e 100644 --- a/src/main/deploy/pathplanner/paths/Fourth Test.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.1040376140285995, - "y": 2.868407914201183 + "x": 4.127713815789473, + "y": 2.92 }, "prevControl": null, "nextControl": { - "x": 3.1152779061883633, - "y": 1.8700347479043382 + "x": 3.138954107949237, + "y": 1.921626833703155 }, "isLocked": false, "linkedName": "third" }, { "anchor": { - "x": 1.1514654215976328, - "y": 0.8693047337278097 + "x": 1.2123355263157893, + "y": 0.9075657894736844 }, "prevControl": { - "x": 2.232266241370808, - "y": 1.5271133814102562 + "x": 2.3080690576732694, + "y": 1.5401878387630243 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 146.1455719493309 + "rotation": 145.0079798014414 }, "reversed": false, "folder": "RightSide", diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index d20ffc8..55f84df 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.91479328464004, + "x": 4.91, "y": 2.9057453464003937 }, "prevControl": null, "nextControl": { - "x": 4.050264268984221, + "x": 4.045470984344181, "y": 1.7830794656065079 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.1514654215976328, - "y": 0.8693047337278097 + "x": 1.2123355263157893, + "y": 0.9075657894736844 }, "prevControl": { - "x": 2.645024731878695, - "y": 1.4931871764053244 + "x": 2.705894836596852, + "y": 1.5314482321511993 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 146.1455719493309 + "rotation": 145.0079798014414 }, "reversed": false, "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": -149.5631867906827 + "rotation": -150.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path index 938bd35..75799e3 100644 --- a/src/main/deploy/pathplanner/paths/Third Test.path +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.1514654215976328, - "y": 0.8693047337278097 + "x": 1.2123355263157893, + "y": 0.908 }, "prevControl": null, "nextControl": { - "x": 2.12943324704142, - "y": 1.3645529000246541 + "x": 2.190303351759577, + "y": 1.4032481662968443 }, "isLocked": false, "linkedName": "balls" }, { "anchor": { - "x": 4.1040376140285995, - "y": 2.868407914201183 + "x": 4.128, + "y": 2.92 }, "prevControl": { - "x": 3.011638621794871, - "y": 2.025462663338264 + "x": 3.0709996039264857, + "y": 2.033071357500476 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.6452380952380952, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], "globalConstraints": { "maxVelocity": 4.5, "maxAcceleration": 3.0, @@ -48,7 +60,7 @@ "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": 146.1455719493309 + "rotation": 145.0079798014414 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path new file mode 100644 index 0000000..300a479 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2621575342465754, + "y": 7.345676369863014 + }, + "prevControl": null, + "nextControl": { + "x": 2.178724315068493, + "y": 6.98505993150685 + }, + "isLocked": false, + "linkedName": "leftsource" + }, + { + "anchor": { + "x": 4.02, + "y": 5.1 + }, + "prevControl": { + "x": 3.080265410958904, + "y": 5.362285958904109 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TwoLeft" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": 36.86989764584405 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path new file mode 100644 index 0000000..fb6f2ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/end.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2621575342465754, + "y": 7.345676369863014 + }, + "prevControl": null, + "nextControl": { + "x": 2.5693921232876713, + "y": 5.843107876712329 + }, + "isLocked": false, + "linkedName": "leftsource" + }, + { + "anchor": { + "x": 3.66, + "y": 4.96 + }, + "prevControl": { + "x": 2.4191352739726026, + "y": 5.767979452054794 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": 36.86989764584405 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path new file mode 100644 index 0000000..021ca94 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.897, + "y": 5.873159246575342 + }, + "prevControl": null, + "nextControl": { + "x": 5.6671875, + "y": 5.737664473684211 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.92, + "y": 5.18 + }, + "prevControl": { + "x": 5.522861842105263, + "y": 5.689555921052631 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ThreeRight" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.13119678106381 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": -45.000000000000135 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path new file mode 100644 index 0000000..2d68286 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.92, + "y": 5.18 + }, + "prevControl": null, + "nextControl": { + "x": 4.24216609589041, + "y": 6.023467465753424 + }, + "isLocked": false, + "linkedName": "ThreeRight" + }, + { + "anchor": { + "x": 1.2621575342465754, + "y": 7.345676369863014 + }, + "prevControl": { + "x": 2.76472602739726, + "y": 6.203724315068493 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "leftsource" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.86989764584405 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": -30.13119678106381 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/tosource2.path b/src/main/deploy/pathplanner/paths/tosource2.path new file mode 100644 index 0000000..c42c31b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.02, + "y": 5.1 + }, + "prevControl": null, + "nextControl": { + "x": 3.1403681506849317, + "y": 6.128595890410959 + }, + "isLocked": false, + "linkedName": "TwoLeft" + }, + { + "anchor": { + "x": 1.2621575342465754, + "y": 7.345676369863014 + }, + "prevControl": { + "x": 2.4792380136986303, + "y": 6.008390410958905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "leftsource" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.86989764584405 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f3ab840..1888b38 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,6 +3,8 @@ "robotLength": 0.736, "holonomicMode": true, "pathFolders": [ + "MiddleAndNet", + "LeftSide", "RightSide" ], "autoFolders": [], diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 3c03024..a90f59d 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 54; - public static final String GIT_SHA = "12dead605185ae200ea91b50f339b731a035771c"; - public static final String GIT_DATE = "2025-03-17 09:31:13 PDT"; - public static final String GIT_BRANCH = "Oscar"; - public static final String BUILD_DATE = "2025-03-18 15:22:12 PDT"; - public static final long BUILD_UNIX_TIME = 1742336532651L; + public static final int GIT_REVISION = 55; + public static final String GIT_SHA = "c6dabfed26c00a5524f50eebce2067592d306665"; + public static final String GIT_DATE = "2025-03-18 16:46:24 PDT"; + public static final String GIT_BRANCH = "postLA"; + public static final String BUILD_DATE = "2025-03-19 16:57:49 PDT"; + public static final long BUILD_UNIX_TIME = 1742428669565L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5bb7744..61ddf26 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,8 +13,6 @@ import com.pathplanner.lib.commands.PathfindingCommand; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; @@ -30,7 +28,6 @@ import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; import frc.robot.commands.PIDtoNearest; -import frc.robot.commands.PIDtoPosition; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; @@ -128,9 +125,9 @@ public Robot() { GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); + NamedCommands.registerCommand("AfterIntake", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putData("posey posey", new PIDtoNearest(false)); // Warmup Commands for PathPlanner PathfindingCommand.warmupCommand().schedule(); diff --git a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java index ca92f6f..f2d7213 100644 --- a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java @@ -85,4 +85,4 @@ public boolean isFinished() { return (Timer.getFPGATimestamp() - readyToShoot > 0.4) && (!waitForStall || EndEffector.get().isStalling()); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/PIDtoNearest.java b/src/main/java/frc/robot/commands/PIDtoNearest.java index f53c2b5..0245ab0 100644 --- a/src/main/java/frc/robot/commands/PIDtoNearest.java +++ b/src/main/java/frc/robot/commands/PIDtoNearest.java @@ -113,7 +113,7 @@ public PIDtoNearest(boolean ignoreLR) { public void initialize() { Pose2d currPose = Swerve.get().getState().Pose; goalPose = ignoreLR?findClosestPose(currPose):findClosestPose(currPose, HumanControls.OperatorPanel2025.leftOrRight.getAsBoolean()?ReefSide.Right:ReefSide.Left); - System.out.println("voy a ir a " + goalPose.getX() + " y " + goalPose.getY()); + //System.out.println("voy a ir a " + goalPose.getX() + " y " + goalPose.getY()); xController.reset(currPose.getX()); yController.reset(currPose.getY()); omegaController.reset(currPose.getRotation().getRadians()); From ba87a3b85705a755da803587563fcb8ec16a7c78 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Thu, 20 Mar 2025 14:36:38 -0700 Subject: [PATCH 20/37] add leftside auto, change positions, make auto position faster --- .../deploy/pathplanner/autos/LeftSide.auto | 12 ++++++++++ .../deploy/pathplanner/paths/backtoreef.path | 24 ++++++++++++++----- src/main/deploy/pathplanner/paths/end.path | 24 ++++++++++++++----- .../pathplanner/paths/thentosource.path | 4 ++-- .../deploy/pathplanner/paths/tosource2.path | 4 ++-- src/main/java/frc/robot/BuildConstants.java | 10 ++++---- src/main/java/frc/robot/Robot.java | 6 +++-- .../java/frc/robot/commands/PIDtoNearest.java | 13 ++++------ .../frc/robot/commands/PIDtoPosition.java | 4 ++-- .../frc/robot/subsystems/GroundCoral.java | 14 ++++++----- .../java/frc/robot/subsystems/Swerve.java | 6 ++--- src/main/java/frc/robot/util/GameInfo.java | 6 ++--- 12 files changed, 81 insertions(+), 46 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/LeftSide.auto b/src/main/deploy/pathplanner/autos/LeftSide.auto index 3ff0f9a..96da847 100644 --- a/src/main/deploy/pathplanner/autos/LeftSide.auto +++ b/src/main/deploy/pathplanner/autos/LeftSide.auto @@ -57,6 +57,12 @@ "data": { "pathName": "backtoreef" } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } } ] } @@ -108,6 +114,12 @@ "data": { "pathName": "end" } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index 300a479..e0beb18 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2621575342465754, + "x": 1.248, "y": 7.345676369863014 }, "prevControl": null, "nextControl": { - "x": 2.178724315068493, - "y": 6.98505993150685 + "x": 2.0205592105263155, + "y": 6.747944078947367 }, "isLocked": false, "linkedName": "leftsource" @@ -20,8 +20,8 @@ "y": 5.1 }, "prevControl": { - "x": 3.080265410958904, - "y": 5.362285958904109 + "x": 3.6754934210526313, + "y": 5.785773026315789 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.68716577540107, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], "globalConstraints": { "maxVelocity": 4.5, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index fb6f2ef..db0b2f7 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2621575342465754, + "x": 1.248, "y": 7.345676369863014 }, "prevControl": null, "nextControl": { - "x": 2.5693921232876713, - "y": 5.843107876712329 + "x": 1.9339638157894734, + "y": 6.747944078947367 }, "isLocked": false, "linkedName": "leftsource" @@ -20,8 +20,8 @@ "y": 4.96 }, "prevControl": { - "x": 2.4191352739726026, - "y": 5.767979452054794 + "x": 2.8768914473684206, + "y": 6.064802631578947 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.6377005347593584, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], "globalConstraints": { "maxVelocity": 4.5, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path index 2d68286..2bb8b6d 100644 --- a/src/main/deploy/pathplanner/paths/thentosource.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.2621575342465754, + "x": 1.248, "y": 7.345676369863014 }, "prevControl": { - "x": 2.76472602739726, + "x": 2.7505684931506846, "y": 6.203724315068493 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/tosource2.path b/src/main/deploy/pathplanner/paths/tosource2.path index c42c31b..d2afa21 100644 --- a/src/main/deploy/pathplanner/paths/tosource2.path +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.2621575342465754, + "x": 1.248, "y": 7.345676369863014 }, "prevControl": { - "x": 2.4792380136986303, + "x": 2.465080479452055, "y": 6.008390410958905 }, "nextControl": null, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index a90f59d..2563578 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 55; - public static final String GIT_SHA = "c6dabfed26c00a5524f50eebce2067592d306665"; - public static final String GIT_DATE = "2025-03-18 16:46:24 PDT"; + public static final int GIT_REVISION = 56; + public static final String GIT_SHA = "47650a0c4f0865317e72e3720194156da9ff54f5"; + public static final String GIT_DATE = "2025-03-19 17:02:23 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-19 16:57:49 PDT"; - public static final long BUILD_UNIX_TIME = 1742428669565L; + public static final String BUILD_DATE = "2025-03-20 14:23:49 PDT"; + public static final long BUILD_UNIX_TIME = 1742505829911L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 61ddf26..424f83f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; @@ -154,6 +155,7 @@ public Robot() { } private void configureDriverStationBinds() { + HumanControls.DriverPanel.resetGyro.whileTrue(new PIDtoNearest(false)); HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(() -> Swerve.get().seedFieldCentric())); HumanControls.OperatorPanel2025.releaseCoral.whileTrue( new ConditionalCommand( @@ -181,7 +183,6 @@ private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.GroundCoral.whileTrue( new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), () -> !HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean())); - HumanControls.OperatorPanel2025.L1.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), @@ -219,7 +220,8 @@ private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.Climb1.whileTrue(new PIDtoNearest(false)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceProcessor, Swerve.get()::FaceSource))); - // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new + HumanControls.OperatorPanel2025.releaseCoral.and(HumanControls.OperatorPanel2025.pickupAlgae).whileTrue(new PrintCommand("trebuchet")); + // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new // MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); } diff --git a/src/main/java/frc/robot/commands/PIDtoNearest.java b/src/main/java/frc/robot/commands/PIDtoNearest.java index 0245ab0..4903c49 100644 --- a/src/main/java/frc/robot/commands/PIDtoNearest.java +++ b/src/main/java/frc/robot/commands/PIDtoNearest.java @@ -5,23 +5,17 @@ package frc.robot.commands; -import static edu.wpi.first.units.Units.Meters; - import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.HumanControls; -import frc.robot.HumanControls.OperatorPanel2025; import frc.robot.subsystems.Swerve; import frc.robot.util.GameInfo; //import frc.robot.util.PoseUtil; import frc.robot.util.GameInfo.ReefSide; -import frc.team696.lib.Util; import frc.team696.lib.Logging.BackupLogger; /** @@ -97,12 +91,13 @@ public PIDtoNearest() { */ public PIDtoNearest(boolean ignoreLR) { addRequirements(Swerve.get()); - xController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(1.3, 1.4)); - yController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(1.3, 1.4)); + xController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(2.45, 2.2)); + yController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(2.45 + , 2.2)); xController.setTolerance(0.01); yController.setTolerance(0.01); - omegaController = new ProfiledPIDController(10, 0, 0, new TrapezoidProfile.Constraints(5, 2)); + omegaController = new ProfiledPIDController(10, 0, 0, new TrapezoidProfile.Constraints(5, 3.5)); omegaController.enableContinuousInput(-Math.PI, Math.PI); omegaController.setTolerance(0.08); this.ignoreLR = ignoreLR; diff --git a/src/main/java/frc/robot/commands/PIDtoPosition.java b/src/main/java/frc/robot/commands/PIDtoPosition.java index 60a4ede..b0a0e23 100644 --- a/src/main/java/frc/robot/commands/PIDtoPosition.java +++ b/src/main/java/frc/robot/commands/PIDtoPosition.java @@ -23,11 +23,11 @@ public class PIDtoPosition extends Command { private ProfiledPIDController xController, yController, omegaController; private Pose2d goalPose; - private double calculateWithTolerance(ProfiledPIDController controller, double measurement, double goal){ + /*private double calculateWithTolerance(ProfiledPIDController controller, double measurement, double goal){ double tolerance=controller.getPositionTolerance(); double error=goal-measurement; return Math.abs(error) { position(Positions.Intake.value); - rollerMotor.set(0.7); + rollerMotor.set(0.8); }, () -> { angleMotor.stopMotor(); - rollerMotor.stopMotor(); + //rollerMotor.stopMotor(); }); } @@ -106,6 +106,8 @@ public Command Stowed() { this::stop); } + + public Command Ready() { return this.runEnd( () -> { @@ -114,7 +116,7 @@ public Command Ready() { } else { position(Positions.Stowed.value); } - rollerMotor.set(0.3); + rollerMotor.set(0.4); }, this::stop); } @@ -122,7 +124,7 @@ public Command Ready() { public Command Spit() { return this.runEnd(() -> { position(Positions.Spit.value); - rollerMotor.set(-0.4); + rollerMotor.set(-0.3); }, this::stop); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 8e37d72..87e8788 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -321,9 +321,9 @@ public void periodic() { setVisionMeasurementStdDevs(VecBuilder.fill(0.001, 0.001, 0.001)); } else { setVisionMeasurementStdDevs( - VecBuilder.fill(Estimate.ambiguity * Math.pow(Estimate.distToTag, 2), - Estimate.ambiguity * Math.pow(Estimate.distToTag, 2), - Estimate.ambiguity * Math.pow(Estimate.distToTag, 2))); + VecBuilder.fill(Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.15, + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.15, + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.15)); } return true; }; diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 414dbd0..9cedfa7 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -131,16 +131,16 @@ RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1 - wristOffset), RobotSide.Back, new CoralScoringPosition(0, -1., -8. - wristOffset)), Position.L2, Map.of( RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56 - wristOffset), - RobotSide.Back, new CoralScoringPosition(4., -1., -8.1 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(3., -1., -8.1 - wristOffset)), Position.L3, Map.of( RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1 - wristOffset), RobotSide.Back, new CoralScoringPosition(25., -1., -8.1 - wristOffset)), Position.L4, Map.of( RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6 - wristOffset), - RobotSide.Back, new CoralScoringPosition(64, -1.1, -9. - wristOffset)), + RobotSide.Back, new CoralScoringPosition(62, -1.1, -9. - wristOffset)), Position.Intake, Map.of( RobotSide.Front, new CoralScoringPosition(6., 1., -0.9 - wristOffset), - RobotSide.Back, new CoralScoringPosition(0, 0, 0 - wristOffset))); + RobotSide.Back, new CoralScoringPosition(0, 0, 0.3 - wristOffset))); L2Algae = new CoralScoringPosition(17., -3., 1. - wristOffset); L3Algae = new CoralScoringPosition(38., -3., 1. - wristOffset); Net = new CoralScoringPosition(67., 0., 6.5 - wristOffset); From 72491d54f68fcec3f3403fc113b7da99ba758832 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Thu, 20 Mar 2025 18:00:38 -0700 Subject: [PATCH 21/37] create middle auto --- .../deploy/pathplanner/autos/MiddleNet.auto | 49 +++++++++++++++++ .../deploy/pathplanner/paths/middlemove.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/middlestart.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/middletonet.path | 54 +++++++++++++++++++ src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/subsystems/GroundCoral.java | 10 ++-- .../java/frc/robot/subsystems/Swerve.java | 2 +- 8 files changed, 223 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/MiddleNet.auto create mode 100644 src/main/deploy/pathplanner/paths/middlemove.path create mode 100644 src/main/deploy/pathplanner/paths/middlestart.path create mode 100644 src/main/deploy/pathplanner/paths/middletonet.path diff --git a/src/main/deploy/pathplanner/autos/MiddleNet.auto b/src/main/deploy/pathplanner/autos/MiddleNet.auto new file mode 100644 index 0000000..b82267e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MiddleNet.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middlestart" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "middlemove" + } + }, + { + "type": "named", + "data": { + "name": "L3Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "middletonet" + } + }, + { + "type": "named", + "data": { + "name": "Barge" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path new file mode 100644 index 0000000..c326b26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.675, + "y": 3.875 + }, + "prevControl": null, + "nextControl": { + "x": 5.675, + "y": 4.125 + }, + "isLocked": false, + "linkedName": "FourLeft" + }, + { + "anchor": { + "x": 5.675, + "y": 4.27516447368421 + }, + "prevControl": { + "x": 5.675, + "y": 4.02516447368421 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FourBall" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "MiddleAndNet", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middlestart.path b/src/main/deploy/pathplanner/paths/middlestart.path new file mode 100644 index 0000000..6b1f535 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middlestart.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.05, + "y": 3.875 + }, + "prevControl": null, + "nextControl": { + "x": 6.389948058621888, + "y": 3.875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.675, + "y": 3.875 + }, + "prevControl": { + "x": 6.010450938895257, + "y": 3.875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FourLeft" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "MiddleAndNet", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path new file mode 100644 index 0000000..752b465 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.675, + "y": 4.27516447368421 + }, + "prevControl": null, + "nextControl": { + "x": 6.551198630136986, + "y": 5.251833994232156 + }, + "isLocked": false, + "linkedName": "FourBall" + }, + { + "anchor": { + "x": 7.618022260273973, + "y": 6.6995719178082185 + }, + "prevControl": { + "x": 6.656378424657535, + "y": 6.158647260273973 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "MiddleAndNet", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 2563578..7345240 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 56; - public static final String GIT_SHA = "47650a0c4f0865317e72e3720194156da9ff54f5"; - public static final String GIT_DATE = "2025-03-19 17:02:23 PDT"; + public static final int GIT_REVISION = 57; + public static final String GIT_SHA = "ba87a3b85705a755da803587563fcb8ec16a7c78"; + public static final String GIT_DATE = "2025-03-20 14:36:38 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-20 14:23:49 PDT"; - public static final long BUILD_UNIX_TIME = 1742505829911L; + public static final String BUILD_DATE = "2025-03-20 17:57:00 PDT"; + public static final long BUILD_UNIX_TIME = 1742518620311L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 424f83f..4f37b11 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -127,6 +127,8 @@ public Robot() { NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); NamedCommands.registerCommand("AfterIntake", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1).asProxy()); + NamedCommands.registerCommand("Barge", new MoveSuperStructure(GameInfo.Net, 1.).asProxy()); + NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index 9be925a..4bf2897 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -88,7 +88,7 @@ public Command Intake() { rollerMotor.set(0.8); }, () -> { angleMotor.stopMotor(); - //rollerMotor.stopMotor(); + rollerMotor.stopMotor(); }); } @@ -106,8 +106,6 @@ public Command Stowed() { this::stop); } - - public Command Ready() { return this.runEnd( () -> { @@ -116,7 +114,7 @@ public Command Ready() { } else { position(Positions.Stowed.value); } - rollerMotor.set(0.4); + rollerMotor.set(0.45); }, this::stop); } @@ -124,7 +122,7 @@ public Command Ready() { public Command Spit() { return this.runEnd(() -> { position(Positions.Spit.value); - rollerMotor.set(-0.3); + rollerMotor.set(-0.2); }, this::stop); } @@ -133,4 +131,4 @@ public void periodic() { SmartDashboard.putNumber("GroundCoral/Position", getPosition()); // This method will be called once per scheduler run } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 87e8788..422e0ca 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -312,7 +312,7 @@ public void periodic() { if (Estimate.distToTag > 3.5) return false; - if (Estimate.ambiguity > 0.4) + if (Estimate.ambiguity > 0.6) return false; // Too Ambiguous, Ignore if (getState().Speeds.omegaRadiansPerSecond > 2.5) return false; // Rotating too fast, ignore From dfa4ee8a5ab49aaddbf4b0a144c55894ee6e3f3a Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Fri, 21 Mar 2025 15:37:52 -0700 Subject: [PATCH 22/37] add climber code (untested) --- README.md | 3 +- src/main/java/frc/robot/BotConstants.java | 10 +++ src/main/java/frc/robot/BuildConstants.java | 10 +-- .../java/frc/robot/subsystems/Climber.java | 62 +++++++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 7 ++- 5 files changed, 84 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Climber.java diff --git a/README.md b/README.md index 1560f68..6677a2a 100644 --- a/README.md +++ b/README.md @@ -1 +1,2 @@ -# 2025BuildSeason \ No newline at end of file +# 2025BuildSeason +The official robot code for the 2025 onseason robot, Hail Mary \ No newline at end of file diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 2cd882d..2f9df8a 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -105,5 +105,15 @@ public static class GroundCoral { } } + + public static class Climber{ + public static int motorID=19; + public static TalonFXConfiguration cfg=new TalonFXConfiguration(); + static{ + cfg.Slot0.kP=1; + cfg.CurrentLimits.StatorCurrentLimitEnable=false; + + } + } } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7345240..25e2d06 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 57; - public static final String GIT_SHA = "ba87a3b85705a755da803587563fcb8ec16a7c78"; - public static final String GIT_DATE = "2025-03-20 14:36:38 PDT"; + public static final int GIT_REVISION = 58; + public static final String GIT_SHA = "72491d54f68fcec3f3403fc113b7da99ba758832"; + public static final String GIT_DATE = "2025-03-20 18:00:38 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-20 17:57:00 PDT"; - public static final long BUILD_UNIX_TIME = 1742518620311L; + public static final String BUILD_DATE = "2025-03-21 15:19:57 PDT"; + public static final long BUILD_UNIX_TIME = 1742595597220L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..76d8b94 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.BotConstants; +import frc.team696.lib.Logging.BackupLogger; + +public class Climber extends SubsystemBase { + private static Climber m_Climber=null; + public static synchronized Climber get(){ + if(m_Climber==null){ + m_Climber=new Climber(); + } + return m_Climber; + } + + private TalonFX motor; + private MotionMagicVoltage positionReq; + + private Climber() { + motor=new TalonFX(BotConstants.Climber.motorID, BotConstants.canivoreBus); + positionReq=new MotionMagicVoltage(0); + zero(); + this.setDefaultCommand(In()); + } + + public void stop(){ + motor.stopMotor(); + } + + public Command Out(){ + return this.runEnd( + ()->{motor.setControl(positionReq.withPosition(14));}, + ()->{stop();} + ); + } + public Command In(){ + return this.runEnd( + ()->{motor.setControl(positionReq.withPosition(0));}, + ()->{stop();} + ); + } + public void resetPosition(double newPosition){ + motor.setPosition(newPosition); + } + + public void zero(){ + resetPosition(0); + } + + @Override + public void periodic() { + BackupLogger.addToQueue("climber/Postion", motor.getPosition().getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 422e0ca..0c79f94 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -392,12 +392,12 @@ public Rotation2d angleTo(Pose2d position) { return angleTo(position.getTranslation()); } - public Rotation2d FaceHexFace() { + public Rotation2d FaceHexFace(Translation2d pose) { Translation2d reefPosition = Util.getAlliance() == Alliance.Blue ? GameInfo.blueReef : (new Translation2d(GameInfo.fieldLengthMeters.in(Meters) - GameInfo.blueReef.getX(), GameInfo.blueReef.getY())); - Rotation2d angleToReef = getPose().getTranslation().minus(reefPosition).getAngle(); + Rotation2d angleToReef = pose.minus(reefPosition).getAngle(); Rotation2d hexAngleToReef = Rotation2d .fromDegrees( MathUtil.inputModulus( @@ -406,6 +406,9 @@ public Rotation2d FaceHexFace() { return hexAngleToReef; } + public Rotation2d FaceHexFace() { + return FaceHexFace(Swerve.get().getPose().getTranslation()); + } public Rotation2d FaceNet() { return Util.getAlliance() == Alliance.Blue ? Rotation2d.fromDegrees(-90) : Rotation2d.fromDegrees(90); } From 957945c14d1b67169eea575fe4b9c660fe49e59d Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Sat, 22 Mar 2025 16:20:22 -0700 Subject: [PATCH 23/37] things working good --- .../deploy/pathplanner/paths/Fifth Test.path | 18 +- .../deploy/pathplanner/paths/First Test.path | 16 +- .../deploy/pathplanner/paths/Fourth Test.path | 16 +- .../deploy/pathplanner/paths/MiddleOne.path | 4 +- .../deploy/pathplanner/paths/Second Test.path | 18 +- .../deploy/pathplanner/paths/Third Test.path | 20 +- .../deploy/pathplanner/paths/backtoreef.path | 10 +- src/main/deploy/pathplanner/paths/end.path | 8 +- .../deploy/pathplanner/paths/gotoreef.path | 26 ++- .../deploy/pathplanner/paths/middlemove.path | 4 +- .../deploy/pathplanner/paths/middlestart.path | 4 +- .../deploy/pathplanner/paths/middletonet.path | 4 +- .../pathplanner/paths/thentosource.path | 18 +- .../deploy/pathplanner/paths/tosource2.path | 8 +- src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/Auto.java | 205 ------------------ src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/TunerConstants.java | 6 +- .../java/frc/robot/commands/PIDtoNearest.java | 5 +- .../frc/robot/commands/PIDtoPosition.java | 1 - .../java/frc/robot/subsystems/Climber.java | 43 ++-- .../java/frc/robot/subsystems/Swerve.java | 6 +- 23 files changed, 138 insertions(+), 324 deletions(-) delete mode 100644 src/main/java/frc/robot/Auto.java diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path index 4caf860..5af0781 100644 --- a/src/main/deploy/pathplanner/paths/Fifth Test.path +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -4,24 +4,24 @@ { "anchor": { "x": 1.2123355263157893, - "y": 0.9075657894736844 + "y": 1.0 }, "prevControl": null, "nextControl": { "x": 1.9762023285840344, - "y": 1.5811281045624415 + "y": 1.673562315088757 }, "isLocked": false, "linkedName": "balls" }, { "anchor": { - "x": 3.7139802631578944, - "y": 3.0435855263157894 + "x": 3.744, + "y": 3.044 }, "prevControl": { - "x": 2.519368499721011, - "y": 1.9875936315919234 + "x": 2.5493882365631166, + "y": 1.988008105276134 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "L4", - "waypointRelativePos": 0.6, + "waypointRelativePos": 0.7214285714285711, "endWaypointRelativePos": null, "command": { "type": "named", @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path index 2806cd5..7f26f75 100644 --- a/src/main/deploy/pathplanner/paths/First Test.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 6.896282128328403, + "x": 6.896, "y": 1.8521102995562126 }, "prevControl": null, "nextControl": { - "x": 5.944115584935899, + "x": 5.9438334566074955, "y": 2.27752134183925 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.91, - "y": 2.9057453464003937 + "x": 5.0, + "y": 2.92 }, "prevControl": { - "x": 5.787305612056213, - "y": 2.5072519877958577 + "x": 5.877305612056213, + "y": 2.521506641395464 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path index a32206e..464f2a4 100644 --- a/src/main/deploy/pathplanner/paths/Fourth Test.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.127713815789473, - "y": 2.92 + "x": 4.09, + "y": 2.87 }, "prevControl": null, "nextControl": { - "x": 3.138954107949237, - "y": 1.921626833703155 + "x": 3.1012402921597637, + "y": 1.8716268337031552 }, "isLocked": false, "linkedName": "third" @@ -17,11 +17,11 @@ { "anchor": { "x": 1.2123355263157893, - "y": 0.9075657894736844 + "y": 1.0 }, "prevControl": { "x": 2.3080690576732694, - "y": 1.5401878387630243 + "y": 1.63262204928934 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleOne.path b/src/main/deploy/pathplanner/paths/MiddleOne.path index 9d98c10..38e400f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleOne.path +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index 55f84df..cd61203 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.91, - "y": 2.9057453464003937 + "x": 5.0, + "y": 2.91 }, "prevControl": null, "nextControl": { - "x": 4.045470984344181, - "y": 1.7830794656065079 + "x": 4.135470984344181, + "y": 1.7873341192061143 }, "isLocked": false, "linkedName": "Nuts" @@ -17,11 +17,11 @@ { "anchor": { "x": 1.2123355263157893, - "y": 0.9075657894736844 + "y": 1.0 }, "prevControl": { "x": 2.705894836596852, - "y": 1.5314482321511993 + "y": 1.623882442677515 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 145.0079798014414 + "rotation": 145.0 }, "reversed": false, "folder": "RightSide", diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path index 75799e3..129a26e 100644 --- a/src/main/deploy/pathplanner/paths/Third Test.path +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.2123355263157893, - "y": 0.908 + "x": 1.212, + "y": 1.0 }, "prevControl": null, "nextControl": { - "x": 2.190303351759577, - "y": 1.4032481662968443 + "x": 2.1899678254437878, + "y": 1.4952481662968444 }, "isLocked": false, "linkedName": "balls" }, { "anchor": { - "x": 4.128, - "y": 2.92 + "x": 4.09, + "y": 2.87 }, "prevControl": { - "x": 3.0709996039264857, - "y": 2.033071357500476 + "x": 3.032999603926485, + "y": 1.9830713575004761 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index e0beb18..c91ef41 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.248, - "y": 7.345676369863014 + "y": 7.27 }, "prevControl": null, "nextControl": { "x": 2.0205592105263155, - "y": 6.747944078947367 + "y": 6.672267709084353 }, "isLocked": false, "linkedName": "leftsource" @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.68716577540107, + "waypointRelativePos": 0.6309523809523808, "endWaypointRelativePos": null, "command": { "type": "named", @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index db0b2f7..c9b5c61 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.248, - "y": 7.345676369863014 + "y": 7.27 }, "prevControl": null, "nextControl": { "x": 1.9339638157894734, - "y": 6.747944078947367 + "y": 6.672267709084353 }, "isLocked": false, "linkedName": "leftsource" @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path index 021ca94..4ff08b0 100644 --- a/src/main/deploy/pathplanner/paths/gotoreef.path +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.92, - "y": 5.18 + "x": 4.81, + "y": 5.21 }, "prevControl": { - "x": 5.522861842105263, - "y": 5.689555921052631 + "x": 5.412861842105262, + "y": 5.719555921052631 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,22 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.6023809523809524, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path index c326b26..577ae0c 100644 --- a/src/main/deploy/pathplanner/paths/middlemove.path +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middlestart.path b/src/main/deploy/pathplanner/paths/middlestart.path index 6b1f535..2ba5dd8 100644 --- a/src/main/deploy/pathplanner/paths/middlestart.path +++ b/src/main/deploy/pathplanner/paths/middlestart.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path index 752b465..31a18e9 100644 --- a/src/main/deploy/pathplanner/paths/middletonet.path +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path index 2bb8b6d..c706c67 100644 --- a/src/main/deploy/pathplanner/paths/thentosource.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.92, - "y": 5.18 + "x": 4.81, + "y": 5.21 }, "prevControl": null, "nextControl": { - "x": 4.24216609589041, - "y": 6.023467465753424 + "x": 4.13216609589041, + "y": 6.053467465753425 }, "isLocked": false, "linkedName": "ThreeRight" @@ -17,11 +17,11 @@ { "anchor": { "x": 1.248, - "y": 7.345676369863014 + "y": 7.27 }, "prevControl": { - "x": 2.7505684931506846, - "y": 6.203724315068493 + "x": 2.750568493150685, + "y": 6.128047945205479 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/tosource2.path b/src/main/deploy/pathplanner/paths/tosource2.path index d2afa21..5036bed 100644 --- a/src/main/deploy/pathplanner/paths/tosource2.path +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -17,11 +17,11 @@ { "anchor": { "x": 1.248, - "y": 7.345676369863014 + "y": 7.27 }, "prevControl": { "x": 2.465080479452055, - "y": 6.008390410958905 + "y": 5.93271404109589 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 3.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 1888b38..a706e2b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,8 +8,8 @@ "RightSide" ], "autoFolders": [], - "defaultMaxVel": 4.5, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 6.0, + "defaultMaxAccel": 3.2, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java deleted file mode 100644 index 4647ed8..0000000 --- a/src/main/java/frc/robot/Auto.java +++ /dev/null @@ -1,205 +0,0 @@ -package frc.robot; - -import static edu.wpi.first.units.Units.Amps; -import java.util.ArrayList; -import java.util.List; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.config.ModuleConfig; -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.path.PathPlannerPath; -import com.pathplanner.lib.util.PathPlannerLogging; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.subsystems.Swerve; -import frc.team696.lib.Dashboards.ShuffleDashboard; -import frc.team696.lib.Logging.BackupLogger; -import frc.team696.lib.Logging.PLog; -import frc.team696.lib.Swerve.SwerveConfigs; -import frc.team696.lib.Swerve.SwerveConstants; - -/** - * Houses all methods related to the Autonomous period and self driving during teleop - */ -@SuppressWarnings("resource") -public class Auto { - /** - * Wrapper For Pathplanners NamedCommand System - */ - public static class NamedCommand { - public String name; - public Command command; - - /** - * @param name to be used inside of the pathplanner GUI - * @param command to be run when called inside pathplanner - */ - public NamedCommand(String name, Command command) { - this.name = name; - this.command = command; - } - - public void register() { - NamedCommands.registerCommand(name, command); - } - } - - public static Auto _instance; - private Swerve _swerve; - - private SendableChooser _autoChooser; - - public SysIdRoutine _driveSysIdRoutine; - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds().withSteerRequestType(SteerRequestType.MotionMagicExpo).withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private Auto (Swerve swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister) { - _swerve = swerve; - - RobotConfig config = new RobotConfig( - SwerveConstants.MASS, - SwerveConstants.MOMENT_OF_INERTIA, - new ModuleConfig( - SwerveConstants.WHEEL_RADIUS, - SwerveConstants.MAX_VELOCITY, - SwerveConstants.WHEEL_COEFFICIENT_OF_FRICTION, - DCMotor.getKrakenX60(1), - Amps.of(SwerveConfigs.drive.CurrentLimits.StatorCurrentLimit), - 1), - SwerveConstants.modPositions); - if (shouldUseGUIValues) { - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - PLog.fatalException("Auto", "Failed To Get RobotConfig", e); - new Alert("Failed to fetch robotConfig GUI Settings", AlertType.kWarning).set(true); - } - } - - AutoBuilder.configure( - ()->_swerve.getState().Pose, - _swerve::resetPose, - ()->_swerve.getState().Speeds, - (speeds, feedforwards) -> m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()), - new PPHolonomicDriveController( - new PIDConstants(10.0, 0.0, 0.0), - new PIDConstants(7.0, 0.0, 0.0)), - config, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - _swerve - ); - - for (NamedCommand namedCommand : commandsToRegister) { - namedCommand.register(); - } - - PathPlannerLogging.setLogTargetPoseCallback((pose) -> { - BackupLogger.addToQueue("696/Auto/Desired", pose); - }); - - PathPlannerLogging.setLogActivePathCallback((poses) -> { - ShuffleDashboard.field.getObject("Path").setPoses(poses); - }); - - _autoChooser = AutoBuilder.buildAutoChooser(); - //SmartDashboard.putData(_autoChooser); - ShuffleDashboard.addAutoChooser(_autoChooser); - - _autoChooser.onChange((command)-> { - //visualize(); - }); - } - /** - * Initializes all things related to auto (ex. pathplanner) - * - * @param swerve The swerve subsystem - * @param commandsToRegister each command to register for path planner - */ - public static void Initialize(Swerve swerve, NamedCommand... commandsToRegister){ - if (_instance != null) throw new RuntimeException ("Don't Initialize Twice!"); - - _instance = new Auto(swerve, false, commandsToRegister); - } - - /** - * Initializes all things related to auto (ex. pathplanner) - * - * @param swerve The swerve subsystem - * @param shouldUseGUIValues Should Pathplanner fetch Robot Config from GUI? - * @param commandsToRegister each command to register for path planner - */ - public static void Initialize(Swerve swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister){ - if (_instance != null) throw new RuntimeException ("Don't Initialize Twice!"); - - _instance = new Auto(swerve, shouldUseGUIValues, commandsToRegister); - } - - /** - * @return The instance Of the Auto Class - */ - public static Auto get(){ - if (_instance == null) throw new RuntimeException ("Please Initialize First!"); - - return _instance; - } - - /** - * @return Selected Command From SendableChooser - */ - public Command Selected() { - return _autoChooser.getSelected(); - } - - /** - * @return Selected Command From SendableChooser Which Will End After 15 Seconds. - * Used for testing on home field - */ - public Command SelectedEndAt15() { - return Selected().raceWith(new WaitCommand(15.0)); - } - - public static Command PathFind(Pose2d end) { - return AutoBuilder.pathfindToPose(end, new PathConstraints(1, 1, Math.PI,Math.PI)); - } - - public void visualize(String name) { - List paths; - List pathPoses = new ArrayList(); - try { - paths = PathPlannerAuto.getPathGroupFromAutoFile(name); - } catch (Exception e) { - PLog.fatalException("Auto", "Failed To Find Path", e); - return; - } - for (int i = 0; i < paths.size(); i++) { - PathPlannerPath path = paths.get(i); - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) - path = path.flipPath(); - pathPoses.addAll(path.getPathPoses()); - } - ShuffleDashboard.field.getObject("traj").setPoses(pathPoses); - } - - public void visualize() { - visualize(_autoChooser.getSelected().getName()); - } - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 25e2d06..bd78fc8 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 58; - public static final String GIT_SHA = "72491d54f68fcec3f3403fc113b7da99ba758832"; - public static final String GIT_DATE = "2025-03-20 18:00:38 PDT"; + public static final int GIT_REVISION = 59; + public static final String GIT_SHA = "dfa4ee8a5ab49aaddbf4b0a144c55894ee6e3f3a"; + public static final String GIT_DATE = "2025-03-21 15:37:52 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-21 15:19:57 PDT"; - public static final long BUILD_UNIX_TIME = 1742595597220L; + public static final String BUILD_DATE = "2025-03-22 16:19:43 PDT"; + public static final long BUILD_UNIX_TIME = 1742685583973L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f37b11..748bce1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; @@ -96,6 +97,9 @@ public Robot() { Swerve.get(); Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); + logBuildInfo(); + putSwerveSysIDCalibrationButtons(); + SignalLogger.start(); configureDriverStationBinds(); Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( @@ -235,10 +239,8 @@ public void robotPeriodic() { // BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler // Time in Microseconds, anything over // 20,000 should - m_SwerveTelemetry.telemeterize(Swerve.get().getState()); - // BackupLogger.logSystemInformation(); - // + BackupLogger.logSystemInformation(); } @Override diff --git a/src/main/java/frc/robot/TunerConstants.java b/src/main/java/frc/robot/TunerConstants.java index df8872d..2742841 100644 --- a/src/main/java/frc/robot/TunerConstants.java +++ b/src/main/java/frc/robot/TunerConstants.java @@ -31,8 +31,8 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKP(0.18).withKI(0).withKD(0) + .withKS(0.1309).withKV(0.11584); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -48,7 +48,7 @@ public class TunerConstants { // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot diff --git a/src/main/java/frc/robot/commands/PIDtoNearest.java b/src/main/java/frc/robot/commands/PIDtoNearest.java index 4903c49..5728b8d 100644 --- a/src/main/java/frc/robot/commands/PIDtoNearest.java +++ b/src/main/java/frc/robot/commands/PIDtoNearest.java @@ -14,7 +14,6 @@ import frc.robot.HumanControls; import frc.robot.subsystems.Swerve; import frc.robot.util.GameInfo; -//import frc.robot.util.PoseUtil; import frc.robot.util.GameInfo.ReefSide; import frc.team696.lib.Logging.BackupLogger; @@ -92,8 +91,7 @@ public PIDtoNearest() { public PIDtoNearest(boolean ignoreLR) { addRequirements(Swerve.get()); xController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(2.45, 2.2)); - yController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(2.45 - , 2.2)); + yController = new ProfiledPIDController(8, 0.0, 0.0, new TrapezoidProfile.Constraints(2.45, 2.2)); xController.setTolerance(0.01); yController.setTolerance(0.01); @@ -129,7 +127,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - // System.out.println("There!"); Swerve.get().Drive(new ChassisSpeeds(0, 0, 0)); } diff --git a/src/main/java/frc/robot/commands/PIDtoPosition.java b/src/main/java/frc/robot/commands/PIDtoPosition.java index b0a0e23..a9810fe 100644 --- a/src/main/java/frc/robot/commands/PIDtoPosition.java +++ b/src/main/java/frc/robot/commands/PIDtoPosition.java @@ -74,7 +74,6 @@ public void execute() // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - System.out.println("There!"); Swerve.get().Drive(new ChassisSpeeds(0,0,0)); } public boolean atGoalPose(Pose2d goal, Pose2d curr){ diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 76d8b94..3c8dbc5 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -13,10 +13,11 @@ import frc.team696.lib.Logging.BackupLogger; public class Climber extends SubsystemBase { - private static Climber m_Climber=null; - public static synchronized Climber get(){ - if(m_Climber==null){ - m_Climber=new Climber(); + private static Climber m_Climber = null; + + public static synchronized Climber get() { + if (m_Climber == null) { + m_Climber = new Climber(); } return m_Climber; } @@ -25,33 +26,41 @@ public static synchronized Climber get(){ private MotionMagicVoltage positionReq; private Climber() { - motor=new TalonFX(BotConstants.Climber.motorID, BotConstants.canivoreBus); - positionReq=new MotionMagicVoltage(0); + motor = new TalonFX(BotConstants.Climber.motorID, BotConstants.canivoreBus); + positionReq = new MotionMagicVoltage(0); zero(); this.setDefaultCommand(In()); } - public void stop(){ + public void stop() { motor.stopMotor(); } - public Command Out(){ + public Command Out() { return this.runEnd( - ()->{motor.setControl(positionReq.withPosition(14));}, - ()->{stop();} - ); + () -> { + motor.setControl(positionReq.withPosition(14)); + }, + () -> { + stop(); + }); } - public Command In(){ + + public Command In() { return this.runEnd( - ()->{motor.setControl(positionReq.withPosition(0));}, - ()->{stop();} - ); + () -> { + motor.setControl(positionReq.withPosition(0)); + }, + () -> { + stop(); + }); } - public void resetPosition(double newPosition){ + + public void resetPosition(double newPosition) { motor.setPosition(newPosition); } - public void zero(){ + public void zero() { resetPosition(0); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0c79f94..8328951 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -321,9 +321,9 @@ public void periodic() { setVisionMeasurementStdDevs(VecBuilder.fill(0.001, 0.001, 0.001)); } else { setVisionMeasurementStdDevs( - VecBuilder.fill(Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.15, - Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.15, - Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.15)); + VecBuilder.fill(Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.01, + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.01, + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.01)); } return true; }; From 92aa4bd3481558cb9e53d7c23d4cd71c22a30be3 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Mon, 24 Mar 2025 19:13:03 -0700 Subject: [PATCH 24/37] monday --- .../deploy/pathplanner/paths/Fifth Test.path | 8 ++--- .../deploy/pathplanner/paths/Fourth Test.path | 12 +++---- .../deploy/pathplanner/paths/Second Test.path | 8 ++--- .../deploy/pathplanner/paths/Third Test.path | 4 +-- .../deploy/pathplanner/paths/backtoreef.path | 2 +- src/main/deploy/pathplanner/paths/end.path | 2 +- .../deploy/pathplanner/paths/gotoreef.path | 6 ++-- .../pathplanner/paths/thentosource.path | 4 +-- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/BotConstants.java | 7 +++-- src/main/java/frc/robot/BuildConstants.java | 10 +++--- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/Elevator.java | 6 ++++ .../java/frc/robot/subsystems/Swerve.java | 31 +++++++++++++++++-- src/main/java/frc/robot/util/GameInfo.java | 15 ++++++--- 15 files changed, 80 insertions(+), 39 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path index 5af0781..6edebcd 100644 --- a/src/main/deploy/pathplanner/paths/Fifth Test.path +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2123355263157893, - "y": 1.0 + "x": 1.212, + "y": 0.91 }, "prevControl": null, "nextControl": { - "x": 1.9762023285840344, - "y": 1.673562315088757 + "x": 1.975866802268245, + "y": 1.5835623150887574 }, "isLocked": false, "linkedName": "balls" diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path index 464f2a4..21bcf4e 100644 --- a/src/main/deploy/pathplanner/paths/Fourth Test.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -4,24 +4,24 @@ { "anchor": { "x": 4.09, - "y": 2.87 + "y": 2.8600000000000003 }, "prevControl": null, "nextControl": { "x": 3.1012402921597637, - "y": 1.8716268337031552 + "y": 1.8616268337031554 }, "isLocked": false, "linkedName": "third" }, { "anchor": { - "x": 1.2123355263157893, - "y": 1.0 + "x": 1.212, + "y": 0.91 }, "prevControl": { - "x": 2.3080690576732694, - "y": 1.63262204928934 + "x": 2.30773353135748, + "y": 1.5426220492893399 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index cd61203..984a83e 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.2123355263157893, - "y": 1.0 + "x": 1.212, + "y": 0.91 }, "prevControl": { - "x": 2.705894836596852, - "y": 1.623882442677515 + "x": 2.7055593102810627, + "y": 1.5338824426775153 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path index 129a26e..505fa0a 100644 --- a/src/main/deploy/pathplanner/paths/Third Test.path +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.212, - "y": 1.0 + "y": 0.91 }, "prevControl": null, "nextControl": { "x": 2.1899678254437878, - "y": 1.4952481662968444 + "y": 1.4052481662968443 }, "isLocked": false, "linkedName": "balls" diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index c91ef41..5b44251 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.6309523809523808, + "waypointRelativePos": 0.6880952380952379, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index c9b5c61..775f65f 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.6377005347593584, + "waypointRelativePos": 0.7166666666666663, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path index 4ff08b0..0585be1 100644 --- a/src/main/deploy/pathplanner/paths/gotoreef.path +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 4.81, + "x": 4.91, "y": 5.21 }, "prevControl": { - "x": 5.412861842105262, + "x": 5.512861842105263, "y": 5.719555921052631 }, "nextControl": null, @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "L4", - "waypointRelativePos": 0.6023809523809524, + "waypointRelativePos": 0.72, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path index c706c67..dc45923 100644 --- a/src/main/deploy/pathplanner/paths/thentosource.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.81, + "x": 4.91, "y": 5.21 }, "prevControl": null, "nextControl": { - "x": 4.13216609589041, + "x": 4.232166095890411, "y": 6.053467465753425 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a706e2b..3e755a1 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,7 +13,7 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, - "robotMass": 115.0, + "robotMass": 107.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.044, diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 2f9df8a..c139053 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -31,14 +31,15 @@ public static class Elevator { public static TalonFXConfiguration cfg; static { cfg = new TalonFXConfiguration(); - cfg.Slot0.kP = 6.; + cfg.Slot0.kP = 16.;//6.; cfg.Slot0.kG = 0.05; // cfg.Slot0.kS = 0.02; cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; - cfg.MotionMagic.MotionMagicAcceleration = 120.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 120.; + cfg.MotionMagic.MotionMagicAcceleration = 140.; + //cfg.MotionMagic.MotionMagicJerk= 500; cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.CurrentLimits.StatorCurrentLimit = 120.; cfg.CurrentLimits.StatorCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bd78fc8..d4ddf2c 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 59; - public static final String GIT_SHA = "dfa4ee8a5ab49aaddbf4b0a144c55894ee6e3f3a"; - public static final String GIT_DATE = "2025-03-21 15:37:52 PDT"; + public static final int GIT_REVISION = 60; + public static final String GIT_SHA = "957945c14d1b67169eea575fe4b9c660fe49e59d"; + public static final String GIT_DATE = "2025-03-22 16:20:22 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-22 16:19:43 PDT"; - public static final long BUILD_UNIX_TIME = 1742685583973L; + public static final String BUILD_DATE = "2025-03-24 16:23:24 PDT"; + public static final long BUILD_UNIX_TIME = 1742858604753L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 748bce1..31067f1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -82,7 +82,7 @@ public void putSwerveSysIDCalibrationButtons() { SmartDashboard.putData("CTRESwerveCalibration/QuasistaticForward", Swerve.get().sysIdQuasistatic(SysIdRoutine.Direction.kForward)); SmartDashboard.putData("CTRESwerveCalibration/QuasistaticReverse", - Swerve.get().sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + Swerve.get().sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } private final SendableChooser autoChooser; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 1209252..c0ee7c0 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Second; @@ -124,5 +125,10 @@ public void resetPosition(double newPosition) { @Override public void periodic() { + if(Math.abs(Swerve.get().getPigeon2().getRoll().getValue().in(Degrees))>15||Math.abs(Swerve.get().getPigeon2().getPitch().getValue().in(Degrees))>15){ + if(this.getCurrentCommand()!=null) + this.getCurrentCommand().cancel(); + goToPosition(0); + } } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 8328951..add1822 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -91,6 +91,8 @@ public static synchronized Swerve get() { /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + /* * SysId routine for characterizing translation. This is used to find PID gains * for the drive motors. @@ -106,9 +108,34 @@ public static synchronized Swerve get() { output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per secondĀ², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this + ) + ); /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineRotation;//m_sysIdRoutineTranslation; /** * Constructs a CTRE SwerveDrivetrain using the specified constants. diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 9cedfa7..d098c9f 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -10,6 +10,10 @@ import java.util.Map; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.IdealStartingState; +import com.pathplanner.lib.path.Waypoint; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -69,7 +73,10 @@ public static Translation2d mirrorTranslation(Translation2d starting) { return new Translation2d(17.55- starting.getX(), starting.getY()); } public static Translation2d mirrorTranslationXY(Translation2d starting) { - return new Translation2d(17.55- starting.getX(), fieldWidthMeters.in(Meters)-starting.getY()); + Waypoint temp = new Waypoint(starting, starting, starting); + temp.flip(); + //return temp.prevControl(); + return new Translation2d(17.55- starting.getX(), 8.05-starting.getY()); } public static Map> getScoringPoses(){ @@ -131,13 +138,13 @@ RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1 - wristOffset), RobotSide.Back, new CoralScoringPosition(0, -1., -8. - wristOffset)), Position.L2, Map.of( RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56 - wristOffset), - RobotSide.Back, new CoralScoringPosition(3., -1., -8.1 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(3., -1., -8.3 - wristOffset)), Position.L3, Map.of( RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1 - wristOffset), - RobotSide.Back, new CoralScoringPosition(25., -1., -8.1 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(25., -1., -8.3 - wristOffset)), Position.L4, Map.of( RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6 - wristOffset), - RobotSide.Back, new CoralScoringPosition(62, -1.1, -9. - wristOffset)), + RobotSide.Back, new CoralScoringPosition(64, -1.4, -9.7 - wristOffset)), Position.Intake, Map.of( RobotSide.Front, new CoralScoringPosition(6., 1., -0.9 - wristOffset), RobotSide.Back, new CoralScoringPosition(0, 0, 0.3 - wristOffset))); From 996be687d8b72f9bb672330a5cd759d71e115d13 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Tue, 25 Mar 2025 21:02:19 -0700 Subject: [PATCH 25/37] tuesday --- .../deploy/pathplanner/paths/Fifth Test.path | 4 ++-- .../deploy/pathplanner/paths/First Test.path | 4 ++-- .../deploy/pathplanner/paths/Fourth Test.path | 12 +++++------ .../deploy/pathplanner/paths/MiddleOne.path | 4 ++-- .../deploy/pathplanner/paths/Second Test.path | 4 ++-- .../deploy/pathplanner/paths/Third Test.path | 14 ++++++------- .../deploy/pathplanner/paths/backtoreef.path | 20 +++++++++---------- src/main/deploy/pathplanner/paths/end.path | 18 ++++++++--------- .../deploy/pathplanner/paths/gotoreef.path | 14 ++++++------- .../deploy/pathplanner/paths/middlemove.path | 4 ++-- .../deploy/pathplanner/paths/middlestart.path | 4 ++-- .../deploy/pathplanner/paths/middletonet.path | 4 ++-- .../pathplanner/paths/thentosource.path | 16 +++++++-------- .../deploy/pathplanner/paths/tosource2.path | 20 +++++++++---------- src/main/deploy/pathplanner/settings.json | 4 ++-- src/main/java/frc/robot/BotConstants.java | 4 ++-- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 3 +-- .../java/frc/robot/subsystems/Swerve.java | 6 +++--- src/main/java/frc/robot/util/GameInfo.java | 2 +- src/main/java/frc/team696 | 2 +- 21 files changed, 86 insertions(+), 87 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path index 6edebcd..0ac59d0 100644 --- a/src/main/deploy/pathplanner/paths/Fifth Test.path +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path index 7f26f75..c97f4b6 100644 --- a/src/main/deploy/pathplanner/paths/First Test.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path index 21bcf4e..4718d06 100644 --- a/src/main/deploy/pathplanner/paths/Fourth Test.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.09, - "y": 2.8600000000000003 + "x": 4.071960616438356, + "y": 2.8529965753424653 }, "prevControl": null, "nextControl": { - "x": 3.1012402921597637, - "y": 1.8616268337031554 + "x": 3.08320090859812, + "y": 1.8546234090456204 }, "isLocked": false, "linkedName": "third" @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleOne.path b/src/main/deploy/pathplanner/paths/MiddleOne.path index 38e400f..9d98c10 100644 --- a/src/main/deploy/pathplanner/paths/MiddleOne.path +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index 984a83e..94c84f2 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path index 505fa0a..78378e1 100644 --- a/src/main/deploy/pathplanner/paths/Third Test.path +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -9,19 +9,19 @@ "prevControl": null, "nextControl": { "x": 2.1899678254437878, - "y": 1.4052481662968443 + "y": 1.4052481662968441 }, "isLocked": false, "linkedName": "balls" }, { "anchor": { - "x": 4.09, - "y": 2.87 + "x": 4.071960616438356, + "y": 2.8529965753424653 }, "prevControl": { - "x": 3.032999603926485, - "y": 1.9830713575004761 + "x": 3.0149602203648413, + "y": 1.9660679328429413 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index 5b44251..ad7930e 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.248, - "y": 7.27 + "x": 1.2471318493150685, + "y": 7.285573630136986 }, "prevControl": null, "nextControl": { - "x": 2.0205592105263155, - "y": 6.672267709084353 + "x": 2.019691059841383, + "y": 6.687841339221339 }, "isLocked": false, "linkedName": "leftsource" }, { "anchor": { - "x": 4.02, - "y": 5.1 + "x": 3.8916523972602737, + "y": 5.076797945205479 }, "prevControl": { - "x": 3.6754934210526313, - "y": 5.785773026315789 + "x": 3.5471458183129054, + "y": 5.762570971521269 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index 775f65f..c2ae328 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 1.248, - "y": 7.27 + "x": 1.2471318493150685, + "y": 7.285573630136986 }, "prevControl": null, "nextControl": { - "x": 1.9339638157894734, - "y": 6.672267709084353 + "x": 1.933095665104542, + "y": 6.687841339221339 }, "isLocked": false, "linkedName": "leftsource" }, { "anchor": { - "x": 3.66, + "x": 3.61, "y": 4.96 }, "prevControl": { - "x": 2.8768914473684206, + "x": 2.8268914473684204, "y": 6.064802631578947 }, "nextControl": null, @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.7166666666666663, + "waypointRelativePos": 0.5880952380952383, "endWaypointRelativePos": null, "command": { "type": "named", @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path index 0585be1..e9d3d17 100644 --- a/src/main/deploy/pathplanner/paths/gotoreef.path +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.897, - "y": 5.873159246575342 + "x": 7.077097602739726, + "y": 6.038441780821918 }, "prevControl": null, "nextControl": { - "x": 5.6671875, - "y": 5.737664473684211 + "x": 5.8472851027397255, + "y": 5.902947007930787 }, "isLocked": false, "linkedName": null @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, @@ -54,7 +54,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -30.13119678106381 + "rotation": -29.999999999999996 }, "reversed": false, "folder": "LeftSide", diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path index 577ae0c..c326b26 100644 --- a/src/main/deploy/pathplanner/paths/middlemove.path +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middlestart.path b/src/main/deploy/pathplanner/paths/middlestart.path index 2ba5dd8..6b1f535 100644 --- a/src/main/deploy/pathplanner/paths/middlestart.path +++ b/src/main/deploy/pathplanner/paths/middlestart.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path index 31a18e9..752b465 100644 --- a/src/main/deploy/pathplanner/paths/middletonet.path +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path index dc45923..833321d 100644 --- a/src/main/deploy/pathplanner/paths/thentosource.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.248, - "y": 7.27 + "x": 1.2471318493150685, + "y": 7.285573630136986 }, "prevControl": { - "x": 2.750568493150685, - "y": 6.128047945205479 + "x": 2.7497003424657525, + "y": 6.143621575342465 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 36.86989764584405 + "rotation": 36.87 }, "reversed": false, "folder": "LeftSide", "idealStartingState": { "velocity": 0, - "rotation": -30.13119678106381 + "rotation": -29.999999999999996 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/tosource2.path b/src/main/deploy/pathplanner/paths/tosource2.path index 5036bed..96e000f 100644 --- a/src/main/deploy/pathplanner/paths/tosource2.path +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.02, - "y": 5.1 + "x": 3.8916523972602737, + "y": 5.076797945205479 }, "prevControl": null, "nextControl": { - "x": 3.1403681506849317, - "y": 6.128595890410959 + "x": 3.012020547945206, + "y": 6.105393835616439 }, "isLocked": false, "linkedName": "TwoLeft" }, { "anchor": { - "x": 1.248, - "y": 7.27 + "x": 1.2471318493150685, + "y": 7.285573630136986 }, "prevControl": { - "x": 2.465080479452055, - "y": 5.93271404109589 + "x": 2.4642123287671236, + "y": 5.948287671232876 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 3e755a1..d007c5a 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,8 +8,8 @@ "RightSide" ], "autoFolders": [], - "defaultMaxVel": 6.0, - "defaultMaxAccel": 3.2, + "defaultMaxVel": 4.5, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index c139053..c81913b 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -37,8 +37,8 @@ public static class Elevator { cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - cfg.MotionMagic.MotionMagicCruiseVelocity = 120.; - cfg.MotionMagic.MotionMagicAcceleration = 140.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; + cfg.MotionMagic.MotionMagicAcceleration = 120.; //cfg.MotionMagic.MotionMagicJerk= 500; cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.CurrentLimits.StatorCurrentLimit = 120.; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d4ddf2c..5f3d847 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 60; - public static final String GIT_SHA = "957945c14d1b67169eea575fe4b9c660fe49e59d"; - public static final String GIT_DATE = "2025-03-22 16:20:22 PDT"; + public static final int GIT_REVISION = 61; + public static final String GIT_SHA = "92aa4bd3481558cb9e53d7c23d4cd71c22a30be3"; + public static final String GIT_DATE = "2025-03-24 19:13:03 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-24 16:23:24 PDT"; - public static final long BUILD_UNIX_TIME = 1742858604753L; + public static final String BUILD_DATE = "2025-03-25 20:37:38 PDT"; + public static final long BUILD_UNIX_TIME = 1742960258205L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 31067f1..8ac3232 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -93,12 +93,11 @@ public Robot() { Elevator.get(); EndEffector.get(); GroundCoral.get(); - LED.get(); + //LED.get(); Swerve.get(); Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); logBuildInfo(); - putSwerveSysIDCalibrationButtons(); SignalLogger.start(); configureDriverStationBinds(); Swerve.get().setDefaultCommand(Swerve.get().applyRequest( diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index add1822..f790537 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -51,7 +51,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private static Swerve m_CommandSwerveDrivetrain = null; - public LimeLightCam CamA = new LimeLightCam("limelight-right"); + public LimeLightCam CamA = new LimeLightCam("limelight"); public LimeLightCam CamB = new LimeLightCam("limelight-left"); Rotation2d yawOffset = new Rotation2d(0); @@ -263,9 +263,9 @@ private void configureAutoBuilder() { .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), new PPHolonomicDriveController( // PID constants for translation - new PIDConstants(12, 0, 0), + new PIDConstants(12, 0, 0.00 ), // PID constants for rotation - new PIDConstants(8, 0, 0)), + new PIDConstants(8, 0, 0.00)), config, // Assume the path needs to be flipped for Red vs Blue, this is normally the // case diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index d098c9f..33bb741 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -144,7 +144,7 @@ RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1 - wristOffset), RobotSide.Back, new CoralScoringPosition(25., -1., -8.3 - wristOffset)), Position.L4, Map.of( RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6 - wristOffset), - RobotSide.Back, new CoralScoringPosition(64, -1.4, -9.7 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(64, -1.45, -9.5 - wristOffset)), Position.Intake, Map.of( RobotSide.Front, new CoralScoringPosition(6., 1., -0.9 - wristOffset), RobotSide.Back, new CoralScoringPosition(0, 0, 0.3 - wristOffset))); diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 index e86806b..a985f86 160000 --- a/src/main/java/frc/team696 +++ b/src/main/java/frc/team696 @@ -1 +1 @@ -Subproject commit e86806b12610b19054a842c1333b7b87c656e8c5 +Subproject commit a985f86cfae3df59e09714ec1704636f6853e11a From 5c13233fd076e39cf3b18bea66de4da16edc45a1 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Wed, 26 Mar 2025 12:36:32 -0700 Subject: [PATCH 26/37] pre idaho refactor --- src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/Robot.java | 29 +++----- .../java/frc/robot/subsystems/Elevator.java | 6 +- src/main/java/frc/robot/subsystems/LED.java | 73 ------------------- .../java/frc/robot/subsystems/Swerve.java | 2 + src/main/java/frc/robot/util/GameInfo.java | 8 -- 6 files changed, 20 insertions(+), 108 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/LED.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 5f3d847..33e6d18 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 61; - public static final String GIT_SHA = "92aa4bd3481558cb9e53d7c23d4cd71c22a30be3"; - public static final String GIT_DATE = "2025-03-24 19:13:03 PDT"; + public static final int GIT_REVISION = 62; + public static final String GIT_SHA = "996be687d8b72f9bb672330a5cd759d71e115d13"; + public static final String GIT_DATE = "2025-03-25 21:02:19 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-25 20:37:38 PDT"; - public static final long BUILD_UNIX_TIME = 1742960258205L; + public static final String BUILD_DATE = "2025-03-26 12:33:45 PDT"; + public static final long BUILD_UNIX_TIME = 1743017625640L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8ac3232..2592060 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,12 +11,10 @@ import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathfindingCommand; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; @@ -39,7 +36,6 @@ import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; import frc.robot.subsystems.GroundCoral; -import frc.robot.subsystems.LED; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -82,7 +78,7 @@ public void putSwerveSysIDCalibrationButtons() { SmartDashboard.putData("CTRESwerveCalibration/QuasistaticForward", Swerve.get().sysIdQuasistatic(SysIdRoutine.Direction.kForward)); SmartDashboard.putData("CTRESwerveCalibration/QuasistaticReverse", - Swerve.get().sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + Swerve.get().sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } private final SendableChooser autoChooser; @@ -93,7 +89,7 @@ public Robot() { Elevator.get(); EndEffector.get(); GroundCoral.get(); - //LED.get(); + // LED.get(); Swerve.get(); Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); @@ -129,14 +125,16 @@ public Robot() { GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); - NamedCommands.registerCommand("AfterIntake", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1).asProxy()); + NamedCommands.registerCommand("AfterIntake", + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, + false, 0.1).asProxy()); NamedCommands.registerCommand("Barge", new MoveSuperStructure(GameInfo.Net, 1.).asProxy()); NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); // Warmup Commands for PathPlanner - PathfindingCommand.warmupCommand().schedule(); + // PathfindingCommand.warmupCommand().schedule(); Elevator.get().setDefaultCommand(Elevator.get().positionCommand(() -> { if (!HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean()) { @@ -220,24 +218,19 @@ private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.SouceCoral.whileTrue((new MoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.6, false, 0.1)) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceSource, Swerve.get()::FaceHexFace))); - HumanControls.OperatorPanel2025.SouceCoral.onFalse(new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1)); - //HumanControls.OperatorPanel2025.Climb1.whileTrue(new MoveSuperStructure(GameInfo.ClimbUp, 0)); + HumanControls.OperatorPanel2025.SouceCoral.onFalse(new MoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1)); HumanControls.OperatorPanel2025.Climb1.whileTrue(new PIDtoNearest(false)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceProcessor, Swerve.get()::FaceSource))); - HumanControls.OperatorPanel2025.releaseCoral.and(HumanControls.OperatorPanel2025.pickupAlgae).whileTrue(new PrintCommand("trebuchet")); - // HumanControls.OperatorPanel2025.pickupAlgae.whileTrue(new - // MoveSuperStructure(GameInfo.ground, -0.8, false, -0.8)); + /*HumanControls.OperatorPanel2025.releaseCoral.and(HumanControls.OperatorPanel2025.pickupAlgae) + .whileTrue(new PrintCommand("trebuchet"));*/ + } @Override public void robotPeriodic() { - long start = RobotController.getTime(); CommandScheduler.getInstance().run(); - long elapsed = RobotController.getTime() - start; - // BackupLogger.addToQueue("SchedulerTimeMicroSeconds", elapsed); // Scheduler - // Time in Microseconds, anything over - // 20,000 should m_SwerveTelemetry.telemeterize(Swerve.get().getState()); BackupLogger.logSystemInformation(); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index c0ee7c0..315d021 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -4,7 +4,6 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Second; @@ -61,7 +60,6 @@ public void stop() { } /** - * * Use ONLY for SysID. Sets the elevator motors to a specific voltage */ public void DriveVoltage(Voltage v) { @@ -125,10 +123,10 @@ public void resetPosition(double newPosition) { @Override public void periodic() { - if(Math.abs(Swerve.get().getPigeon2().getRoll().getValue().in(Degrees))>15||Math.abs(Swerve.get().getPigeon2().getPitch().getValue().in(Degrees))>15){ + /*if(Math.abs(Swerve.get().getPigeon2().getRoll().getValue().in(Degrees))>15||Math.abs(Swerve.get().getPigeon2().getPitch().getValue().in(Degrees))>15){ if(this.getCurrentCommand()!=null) this.getCurrentCommand().cancel(); goToPosition(0); - } + }*/ } } diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java deleted file mode 100644 index ddc229c..0000000 --- a/src/main/java/frc/robot/subsystems/LED.java +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix.led.CANdle; -import com.ctre.phoenix.led.CANdle.LEDStripType; -import com.ctre.phoenix.led.CANdle.VBatOutputMode; -import com.ctre.phoenix.led.CANdleConfiguration; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class LED extends SubsystemBase { - private static LED m_LED = null; - private final int _ledOffset = 8; - private final int _numLed = 36 + 8; - - public static synchronized LED get() { - if (m_LED == null) { - m_LED = new LED(); - } - return m_LED; - } - - CANdle candle = new CANdle(0); - - public double getVbat() { - return candle.getBusVoltage(); - } - - public double get5V() { - return candle.get5VRailVoltage(); - } - - public double getCurrent() { - return candle.getCurrent(); - } - - public double getTemperature() { - return candle.getTemperature(); - } - - private void setColor(int r, int g, int b) { - candle.clearAnimation(0); - - candle.setLEDs(r, g, b, 255, _ledOffset, _numLed); - } - - private Command Color(int r, int g, int b) { - return this.startEnd(() -> setColor(r, g, b), () -> setColor(0, 0, 0)).ignoringDisable(true); - } - - /** Creates a new LED. */ - private LED() { - CANdleConfiguration _candleConfiguration = new CANdleConfiguration(); - _candleConfiguration.statusLedOffWhenActive = true; - _candleConfiguration.disableWhenLOS = false; - _candleConfiguration.stripType = LEDStripType.RGB; - _candleConfiguration.brightnessScalar = 1; - _candleConfiguration.vBatOutputMode = VBatOutputMode.On; - _candleConfiguration.enableOptimizations = true; - _candleConfiguration.v5Enabled = true; - candle.configAllSettings(_candleConfiguration); - this.setDefaultCommand(Color(255, 0, 0).ignoringDisable(true)); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f790537..df58b4b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -97,6 +97,7 @@ public static synchronized Swerve get() { * SysId routine for characterizing translation. This is used to find PID gains * for the drive motors. */ + @SuppressWarnings("unused") private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( null, // Use default ramp rate (1 V/s) @@ -113,6 +114,7 @@ public static synchronized Swerve get() { * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. */ + @SuppressWarnings("unused") private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( new SysIdRoutine.Config( /* This is in radians per secondĀ², but SysId only supports "volts per second" */ diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 33bb741..634792a 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -5,15 +5,10 @@ package frc.robot.util; import static edu.wpi.first.units.Units.Feet; -import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Rotation; import java.util.Map; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.IdealStartingState; -import com.pathplanner.lib.path.Waypoint; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -73,9 +68,6 @@ public static Translation2d mirrorTranslation(Translation2d starting) { return new Translation2d(17.55- starting.getX(), starting.getY()); } public static Translation2d mirrorTranslationXY(Translation2d starting) { - Waypoint temp = new Waypoint(starting, starting, starting); - temp.flip(); - //return temp.prevControl(); return new Translation2d(17.55- starting.getX(), 8.05-starting.getY()); } From 6efa130eaa6c95a15c48428baefeb9df9c7e8b3d Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Thu, 27 Mar 2025 16:44:53 -0700 Subject: [PATCH 27/37] Idaho: Practice Day --- .../pathplanner/autos/Copy of RightSide.auto | 139 ------------------ .../pathplanner/autos/Old RightSide.auto | 73 +++++++++ .../deploy/pathplanner/autos/RightSide.auto | 106 ++++++++++--- .../deploy/pathplanner/paths/New Path.path | 54 +++++++ src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 8 +- 8 files changed, 225 insertions(+), 171 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Copy of RightSide.auto create mode 100644 src/main/deploy/pathplanner/autos/Old RightSide.auto create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/autos/Copy of RightSide.auto b/src/main/deploy/pathplanner/autos/Copy of RightSide.auto deleted file mode 100644 index c29613b..0000000 --- a/src/main/deploy/pathplanner/autos/Copy of RightSide.auto +++ /dev/null @@ -1,139 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "First Test" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Second Test" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Third Test" - } - }, - { - "type": "named", - "data": { - "name": "AfterIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Fourth Test" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Fifth Test" - } - }, - { - "type": "named", - "data": { - "name": "AfterIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Old RightSide.auto b/src/main/deploy/pathplanner/autos/Old RightSide.auto new file mode 100644 index 0000000..decbbb1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Old RightSide.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "First Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Second Test" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Third Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Fourth Test" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Fifth Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightSide.auto b/src/main/deploy/pathplanner/autos/RightSide.auto index decbbb1..c29613b 100644 --- a/src/main/deploy/pathplanner/autos/RightSide.auto +++ b/src/main/deploy/pathplanner/autos/RightSide.auto @@ -17,21 +17,54 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "Second Test" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Second Test" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] } }, { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "Third Test" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Third Test" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] } }, { @@ -41,21 +74,54 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "Fourth Test" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fourth Test" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] } }, { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "Fifth Test" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fifth Test" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] } }, { diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..d400128 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3523116438356164, + "y": 7.135316780821918 + }, + "prevControl": null, + "nextControl": { + "x": 2.352311643835617, + "y": 7.135316780821918 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "MiddleAndNet", + "idealStartingState": { + "velocity": 0, + "rotation": -139.84400037508067 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index d007c5a..c891a95 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,10 +13,10 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, - "robotMass": 107.0, + "robotMass": 54.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.044, + "driveWheelRadius": 0.0406, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 33e6d18..0e88154 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 62; - public static final String GIT_SHA = "996be687d8b72f9bb672330a5cd759d71e115d13"; - public static final String GIT_DATE = "2025-03-25 21:02:19 PDT"; + public static final int GIT_REVISION = 63; + public static final String GIT_SHA = "5c13233fd076e39cf3b18bea66de4da16edc45a1"; + public static final String GIT_DATE = "2025-03-26 12:36:32 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-26 12:33:45 PDT"; - public static final long BUILD_UNIX_TIME = 1743017625640L; + public static final String BUILD_DATE = "2025-03-27 14:15:11 PDT"; + public static final long BUILD_UNIX_TIME = 1743110111033L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2592060..b0b2912 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -84,12 +84,12 @@ public void putSwerveSysIDCalibrationButtons() { private final SendableChooser autoChooser; public Robot() { + // TODO: strip out groundcoral system thetaController.enableContinuousInput(-180, 180); Arm.get(); Elevator.get(); EndEffector.get(); GroundCoral.get(); - // LED.get(); Swerve.get(); Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index df58b4b..8eb00cc 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -445,15 +445,15 @@ public Rotation2d FaceNet() { public Rotation2d FaceSource() { if (Util.getAlliance() == Alliance.Blue) { if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { - return Rotation2d.fromDegrees(37); + return Rotation2d.fromDegrees(36); } else { - return Rotation2d.fromDegrees(127); + return Rotation2d.fromDegrees(140); } } else { if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { - return Rotation2d.fromDegrees(-37); + return Rotation2d.fromDegrees(-36); } else { - return Rotation2d.fromDegrees(-127); + return Rotation2d.fromDegrees(-140); } } } From e147b8f333423b8f907193d8ef9d47859b5c9548 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Sat, 29 Mar 2025 07:36:40 -0700 Subject: [PATCH 28/37] Idaho: Qualifications Day --- .../deploy/pathplanner/paths/MiddleOne.path | 12 ++-- src/main/java/frc/robot/BuildConstants.java | 10 +-- .../java/frc/robot/subsystems/Climber.java | 71 ------------------- .../frc/robot/subsystems/GroundCoral.java | 36 +++++----- 4 files changed, 30 insertions(+), 99 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/Climber.java diff --git a/src/main/deploy/pathplanner/paths/MiddleOne.path b/src/main/deploy/pathplanner/paths/MiddleOne.path index 9d98c10..828b306 100644 --- a/src/main/deploy/pathplanner/paths/MiddleOne.path +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -4,24 +4,24 @@ { "anchor": { "x": 7.05, - "y": 3.875 + "y": 3.91 }, "prevControl": null, "nextControl": { "x": 6.389948058621888, - "y": 3.875 + "y": 3.91 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.675, - "y": 3.875 + "x": 5.68, + "y": 3.91 }, "prevControl": { - "x": 6.010450938895257, - "y": 3.875 + "x": 6.015450938895257, + "y": 3.91 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 0e88154..0a3c310 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 63; - public static final String GIT_SHA = "5c13233fd076e39cf3b18bea66de4da16edc45a1"; - public static final String GIT_DATE = "2025-03-26 12:36:32 PDT"; + public static final int GIT_REVISION = 64; + public static final String GIT_SHA = "6efa130eaa6c95a15c48428baefeb9df9c7e8b3d"; + public static final String GIT_DATE = "2025-03-27 16:44:53 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-27 14:15:11 PDT"; - public static final long BUILD_UNIX_TIME = 1743110111033L; + public static final String BUILD_DATE = "2025-03-28 16:18:41 PDT"; + public static final long BUILD_UNIX_TIME = 1743203921383L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java deleted file mode 100644 index 3c8dbc5..0000000 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.BotConstants; -import frc.team696.lib.Logging.BackupLogger; - -public class Climber extends SubsystemBase { - private static Climber m_Climber = null; - - public static synchronized Climber get() { - if (m_Climber == null) { - m_Climber = new Climber(); - } - return m_Climber; - } - - private TalonFX motor; - private MotionMagicVoltage positionReq; - - private Climber() { - motor = new TalonFX(BotConstants.Climber.motorID, BotConstants.canivoreBus); - positionReq = new MotionMagicVoltage(0); - zero(); - this.setDefaultCommand(In()); - } - - public void stop() { - motor.stopMotor(); - } - - public Command Out() { - return this.runEnd( - () -> { - motor.setControl(positionReq.withPosition(14)); - }, - () -> { - stop(); - }); - } - - public Command In() { - return this.runEnd( - () -> { - motor.setControl(positionReq.withPosition(0)); - }, - () -> { - stop(); - }); - } - - public void resetPosition(double newPosition) { - motor.setPosition(newPosition); - } - - public void zero() { - resetPosition(0); - } - - @Override - public void periodic() { - BackupLogger.addToQueue("climber/Postion", motor.getPosition().getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index 4bf2897..aaec143 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -41,23 +41,23 @@ public static synchronized final GroundCoral get() { return m_GroundCoral; } - TalonFX angleMotor = new TalonFX(BotConstants.GroundCoral.angleId, BotConstants.rioBus); - TalonFX rollerMotor = new TalonFX(BotConstants.GroundCoral.rollerId, BotConstants.rioBus); + /*TalonFX angleMotor = new TalonFX(BotConstants.GroundCoral.angleId, BotConstants.rioBus); + TalonFX rollerMotor = new TalonFX(BotConstants.GroundCoral.rollerId, BotConstants.rioBus);*/ MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0); private GroundCoral() { - angleMotor.getConfigurator().apply(BotConstants.GroundCoral.angleCfg); + /*angleMotor.getConfigurator().apply(BotConstants.GroundCoral.angleCfg); rollerMotor.getConfigurator().apply(BotConstants.GroundCoral.rollerCfg); zero(); this.setDefaultCommand(this.runEnd(() -> { angleMotor.setControl(positionRequest.withPosition(0)); }, () -> { angleMotor.stopMotor(); - })); + }));*/ } public void resetPosition(double newPosition) { - angleMotor.setPosition(newPosition); + //angleMotor.setPosition(newPosition); } public void zero() { @@ -65,30 +65,32 @@ public void zero() { } public void stop() { - angleMotor.stopMotor(); - rollerMotor.stopMotor(); + //angleMotor.stopMotor(); + //rollerMotor.stopMotor(); } public boolean isStalling() { - return rollerMotor.getStatorCurrent().getValue() - .in(Amp) > (BotConstants.GroundCoral.rollerCfg.CurrentLimits.StatorCurrentLimit - 20); + /*return rollerMotor.getStatorCurrent().getValue() + .in(Amp) > (BotConstants.GroundCoral.rollerCfg.CurrentLimits.StatorCurrentLimit - 20);*/ + return false; } public double getPosition() { - return angleMotor.getPosition().getValue().in(Rotation); + return 0; + //return angleMotor.getPosition().getValue().in(Rotation); } public void position(double position) { - angleMotor.setControl(positionRequest.withPosition(position)); + //angleMotor.setControl(positionRequest.withPosition(position)); } public Command Intake() { return this.startEnd(() -> { position(Positions.Intake.value); - rollerMotor.set(0.8); + //rollerMotor.set(0.8); }, () -> { - angleMotor.stopMotor(); - rollerMotor.stopMotor(); + //angleMotor.stopMotor(); + //rollerMotor.stopMotor(); }); } @@ -101,7 +103,7 @@ public Command Stowed() { } else { position(Positions.Ready.value); } - rollerMotor.stopMotor(); + //rollerMotor.stopMotor(); }, this::stop); } @@ -114,7 +116,7 @@ public Command Ready() { } else { position(Positions.Stowed.value); } - rollerMotor.set(0.45); + //rollerMotor.set(0.45); }, this::stop); } @@ -122,7 +124,7 @@ public Command Ready() { public Command Spit() { return this.runEnd(() -> { position(Positions.Spit.value); - rollerMotor.set(-0.2); + //rollerMotor.set(-0.2); }, this::stop); } From d8548ccf7b3547b4d8885bf5332e3a9a5f1e47a6 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Wed, 16 Apr 2025 16:48:59 -0700 Subject: [PATCH 29/37] middle autos work nicely --- .../deploy/pathplanner/autos/MiddleNet.auto | 31 +++++++++++++++++-- .../deploy/pathplanner/paths/backtoreef.path | 10 +++--- .../paths/{New Path.path => bargetoreef.path} | 22 ++++++------- src/main/deploy/pathplanner/paths/end.path | 10 +++--- .../deploy/pathplanner/paths/gotoreef.path | 4 +-- .../deploy/pathplanner/paths/middlemove.path | 4 +-- .../deploy/pathplanner/paths/middletonet.path | 14 ++++----- .../pathplanner/paths/thentosource.path | 8 ++--- .../deploy/pathplanner/paths/tosource2.path | 8 ++--- src/main/java/frc/robot/BuildConstants.java | 10 +++--- src/main/java/frc/robot/Robot.java | 6 ++-- src/main/java/frc/robot/util/GameInfo.java | 4 ++- 12 files changed, 80 insertions(+), 51 deletions(-) rename src/main/deploy/pathplanner/paths/{New Path.path => bargetoreef.path} (73%) diff --git a/src/main/deploy/pathplanner/autos/MiddleNet.auto b/src/main/deploy/pathplanner/autos/MiddleNet.auto index b82267e..d5c952b 100644 --- a/src/main/deploy/pathplanner/autos/MiddleNet.auto +++ b/src/main/deploy/pathplanner/autos/MiddleNet.auto @@ -25,13 +25,26 @@ { "type": "named", "data": { - "name": "L3Algae" + "name": "L2Algae" } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "middletonet" + "commands": [ + { + "type": "path", + "data": { + "pathName": "middletonet" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeUp" + } + } + ] } }, { @@ -39,6 +52,18 @@ "data": { "name": "Barge" } + }, + { + "type": "path", + "data": { + "pathName": "bargetoreef" + } + }, + { + "type": "named", + "data": { + "name": "L3Algae" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index ad7930e..4e94845 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.2471318493150685, - "y": 7.285573630136986 + "y": 7.24 }, "prevControl": null, "nextControl": { "x": 2.019691059841383, - "y": 6.687841339221339 + "y": 6.642267709084353 }, "isLocked": false, "linkedName": "leftsource" @@ -17,11 +17,11 @@ { "anchor": { "x": 3.8916523972602737, - "y": 5.076797945205479 + "y": 5.08 }, "prevControl": { "x": 3.5471458183129054, - "y": 5.762570971521269 + "y": 5.76577302631579 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.6880952380952379, + "waypointRelativePos": 0.5857142857142857, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/bargetoreef.path similarity index 73% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/bargetoreef.path index d400128..058d7b3 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/bargetoreef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3523116438356164, - "y": 7.135316780821918 + "x": 7.71, + "y": 5.5 }, "prevControl": null, "nextControl": { - "x": 2.352311643835617, - "y": 7.135316780821918 + "x": 6.542763157894736, + "y": 5.516365131578947 }, "isLocked": false, - "linkedName": null + "linkedName": "Barge" }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 5.13, + "y": 5.12 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 5.703026315789472, + "y": 5.505013157894736 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -29.999999999999996 }, "reversed": false, "folder": "MiddleAndNet", "idealStartingState": { "velocity": 0, - "rotation": -139.84400037508067 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index c2ae328..b23a054 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.2471318493150685, - "y": 7.285573630136986 + "y": 7.24 }, "prevControl": null, "nextControl": { "x": 1.933095665104542, - "y": 6.687841339221339 + "y": 6.642267709084353 }, "isLocked": false, "linkedName": "leftsource" @@ -17,11 +17,11 @@ { "anchor": { "x": 3.61, - "y": 4.96 + "y": 4.93 }, "prevControl": { "x": 2.8268914473684204, - "y": 6.064802631578947 + "y": 6.0348026315789465 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.5880952380952383, + "waypointRelativePos": 0.7761904761904762, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path index e9d3d17..5468753 100644 --- a/src/main/deploy/pathplanner/paths/gotoreef.path +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -17,11 +17,11 @@ { "anchor": { "x": 4.91, - "y": 5.21 + "y": 5.19 }, "prevControl": { "x": 5.512861842105263, - "y": 5.719555921052631 + "y": 5.699555921052632 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path index c326b26..23376c9 100644 --- a/src/main/deploy/pathplanner/paths/middlemove.path +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -17,11 +17,11 @@ { "anchor": { "x": 5.675, - "y": 4.27516447368421 + "y": 4.1 }, "prevControl": { "x": 5.675, - "y": 4.02516447368421 + "y": 3.8499999999999996 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path index 752b465..b876b87 100644 --- a/src/main/deploy/pathplanner/paths/middletonet.path +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -4,28 +4,28 @@ { "anchor": { "x": 5.675, - "y": 4.27516447368421 + "y": 4.1 }, "prevControl": null, "nextControl": { "x": 6.551198630136986, - "y": 5.251833994232156 + "y": 5.076669520547945 }, "isLocked": false, "linkedName": "FourBall" }, { "anchor": { - "x": 7.618022260273973, - "y": 6.6995719178082185 + "x": 7.71, + "y": 5.5 }, "prevControl": { - "x": 6.656378424657535, - "y": 6.158647260273973 + "x": 6.748356164383562, + "y": 4.959075342465755 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Barge" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path index 833321d..e21ba1e 100644 --- a/src/main/deploy/pathplanner/paths/thentosource.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -4,12 +4,12 @@ { "anchor": { "x": 4.91, - "y": 5.21 + "y": 5.19 }, "prevControl": null, "nextControl": { "x": 4.232166095890411, - "y": 6.053467465753425 + "y": 6.033467465753425 }, "isLocked": false, "linkedName": "ThreeRight" @@ -17,11 +17,11 @@ { "anchor": { "x": 1.2471318493150685, - "y": 7.285573630136986 + "y": 7.24 }, "prevControl": { "x": 2.7497003424657525, - "y": 6.143621575342465 + "y": 6.098047945205479 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/tosource2.path b/src/main/deploy/pathplanner/paths/tosource2.path index 96e000f..74a41c5 100644 --- a/src/main/deploy/pathplanner/paths/tosource2.path +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.8916523972602737, - "y": 5.076797945205479 + "y": 5.08 }, "prevControl": null, "nextControl": { "x": 3.012020547945206, - "y": 6.105393835616439 + "y": 6.108595890410959 }, "isLocked": false, "linkedName": "TwoLeft" @@ -17,11 +17,11 @@ { "anchor": { "x": 1.2471318493150685, - "y": 7.285573630136986 + "y": 7.24 }, "prevControl": { "x": 2.4642123287671236, - "y": 5.948287671232876 + "y": 5.902714041095891 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 0a3c310..2e05374 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 64; - public static final String GIT_SHA = "6efa130eaa6c95a15c48428baefeb9df9c7e8b3d"; - public static final String GIT_DATE = "2025-03-27 16:44:53 PDT"; + public static final int GIT_REVISION = 65; + public static final String GIT_SHA = "e147b8f333423b8f907193d8ef9d47859b5c9548"; + public static final String GIT_DATE = "2025-03-29 07:36:40 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-03-28 16:18:41 PDT"; - public static final long BUILD_UNIX_TIME = 1743203921383L; + public static final String BUILD_DATE = "2025-04-16 11:56:19 PDT"; + public static final long BUILD_UNIX_TIME = 1744829779746L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b0b2912..a9808e0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -128,8 +128,10 @@ public Robot() { NamedCommands.registerCommand("AfterIntake", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1).asProxy()); - NamedCommands.registerCommand("Barge", new MoveSuperStructure(GameInfo.Net, 1.).asProxy()); - NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.).asProxy()); + NamedCommands.registerCommand("Barge", new AutoMoveSuperStructure(GameInfo.Net, 1.,0).asProxy()); + NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.).until(()->EndEffector.get().isStalling()).asProxy()); + NamedCommands.registerCommand("L2Algae", new MoveSuperStructure(GameInfo.L2AlgaeLow, -0.8, false, -1.).withTimeout(2).asProxy()); + NamedCommands.registerCommand("AlgaeUp", new MoveSuperStructure(GameInfo.algaeUp, -0.3).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 634792a..5958c27 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -43,7 +43,7 @@ public CoralScoringPosition(double height, double armRot, double wristRot) { public Angle wristRot; } - public static CoralScoringPosition Net, ground, Processor, ClimbUp, ClimbDown, L2Algae, L3Algae; + public static CoralScoringPosition Net, ground, Processor, ClimbUp, ClimbDown, L2Algae, L2AlgaeLow, L3Algae, algaeUp; /* Looking at the Index Dead On */ public enum ReefSide { @@ -141,12 +141,14 @@ RobotSide.Back, new CoralScoringPosition(64, -1.45, -9.5 - wristOffset)), RobotSide.Front, new CoralScoringPosition(6., 1., -0.9 - wristOffset), RobotSide.Back, new CoralScoringPosition(0, 0, 0.3 - wristOffset))); L2Algae = new CoralScoringPosition(17., -3., 1. - wristOffset); + L2AlgaeLow = new CoralScoringPosition(8., -3., 1. - wristOffset); L3Algae = new CoralScoringPosition(38., -3., 1. - wristOffset); Net = new CoralScoringPosition(67., 0., 6.5 - wristOffset); ClimbUp = new CoralScoringPosition(27, 0, 0 - wristOffset); ClimbDown = new CoralScoringPosition(2, 0, 0 - wristOffset); ground = new CoralScoringPosition(5., -6.3, -.5 - wristOffset); Processor = new CoralScoringPosition(0, -5., 1. - wristOffset); + algaeUp=new CoralScoringPosition(0, 0, 3); } } From 3a12afc99bce98cc4047cd0b12f9d5dcd0a9bd03 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Wed, 23 Apr 2025 18:56:11 -0700 Subject: [PATCH 30/37] reduce max speed of the robot for smoother driving, change auto align to be based on position rather than last pressed button --- .../deploy/pathplanner/paths/New Path.path | 54 +++++++++++++++++++ src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/Robot.java | 42 +++++++++++---- .../java/frc/robot/subsystems/Swerve.java | 10 ++++ 4 files changed, 102 insertions(+), 14 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..8a30bb0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.835102739726027, + "y": 6.74464897260274 + }, + "prevControl": { + "x": 7.835102739726027, + "y": 6.74464897260274 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "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/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 2e05374..e62051b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 65; - public static final String GIT_SHA = "e147b8f333423b8f907193d8ef9d47859b5c9548"; - public static final String GIT_DATE = "2025-03-29 07:36:40 PDT"; + public static final int GIT_REVISION = 66; + public static final String GIT_SHA = "d8548ccf7b3547b4d8885bf5332e3a9a5f1e47a6"; + public static final String GIT_DATE = "2025-04-16 16:48:59 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-04-16 11:56:19 PDT"; - public static final long BUILD_UNIX_TIME = 1744829779746L; + public static final String BUILD_DATE = "2025-04-18 16:15:28 PDT"; + public static final long BUILD_UNIX_TIME = 1745018128762L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a9808e0..b80ab28 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,8 +39,8 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; - private double MaxSpeed = SwerveConstants.THEORETICAL_MAX_SPEED.in(MetersPerSecond);// aTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxRotationalRate = RotationsPerSecond.of(10).in(RadiansPerSecond); + private double MaxSpeed = 3;//SwerveConstants.THEORETICAL_MAX_SPEED.in(MetersPerSecond);// aTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxRotationalRate = RotationsPerSecond.of(/*10*/7).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); private ProfiledPIDController thetaController = new ProfiledPIDController(1. / 200., 0, 0., @@ -106,6 +106,26 @@ public Robot() { Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.09), 2) * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); + /* + * HumanControls.DriverPanel.OtherButton.whileTrue(Swerve.get().applyRequest( + * () -> Swerve.fcDriveReq.withVelocityX( + * Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), + * 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + * .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX. + * getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) + * + * .withRotationalRate( + * (thetaController.calculate(Swerve.get().getPose().getRotation().getDegrees(), + * Swerve.get().goalRotation.get().getDegrees())) + * MaxRotationalRate)) + * .alongWith( + * Commands.startEnd(() -> + * thetaController.reset(Swerve.get().getPose().getRotation().getDegrees()), () + * -> { + * }))); + */ HumanControls.DriverPanel.OtherButton.whileTrue(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.09), 2) @@ -115,12 +135,11 @@ public Robot() { .withRotationalRate( (thetaController.calculate(Swerve.get().getPose().getRotation().getDegrees(), - Swerve.get().goalRotation.get().getDegrees())) + Swerve.get().getGoalRotation().getDegrees())) * MaxRotationalRate)) .alongWith( Commands.startEnd(() -> thetaController.reset(Swerve.get().getPose().getRotation().getDegrees()), () -> { }))); - NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( @@ -128,9 +147,11 @@ public Robot() { NamedCommands.registerCommand("AfterIntake", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1).asProxy()); - NamedCommands.registerCommand("Barge", new AutoMoveSuperStructure(GameInfo.Net, 1.,0).asProxy()); - NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.).until(()->EndEffector.get().isStalling()).asProxy()); - NamedCommands.registerCommand("L2Algae", new MoveSuperStructure(GameInfo.L2AlgaeLow, -0.8, false, -1.).withTimeout(2).asProxy()); + NamedCommands.registerCommand("Barge", new AutoMoveSuperStructure(GameInfo.Net, 1., 0).asProxy()); + NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.) + .until(() -> EndEffector.get().isStalling()).asProxy()); + NamedCommands.registerCommand("L2Algae", + new MoveSuperStructure(GameInfo.L2AlgaeLow, -0.8, false, -1.).withTimeout(2).asProxy()); NamedCommands.registerCommand("AlgaeUp", new MoveSuperStructure(GameInfo.algaeUp, -0.3).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -225,8 +246,11 @@ private void configureDriverStationBinds() { HumanControls.OperatorPanel2025.Climb1.whileTrue(new PIDtoNearest(false)); HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6) .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceProcessor, Swerve.get()::FaceSource))); - /*HumanControls.OperatorPanel2025.releaseCoral.and(HumanControls.OperatorPanel2025.pickupAlgae) - .whileTrue(new PrintCommand("trebuchet"));*/ + /* + * HumanControls.OperatorPanel2025.releaseCoral.and(HumanControls. + * OperatorPanel2025.pickupAlgae) + * .whileTrue(new PrintCommand("trebuchet")); + */ } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 8eb00cc..5d9325f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -71,6 +71,16 @@ public Command setGoalRotation(Supplier during, Supplier } + public Rotation2d getGoalRotation(){ + Pose2d pose=getPose(); + if(distTo(GameInfo.blueReef)<2.3){ + return FaceHexFace(); + }if(pose.getX()>6&&pose.getX()<9.8){ + return FaceNet(); + } + return FaceSource(); + } + public void updateYawOffset() { yawOffset = getPose().getRotation().minus(getPigeon2().getRotation2d()); } From 72fbde03f4c7a1bb9391ec0513d89a68c4dfcc40 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Thu, 29 May 2025 19:58:42 -0700 Subject: [PATCH 31/37] Added more documentation, GroundCoral subsystem needs to be stripped out. --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../java/frc/robot/subsystems/EndEffector.java | 7 +++++++ .../java/frc/robot/subsystems/GroundCoral.java | 1 + src/main/java/frc/robot/subsystems/Swerve.java | 4 +++- src/main/java/frc/robot/util/GameInfo.java | 16 ++++++++++++++++ 5 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e62051b..2421748 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 66; - public static final String GIT_SHA = "d8548ccf7b3547b4d8885bf5332e3a9a5f1e47a6"; - public static final String GIT_DATE = "2025-04-16 16:48:59 PDT"; + public static final int GIT_REVISION = 67; + public static final String GIT_SHA = "3a12afc99bce98cc4047cd0b12f9d5dcd0a9bd03"; + public static final String GIT_DATE = "2025-04-23 18:56:11 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-04-18 16:15:28 PDT"; - public static final long BUILD_UNIX_TIME = 1745018128762L; + public static final String BUILD_DATE = "2025-05-29 19:58:25 PDT"; + public static final long BUILD_UNIX_TIME = 1748573905902L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java index 26b113b..eda5a47 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector.java @@ -81,6 +81,13 @@ public boolean isStalling() { return motor.getStatorCurrent().getValue().in(Amps) >= 75 && velocitySignal.getValue().in(RotationsPerSecond) < 5; } + /** + * spins the roller at a fraction of it's power power ? + * + * @param power [-1,1] A function returning the fraction of the power that the + * roller can exert at 12V. Polled every tick. + * @return the command + */ public Command spin(DoubleSupplier power) { return this.runEnd(() -> run(power.getAsDouble()), () -> motor.set(0)); } diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java index aaec143..cdf21c4 100644 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ b/src/main/java/frc/robot/subsystems/GroundCoral.java @@ -1,3 +1,4 @@ +// TODO: strip this entire thing out // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 5d9325f..9d58e7b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -73,7 +73,9 @@ public Command setGoalRotation(Supplier during, Supplier public Rotation2d getGoalRotation(){ Pose2d pose=getPose(); - if(distTo(GameInfo.blueReef)<2.3){ + // the second condition converts the blue reef pos to a red reef pos before checking if the robot is close enough + if(distTo(GameInfo.blueReef)<2.3||distTo((new Translation2d(GameInfo.fieldLengthMeters.in(Meters) - GameInfo.blueReef.getX(), + GameInfo.blueReef.getY())))<2.3){ return FaceHexFace(); }if(pose.getX()>6&&pose.getX()<9.8){ return FaceNet(); diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index 5958c27..c308895 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -64,13 +64,28 @@ public enum Index { public final static Distance fieldLengthMeters = Feet.of(57.53); public final static Distance fieldWidthMeters = Feet.of(26.75); + /** + * Mirrors a Translation2D across the midpoint of the field x-ais + * @param starting The Translation2d to mirror + * @return The mirrored translation (X-Axis) + */ public static Translation2d mirrorTranslation(Translation2d starting) { return new Translation2d(17.55- starting.getX(), starting.getY()); } + + /** + * Mirrors a Translation2D across the midpoint of the field x-ais and of the field y-axis + * @param starting The Translation2d to mirror + * @return The mirrored translation + */ public static Translation2d mirrorTranslationXY(Translation2d starting) { return new Translation2d(17.55- starting.getX(), 8.05-starting.getY()); } + /** + * Returns the relevant collection of scoring poses according to the alliance the robot is currently on + * @return The collection of scoring poses + */ public static Map> getScoringPoses(){ return (Util.getAlliance()==Alliance.Red)?ScoringPosesRed:ScoringPosesBlue; } @@ -93,6 +108,7 @@ public enum RobotSide { } public final static Map> RobotState; + public final static double wristOffset = 1.15; static { ScoringPosesBlue = Map.of( From b6ba8f915732127a8b7ebf1a431ab444ba89ccee Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Mon, 1 Sep 2025 10:03:53 -0700 Subject: [PATCH 32/37] delete the groundcoral subsystem and additional cleanup --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 85 +++++------ .../java/frc/robot/subsystems/Elevator.java | 5 +- .../frc/robot/subsystems/GroundCoral.java | 137 ------------------ 4 files changed, 50 insertions(+), 187 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/GroundCoral.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 2421748..4730f00 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 67; - public static final String GIT_SHA = "3a12afc99bce98cc4047cd0b12f9d5dcd0a9bd03"; - public static final String GIT_DATE = "2025-04-23 18:56:11 PDT"; + public static final int GIT_REVISION = 68; + public static final String GIT_SHA = "72fbde03f4c7a1bb9391ec0513d89a68c4dfcc40"; + public static final String GIT_DATE = "2025-05-29 19:58:42 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-05-29 19:58:25 PDT"; - public static final long BUILD_UNIX_TIME = 1748573905902L; + public static final String BUILD_DATE = "2025-06-06 18:55:52 PDT"; + public static final long BUILD_UNIX_TIME = 1749261352316L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b80ab28..def9c7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,8 +11,12 @@ import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; @@ -23,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; @@ -31,11 +36,11 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; import frc.robot.util.GameInfo; +import frc.robot.util.GameInfo.ReefSide; import frc.team696.lib.Logging.BackupLogger; import frc.team696.lib.Swerve.SwerveConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; -import frc.robot.subsystems.GroundCoral; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -83,13 +88,37 @@ public void putSwerveSysIDCalibrationButtons() { private final SendableChooser autoChooser; + private Command OldTimesAuto(){ + SequentialCommandGroup commandGroup=new SequentialCommandGroup(); + PathConstraints constraints=new PathConstraints(2,1, 15, 10); + for(var index:GameInfo.getScoringPoses().entrySet()){ + Pose2d goalPoseLeft=index.getValue().get(GameInfo.ReefSide.Left); + Pose2d goalPoseRight=index.getValue().get(GameInfo.ReefSide.Right); + + commandGroup.addCommands( + AutoBuilder.pathfindToPose(goalPoseLeft, constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy(), + AutoBuilder.pathfindToPose(new Pose2d(1.397, 7.55, Rotation2d.fromDegrees(35)), constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy(), + AutoBuilder.pathfindToPose(goalPoseRight, constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy(), + AutoBuilder.pathfindToPose(new Pose2d(1.397, 7.55, Rotation2d.fromDegrees(35)), constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy() + ); + } + return commandGroup; + } + public Robot() { // TODO: strip out groundcoral system thetaController.enableContinuousInput(-180, 180); Arm.get(); Elevator.get(); EndEffector.get(); - GroundCoral.get(); Swerve.get(); Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); @@ -156,59 +185,33 @@ public Robot() { autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); + SmartDashboard.putData("L1",new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("L2", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("L3",new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("L4",new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("Old times Auto", OldTimesAuto()); + // Warmup Commands for PathPlanner // PathfindingCommand.warmupCommand().schedule(); Elevator.get().setDefaultCommand(Elevator.get().positionCommand(() -> { - if (!HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean()) { - if (GroundCoral.get().getPosition() > 3.) { - return 25; - } else { - return 0; - } - } else { - if (GroundCoral.get().getPosition() < 5.) { - return 25.; - } else { - return 0; - } - } + return 0; })); Wrist.get().setDefaultCommand(Wrist.get().Position(-0.3)); Arm.get().setDefaultCommand(Arm.get().Position(() -> .7)); EndEffector.get().setDefaultCommand(EndEffector.get().spin(() -> EndEffector.get().idlePower)); - GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); } private void configureDriverStationBinds() { HumanControls.DriverPanel.resetGyro.whileTrue(new PIDtoNearest(false)); HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(() -> Swerve.get().seedFieldCentric())); HumanControls.OperatorPanel2025.releaseCoral.whileTrue( - new ConditionalCommand( - new InstantCommand(() -> { - EndEffector.get().idlePower = -0.6; - }), - GroundCoral.get().Spit(), - () -> !HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean())); - - HumanControls.OperatorPanel2025.unlabedSwitch.onFalse( - new InstantCommand(() -> { - if (GroundCoral.get().getCurrentCommand() != null) - GroundCoral.get().getCurrentCommand().cancel(); - GroundCoral.get().setDefaultCommand(GroundCoral.get().Stowed()); - }).ignoringDisable(true)); - HumanControls.OperatorPanel2025.unlabedSwitch.onTrue( - new InstantCommand(() -> { - if (GroundCoral.get().getCurrentCommand() != null) - GroundCoral.get().getCurrentCommand().cancel(); - Swerve.get().goalRotation = Swerve.get()::FaceHexFace; - GroundCoral.get().setDefaultCommand(GroundCoral.get().Ready()); - EndEffector.get().idlePower = 0; - }).ignoringDisable(true)); - - HumanControls.OperatorPanel2025.GroundCoral.whileTrue( - new ConditionalCommand(Commands.none(), GroundCoral.get().Intake(), - () -> !HumanControls.OperatorPanel2025.unlabedSwitch.getAsBoolean())); + new InstantCommand(() -> { + EndEffector.get().idlePower = -0.6; + })); + + + HumanControls.OperatorPanel2025.L1.whileTrue( new ConditionalCommand( new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 315d021..6b0beea 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -67,10 +67,7 @@ public void DriveVoltage(Voltage v) { } public void goToPosition(double position) { - if (GroundCoral.get().getPosition() > GroundCoral.Positions.Stowed.value + 2. - && GroundCoral.get().getPosition() < GroundCoral.Positions.Ready.value - 1) { - position = Math.max(position, 25); - } + master.get().setControl(positionReq.withPosition(position)); } diff --git a/src/main/java/frc/robot/subsystems/GroundCoral.java b/src/main/java/frc/robot/subsystems/GroundCoral.java deleted file mode 100644 index cdf21c4..0000000 --- a/src/main/java/frc/robot/subsystems/GroundCoral.java +++ /dev/null @@ -1,137 +0,0 @@ -// TODO: strip this entire thing out -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Amp; -import static edu.wpi.first.units.Units.Rotation; - -import com.ctre.phoenix6.controls.MotionMagicDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.BotConstants; - -/** - * represents the other coral system that picks up from ground and can score L1 - */ -public class GroundCoral extends SubsystemBase { - private static GroundCoral m_GroundCoral = null; - - public static enum Positions { - Stowed(0), - Ready(6.18), - Spit(6.18), - Intake(12.5); - - Positions(double value) { - this.value = value; - } - - double value = 0; - } - - public static synchronized final GroundCoral get() { - if (m_GroundCoral == null) { - m_GroundCoral = new GroundCoral(); - } - return m_GroundCoral; - } - - /*TalonFX angleMotor = new TalonFX(BotConstants.GroundCoral.angleId, BotConstants.rioBus); - TalonFX rollerMotor = new TalonFX(BotConstants.GroundCoral.rollerId, BotConstants.rioBus);*/ - MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0); - - private GroundCoral() { - /*angleMotor.getConfigurator().apply(BotConstants.GroundCoral.angleCfg); - rollerMotor.getConfigurator().apply(BotConstants.GroundCoral.rollerCfg); - zero(); - this.setDefaultCommand(this.runEnd(() -> { - angleMotor.setControl(positionRequest.withPosition(0)); - }, () -> { - angleMotor.stopMotor(); - }));*/ - } - - public void resetPosition(double newPosition) { - //angleMotor.setPosition(newPosition); - } - - public void zero() { - resetPosition(0); - } - - public void stop() { - //angleMotor.stopMotor(); - //rollerMotor.stopMotor(); - } - - public boolean isStalling() { - /*return rollerMotor.getStatorCurrent().getValue() - .in(Amp) > (BotConstants.GroundCoral.rollerCfg.CurrentLimits.StatorCurrentLimit - 20);*/ - return false; - } - - public double getPosition() { - return 0; - //return angleMotor.getPosition().getValue().in(Rotation); - } - - public void position(double position) { - //angleMotor.setControl(positionRequest.withPosition(position)); - } - - public Command Intake() { - return this.startEnd(() -> { - position(Positions.Intake.value); - //rollerMotor.set(0.8); - }, () -> { - //angleMotor.stopMotor(); - //rollerMotor.stopMotor(); - }); - } - - public Command Stowed() { - return this.runEnd( - () -> { - if (Elevator.get().getPosition() > 20 - || this.getPosition() < 4) { - position(Positions.Stowed.value); - } else { - position(Positions.Ready.value); - } - //rollerMotor.stopMotor(); - }, - this::stop); - } - - public Command Ready() { - return this.runEnd( - () -> { - if (Elevator.get().getPosition() > 20 || this.getPosition() > 6.) { - position(Positions.Ready.value); - } else { - position(Positions.Stowed.value); - } - //rollerMotor.set(0.45); - }, - this::stop); - } - - public Command Spit() { - return this.runEnd(() -> { - position(Positions.Spit.value); - //rollerMotor.set(-0.2); - }, this::stop); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("GroundCoral/Position", getPosition()); - // This method will be called once per scheduler run - } -} \ No newline at end of file From 2c6837c95df54501c39f00b329a0e177744c52a7 Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Fri, 26 Sep 2025 20:08:06 -0700 Subject: [PATCH 33/37] Update scoring positions and constants to reflect change in wrist gear ratio, not all positins changed, see todo --- src/main/java/frc/robot/BotConstants.java | 4 ++-- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/util/GameInfo.java | 16 ++++++++-------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index c81913b..610e39c 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -70,8 +70,8 @@ public static class Wrist { static { cfg.Slot0.kP = 32.; cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; - cfg.MotionMagic.MotionMagicAcceleration = 40.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; + cfg.MotionMagic.MotionMagicAcceleration = 80.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 40.; cfg.CurrentLimits.StatorCurrentLimitEnable = false; cfg.CurrentLimits.SupplyCurrentLimitEnable = false; cfg.CurrentLimits.StatorCurrentLimit = 120.; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 4730f00..38b7edb 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 68; - public static final String GIT_SHA = "72fbde03f4c7a1bb9391ec0513d89a68c4dfcc40"; - public static final String GIT_DATE = "2025-05-29 19:58:42 PDT"; + public static final int GIT_REVISION = 69; + public static final String GIT_SHA = "b6ba8f915732127a8b7ebf1a431ab444ba89ccee"; + public static final String GIT_DATE = "2025-09-01 10:03:53 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-06-06 18:55:52 PDT"; - public static final long BUILD_UNIX_TIME = 1749261352316L; + public static final String BUILD_DATE = "2025-09-26 19:50:00 PDT"; + public static final long BUILD_UNIX_TIME = 1758941400690L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index c308895..d6624df 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -143,27 +143,27 @@ ReefSide.Right, new Pose2d(4.09, 2.87, Rotation2d.fromDegrees(150)), RobotState = Map.of( Position.L1, Map.of( RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1 - wristOffset), - RobotSide.Back, new CoralScoringPosition(0, -1., -8. - wristOffset)), + RobotSide.Back, new CoralScoringPosition(0, -1., -20)), Position.L2, Map.of( RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56 - wristOffset), - RobotSide.Back, new CoralScoringPosition(3., -1., -8.3 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(3., -1., -20)), Position.L3, Map.of( RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1 - wristOffset), - RobotSide.Back, new CoralScoringPosition(25., -1., -8.3 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(25., -1., -20.5)), Position.L4, Map.of( RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6 - wristOffset), - RobotSide.Back, new CoralScoringPosition(64, -1.45, -9.5 - wristOffset)), + RobotSide.Back, new CoralScoringPosition(64, -1.45, -22.6)), Position.Intake, Map.of( - RobotSide.Front, new CoralScoringPosition(6., 1., -0.9 - wristOffset), + RobotSide.Front, new CoralScoringPosition(6., 1., -4.5), RobotSide.Back, new CoralScoringPosition(0, 0, 0.3 - wristOffset))); L2Algae = new CoralScoringPosition(17., -3., 1. - wristOffset); L2AlgaeLow = new CoralScoringPosition(8., -3., 1. - wristOffset); L3Algae = new CoralScoringPosition(38., -3., 1. - wristOffset); - Net = new CoralScoringPosition(67., 0., 6.5 - wristOffset); + Net = new CoralScoringPosition(67., 0., 10); ClimbUp = new CoralScoringPosition(27, 0, 0 - wristOffset); ClimbDown = new CoralScoringPosition(2, 0, 0 - wristOffset); - ground = new CoralScoringPosition(5., -6.3, -.5 - wristOffset); - Processor = new CoralScoringPosition(0, -5., 1. - wristOffset); + ground = new CoralScoringPosition(5., -6.3, -.5 - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION + Processor = new CoralScoringPosition(0, -5., 1. - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION algaeUp=new CoralScoringPosition(0, 0, 3); } From 6a31eb8e1fb82df01ab88bd7937a7c5a50413b5c Mon Sep 17 00:00:00 2001 From: MelonzBigBoss Date: Wed, 1 Oct 2025 13:52:17 -0700 Subject: [PATCH 34/37] Post Wednesday Changes missed one --- src/main/deploy/pathplanner/autos/LeftSide.auto | 2 +- src/main/deploy/pathplanner/paths/Fifth Test.path | 4 ++-- src/main/deploy/pathplanner/paths/First Test.path | 4 ++-- src/main/deploy/pathplanner/paths/Fourth Test.path | 6 +++--- src/main/deploy/pathplanner/paths/MiddleOne.path | 4 ++-- src/main/deploy/pathplanner/paths/New Path.path | 4 ++-- src/main/deploy/pathplanner/paths/Second Test.path | 4 ++-- src/main/deploy/pathplanner/paths/Third Test.path | 4 ++-- src/main/deploy/pathplanner/paths/backtoreef.path | 12 ++++++------ src/main/deploy/pathplanner/paths/bargetoreef.path | 4 ++-- src/main/deploy/pathplanner/paths/end.path | 4 ++-- src/main/deploy/pathplanner/paths/gotoreef.path | 4 ++-- src/main/deploy/pathplanner/paths/middlemove.path | 4 ++-- src/main/deploy/pathplanner/paths/middlestart.path | 4 ++-- src/main/deploy/pathplanner/paths/middletonet.path | 4 ++-- src/main/deploy/pathplanner/paths/thentosource.path | 4 ++-- src/main/deploy/pathplanner/paths/tosource2.path | 12 ++++++------ src/main/deploy/pathplanner/settings.json | 4 ++-- src/main/java/frc/robot/BotConstants.java | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 4 ++-- 21 files changed, 52 insertions(+), 52 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/LeftSide.auto b/src/main/deploy/pathplanner/autos/LeftSide.auto index 96da847..c225319 100644 --- a/src/main/deploy/pathplanner/autos/LeftSide.auto +++ b/src/main/deploy/pathplanner/autos/LeftSide.auto @@ -133,7 +133,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path index 0ac59d0..333b6d9 100644 --- a/src/main/deploy/pathplanner/paths/Fifth Test.path +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path index c97f4b6..5dda7c8 100644 --- a/src/main/deploy/pathplanner/paths/First Test.path +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Fourth Test.path b/src/main/deploy/pathplanner/paths/Fourth Test.path index 4718d06..2b49563 100644 --- a/src/main/deploy/pathplanner/paths/Fourth Test.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 3.08320090859812, - "y": 1.8546234090456204 + "y": 1.8546234090456206 }, "isLocked": false, "linkedName": "third" @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleOne.path b/src/main/deploy/pathplanner/paths/MiddleOne.path index 828b306..eb16b00 100644 --- a/src/main/deploy/pathplanner/paths/MiddleOne.path +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 8a30bb0..794be5c 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Second Test.path b/src/main/deploy/pathplanner/paths/Second Test.path index 94c84f2..7a0814a 100644 --- a/src/main/deploy/pathplanner/paths/Second Test.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path index 78378e1..5836d3d 100644 --- a/src/main/deploy/pathplanner/paths/Third Test.path +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index 4e94845..5a352e6 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.8916523972602737, - "y": 5.08 + "x": 3.955027760709269, + "y": 5.125649359199437 }, "prevControl": { - "x": 3.5471458183129054, - "y": 5.76577302631579 + "x": 3.610521181761901, + "y": 5.811422385515227 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/bargetoreef.path b/src/main/deploy/pathplanner/paths/bargetoreef.path index 058d7b3..015dfc2 100644 --- a/src/main/deploy/pathplanner/paths/bargetoreef.path +++ b/src/main/deploy/pathplanner/paths/bargetoreef.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index b23a054..cb237f1 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path index 5468753..506ebbf 100644 --- a/src/main/deploy/pathplanner/paths/gotoreef.path +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path index 23376c9..07e5870 100644 --- a/src/main/deploy/pathplanner/paths/middlemove.path +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middlestart.path b/src/main/deploy/pathplanner/paths/middlestart.path index 6b1f535..2507876 100644 --- a/src/main/deploy/pathplanner/paths/middlestart.path +++ b/src/main/deploy/pathplanner/paths/middlestart.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path index b876b87..2af9175 100644 --- a/src/main/deploy/pathplanner/paths/middletonet.path +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/thentosource.path b/src/main/deploy/pathplanner/paths/thentosource.path index e21ba1e..b983fc5 100644 --- a/src/main/deploy/pathplanner/paths/thentosource.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/tosource2.path b/src/main/deploy/pathplanner/paths/tosource2.path index 74a41c5..57b067a 100644 --- a/src/main/deploy/pathplanner/paths/tosource2.path +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.8916523972602737, - "y": 5.08 + "x": 3.955027760709269, + "y": 5.125649359199437 }, "prevControl": null, "nextControl": { - "x": 3.012020547945206, - "y": 6.108595890410959 + "x": 3.0753959113942013, + "y": 6.154245249610397 }, "isLocked": false, "linkedName": "TwoLeft" @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index c891a95..9e8028d 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,8 +8,8 @@ "RightSide" ], "autoFolders": [], - "defaultMaxVel": 4.5, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.0, + "defaultMaxAccel": 2.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 610e39c..a2c2a48 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -56,7 +56,7 @@ public static class Arm { cfg.Slot0.GravityType = GravityTypeValue.Arm_Cosine; cfg.Slot0.kP = 8.; cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 100.; + cfg.CurrentLimits.StatorCurrentLimit = 120.; cfg.MotionMagic.MotionMagicCruiseVelocity = 80.; cfg.MotionMagic.MotionMagicAcceleration = 65.; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 38b7edb..59c7acc 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 69; - public static final String GIT_SHA = "b6ba8f915732127a8b7ebf1a431ab444ba89ccee"; - public static final String GIT_DATE = "2025-09-01 10:03:53 PDT"; + public static final int GIT_REVISION = 70; + public static final String GIT_SHA = "2c6837c95df54501c39f00b329a0e177744c52a7"; + public static final String GIT_DATE = "2025-09-26 20:08:06 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-09-26 19:50:00 PDT"; - public static final long BUILD_UNIX_TIME = 1758941400690L; + public static final String BUILD_DATE = "2025-10-01 12:23:06 PDT"; + public static final long BUILD_UNIX_TIME = 1759346586116L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index def9c7b..80a6340 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -44,7 +44,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; - private double MaxSpeed = 3;//SwerveConstants.THEORETICAL_MAX_SPEED.in(MetersPerSecond);// aTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxSpeed = SwerveConstants.MAX_VELOCITY.in(MetersPerSecond);// aTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxRotationalRate = RotationsPerSecond.of(/*10*/7).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); @@ -174,7 +174,7 @@ public Robot() { NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); NamedCommands.registerCommand("AfterIntake", - new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), 0.15, false, 0.1).asProxy()); NamedCommands.registerCommand("Barge", new AutoMoveSuperStructure(GameInfo.Net, 1., 0).asProxy()); NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.) From 35eb49f4c59a322a36bed9578e58c02cd244ab9c Mon Sep 17 00:00:00 2001 From: Sami Alameddine Date: Fri, 3 Oct 2025 22:02:18 -0700 Subject: [PATCH 35/37] Friday - Auto updates, barge fixes --- src/main/deploy/pathplanner/paths/backtoreef.path | 12 ++++++------ src/main/deploy/pathplanner/paths/bargetoreef.path | 4 ++-- src/main/deploy/pathplanner/paths/middlemove.path | 8 ++++---- src/main/deploy/pathplanner/paths/middletonet.path | 8 ++++---- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/util/GameInfo.java | 6 +++--- 7 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path index 5a352e6..1c79460 100644 --- a/src/main/deploy/pathplanner/paths/backtoreef.path +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2471318493150685, + "x": 1.247, "y": 7.24 }, "prevControl": null, "nextControl": { - "x": 2.019691059841383, - "y": 6.642267709084353 + "x": 1.847315525552532, + "y": 6.762834260559234 }, "isLocked": false, "linkedName": "leftsource" @@ -20,8 +20,8 @@ "y": 5.125649359199437 }, "prevControl": { - "x": 3.610521181761901, - "y": 5.811422385515227 + "x": 3.5557756490487864, + "y": 5.4183140577166995 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.5857142857142857, + "waypointRelativePos": 0.5775401069518717, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/bargetoreef.path b/src/main/deploy/pathplanner/paths/bargetoreef.path index 015dfc2..5c9c465 100644 --- a/src/main/deploy/pathplanner/paths/bargetoreef.path +++ b/src/main/deploy/pathplanner/paths/bargetoreef.path @@ -17,11 +17,11 @@ { "anchor": { "x": 5.13, - "y": 5.12 + "y": 5.17 }, "prevControl": { "x": 5.703026315789472, - "y": 5.505013157894736 + "y": 5.555013157894736 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path index 07e5870..6d78d21 100644 --- a/src/main/deploy/pathplanner/paths/middlemove.path +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.675, - "y": 4.1 + "x": 5.81, + "y": 4.09 }, "prevControl": { - "x": 5.675, - "y": 3.8499999999999996 + "x": 5.81, + "y": 3.84 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path index 2af9175..9391de8 100644 --- a/src/main/deploy/pathplanner/paths/middletonet.path +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.675, - "y": 4.1 + "x": 5.8, + "y": 4.09 }, "prevControl": null, "nextControl": { - "x": 6.551198630136986, - "y": 5.076669520547945 + "x": 6.676198630136986, + "y": 5.066669520547945 }, "isLocked": false, "linkedName": "FourBall" diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 59c7acc..434f5cd 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 70; - public static final String GIT_SHA = "2c6837c95df54501c39f00b329a0e177744c52a7"; - public static final String GIT_DATE = "2025-09-26 20:08:06 PDT"; + public static final int GIT_REVISION = 71; + public static final String GIT_SHA = "6a31eb8e1fb82df01ab88bd7937a7c5a50413b5c"; + public static final String GIT_DATE = "2025-10-01 13:53:19 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-10-01 12:23:06 PDT"; - public static final long BUILD_UNIX_TIME = 1759346586116L; + public static final String BUILD_DATE = "2025-10-03 21:45:33 PDT"; + public static final long BUILD_UNIX_TIME = 1759553133948L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80a6340..b25bb8f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index d6624df..af79482 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -159,11 +159,11 @@ RobotSide.Front, new CoralScoringPosition(6., 1., -4.5), L2Algae = new CoralScoringPosition(17., -3., 1. - wristOffset); L2AlgaeLow = new CoralScoringPosition(8., -3., 1. - wristOffset); L3Algae = new CoralScoringPosition(38., -3., 1. - wristOffset); - Net = new CoralScoringPosition(67., 0., 10); + Net = new CoralScoringPosition(67., 0.4, 12); ClimbUp = new CoralScoringPosition(27, 0, 0 - wristOffset); ClimbDown = new CoralScoringPosition(2, 0, 0 - wristOffset); - ground = new CoralScoringPosition(5., -6.3, -.5 - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION - Processor = new CoralScoringPosition(0, -5., 1. - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION + ground = new CoralScoringPosition(5., -6.3, -2 - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION + Processor = new CoralScoringPosition(0, -5., 1.3 - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION algaeUp=new CoralScoringPosition(0, 0, 3); } From 23d68b0cc9b2b5f702ed325f4ee68414f572fd28 Mon Sep 17 00:00:00 2001 From: pro_ Date: Mon, 6 Oct 2025 18:41:54 -0700 Subject: [PATCH 36/37] Monday Changes, unsure of it works properly --- .vscode/launch.json | 12 +- build.gradle | 2 +- .../deploy/pathplanner/autos/LeftSide.auto | 2 +- src/main/deploy/pathplanner/paths/end.path | 24 +- src/main/java/frc/lib/Auto.java | 250 ++++ src/main/java/frc/lib/Camera/BaseCam.java | 91 ++ .../java/frc/lib/Camera/LimeLightCam.java | 93 ++ .../java/frc/lib/Camera/LimelightHelpers.java | 1268 +++++++++++++++++ .../java/frc/lib/Camera/PhotonVisionCam.java | 77 + .../frc/lib/Dashboards/ShuffleDashboard.java | 35 + .../frc/lib/Datatypes/InterpolatingTable.java | 39 + src/main/java/frc/lib/Field.java | 50 + .../lib/HardwareDevices/CANCoderFactory.java | 89 ++ .../lib/HardwareDevices/GyroInterface.java | 15 + .../lib/HardwareDevices/PigeonFactory.java | 143 ++ .../frc/lib/HardwareDevices/TalonFactory.java | 197 +++ .../java/frc/lib/Logging/AdvantageKitLog.java | 46 + .../java/frc/lib/Logging/BackupLogger.java | 161 +++ src/main/java/frc/lib/Logging/LogEntry.java | 192 +++ src/main/java/frc/lib/Logging/PLog.java | 91 ++ .../frc/lib/Swerve/Commands/TeleopSwerve.java | 165 +++ .../java/frc/lib/Swerve/SwerveConfigs.java | 107 ++ .../java/frc/lib/Swerve/SwerveConstants.java | 116 ++ .../java/frc/lib/Swerve/SwerveDriveState.java | 164 +++ .../frc/lib/Swerve/SwerveDriveSubsystem.java | 444 ++++++ .../java/frc/lib/Swerve/SwerveModule.java | 185 +++ src/main/java/frc/lib/Util.java | 208 +++ src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/team696 | 1 - 30 files changed, 4251 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc/lib/Auto.java create mode 100644 src/main/java/frc/lib/Camera/BaseCam.java create mode 100644 src/main/java/frc/lib/Camera/LimeLightCam.java create mode 100644 src/main/java/frc/lib/Camera/LimelightHelpers.java create mode 100644 src/main/java/frc/lib/Camera/PhotonVisionCam.java create mode 100644 src/main/java/frc/lib/Dashboards/ShuffleDashboard.java create mode 100644 src/main/java/frc/lib/Datatypes/InterpolatingTable.java create mode 100644 src/main/java/frc/lib/Field.java create mode 100644 src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java create mode 100644 src/main/java/frc/lib/HardwareDevices/GyroInterface.java create mode 100644 src/main/java/frc/lib/HardwareDevices/PigeonFactory.java create mode 100644 src/main/java/frc/lib/HardwareDevices/TalonFactory.java create mode 100644 src/main/java/frc/lib/Logging/AdvantageKitLog.java create mode 100644 src/main/java/frc/lib/Logging/BackupLogger.java create mode 100644 src/main/java/frc/lib/Logging/LogEntry.java create mode 100644 src/main/java/frc/lib/Logging/PLog.java create mode 100644 src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java create mode 100644 src/main/java/frc/lib/Swerve/SwerveConfigs.java create mode 100644 src/main/java/frc/lib/Swerve/SwerveConstants.java create mode 100644 src/main/java/frc/lib/Swerve/SwerveDriveState.java create mode 100644 src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java create mode 100644 src/main/java/frc/lib/Swerve/SwerveModule.java create mode 100644 src/main/java/frc/lib/Util.java delete mode 160000 src/main/java/frc/team696 diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..9591fdb 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "2025BuildSeason" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/build.gradle b/build.gradle index d6f1f22..66e0041 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.peterabeles.gversion" version "1.10" } diff --git a/src/main/deploy/pathplanner/autos/LeftSide.auto b/src/main/deploy/pathplanner/autos/LeftSide.auto index c225319..8acb4fc 100644 --- a/src/main/deploy/pathplanner/autos/LeftSide.auto +++ b/src/main/deploy/pathplanner/autos/LeftSide.auto @@ -127,7 +127,7 @@ { "type": "named", "data": { - "name": "L4" + "name": "L1" } } ] diff --git a/src/main/deploy/pathplanner/paths/end.path b/src/main/deploy/pathplanner/paths/end.path index cb237f1..fe8bb6f 100644 --- a/src/main/deploy/pathplanner/paths/end.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.61, - "y": 4.93 + "x": 3.37, + "y": 4.13 }, "prevControl": { - "x": 2.8268914473684204, - "y": 6.0348026315789465 + "x": 2.1965943877551024, + "y": 5.013832908163264 }, "nextControl": null, "isLocked": false, @@ -31,19 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.7761904761904762, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.5, @@ -54,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 29.999999999999996 + "rotation": 90.0 }, "reversed": false, "folder": "LeftSide", diff --git a/src/main/java/frc/lib/Auto.java b/src/main/java/frc/lib/Auto.java new file mode 100644 index 0000000..a1f78f8 --- /dev/null +++ b/src/main/java/frc/lib/Auto.java @@ -0,0 +1,250 @@ +package frc.team696.lib; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.ModuleConfig; +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.path.PathPlannerPath; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.team696.lib.Dashboards.ShuffleDashboard; +import frc.team696.lib.Logging.BackupLogger; +import frc.team696.lib.Logging.PLog; +import frc.team696.lib.Swerve.SwerveConfigs; +import frc.team696.lib.Swerve.SwerveConstants; +import frc.team696.lib.Swerve.SwerveDriveSubsystem; +import frc.team696.lib.Swerve.SwerveModule; + +/** + * Houses all methods related to the Autonomous period and self driving during teleop + */ +@SuppressWarnings("resource") +public class Auto { + /** + * Wrapper For Pathplanners NamedCommand System + */ + public static class NamedCommand { + public String name; + public Command command; + + /** + * @param name to be used inside of the pathplanner GUI + * @param command to be run when called inside pathplanner + */ + public NamedCommand(String name, Command command) { + this.name = name; + this.command = command; + } + + public void register() { + NamedCommands.registerCommand(name, command); + } + } + + public static Auto _instance; + private SwerveDriveSubsystem _swerve; + + private SendableChooser _autoChooser; + + public SysIdRoutine _driveSysIdRoutine; + + private Auto (SwerveDriveSubsystem swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister) { + _swerve = swerve; + + RobotConfig config = new RobotConfig( + SwerveConstants.MASS, + SwerveConstants.MOMENT_OF_INERTIA, + new ModuleConfig( + SwerveConstants.WHEEL_RADIUS, + SwerveConstants.MAX_VELOCITY, + SwerveConstants.WHEEL_COEFFICIENT_OF_FRICTION, + DCMotor.getKrakenX60(1), + Amps.of(SwerveConfigs.drive.CurrentLimits.StatorCurrentLimit), + 1), + SwerveConstants.modPositions); + if (shouldUseGUIValues) { + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + PLog.fatalException("Auto", "Failed To Get RobotConfig", e); + new Alert("Failed to fetch robotConfig GUI Settings", AlertType.kWarning).set(true); + } + } + + AutoBuilder.configure( + _swerve::getPose, + _swerve::resetPose, + _swerve::getRobotRelativeSpeeds, + (speeds, feedforwards) -> _swerve.Drive(speeds), + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), + config, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + _swerve + ); + + for (NamedCommand namedCommand : commandsToRegister) { + namedCommand.register(); + } + + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + BackupLogger.addToQueue("696/Auto/Desired", pose); + }); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + ShuffleDashboard.field.getObject("Path").setPoses(poses); + }); + + _autoChooser = AutoBuilder.buildAutoChooser(); + ShuffleDashboard.addAutoChooser(_autoChooser); + + _autoChooser.onChange((command)-> { + visualize(); + }); + + /* + * Default SysIdRoutine + * + * Copy in your subsystem to add more. + */ + _driveSysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config( + Volts.per(Second).of(1), + Volts.of(3), + Seconds.of(10)), + new SysIdRoutine.Mechanism(_swerve::voltageDriveForward, log -> { + for (int i = 0; i < SwerveModule.s_moduleCount; i++) { + log.motor(SwerveModule.moduleNames[i]) + .voltage(_swerve.getModules()[i].getDriveOutputVoltage()) + .linearPosition(Meters.of(SwerveConstants.DISTANCE_PER_ROTATION.timesDivisor(_swerve.getModules()[i].getDriveMotorPosition()).magnitude())) + .linearVelocity(MetersPerSecond.of(SwerveConstants.DISTANCE_PER_ROTATION.times(_swerve.getModules()[i].getState().speedMetersPerSecond).magnitude())); + } + }, _swerve)); + } + + public static Supplier generateSysIDAutos( SysIdRoutine routine ) { + SendableChooser _sysIdChooser = new SendableChooser<>(); + _sysIdChooser.setDefaultOption("None", Commands.none()); + + _sysIdChooser.addOption("Quasistatic Forwards", routine.quasistatic(SysIdRoutine.Direction.kForward)); + _sysIdChooser.addOption("Quasistatic Backwards", routine.quasistatic(SysIdRoutine.Direction.kReverse)); + + _sysIdChooser.addOption("Dynamic Forwards", routine.dynamic(SysIdRoutine.Direction.kForward)); + _sysIdChooser.addOption("Dynamic Backwards", routine.dynamic(SysIdRoutine.Direction.kReverse)); + + SmartDashboard.putData("SysID Test Chooser",_sysIdChooser); + + return _sysIdChooser::getSelected; + } + + /** + * Initializes all things related to auto (ex. pathplanner) + * + * @param swerve The swerve subsystem + * @param commandsToRegister each command to register for path planner + */ + public static void Initialize(SwerveDriveSubsystem swerve, NamedCommand... commandsToRegister){ + if (_instance != null) throw new RuntimeException ("Don't Initialize Twice!"); + + _instance = new Auto(swerve, false, commandsToRegister); + } + + /** + * Initializes all things related to auto (ex. pathplanner) + * + * @param swerve The swerve subsystem + * @param shouldUseGUIValues Should Pathplanner fetch Robot Config from GUI? + * @param commandsToRegister each command to register for path planner + */ + public static void Initialize(SwerveDriveSubsystem swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister){ + if (_instance != null) throw new RuntimeException ("Don't Initialize Twice!"); + + _instance = new Auto(swerve, shouldUseGUIValues, commandsToRegister); + } + + /** + * @return The instance Of the Auto Class + */ + public static Auto get(){ + if (_instance == null) throw new RuntimeException ("Please Initialize First!"); + + return _instance; + } + + /** + * @return Selected Command From SendableChooser + */ + public Command Selected() { + return _autoChooser.getSelected(); + } + + /** + * @return Selected Command From SendableChooser Which Will End After 15 Seconds. + * Used for testing on home field + */ + public Command SelectedEndAt15() { + return Selected().raceWith(new WaitCommand(15.0)); + } + + public static Command PathFind(Pose2d end) { + return AutoBuilder.pathfindToPose(end, new PathConstraints(1, 1, Math.PI,Math.PI)); + } + + public void visualize(String name) { + List paths; + List pathPoses = new ArrayList(); + try { + paths = PathPlannerAuto.getPathGroupFromAutoFile(name); + } catch (Exception e) { + PLog.fatalException("Auto", "Failed To Find Path", e); + return; + } + for (int i = 0; i < paths.size(); i++) { + PathPlannerPath path = paths.get(i); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + path = path.flipPath(); + pathPoses.addAll(path.getPathPoses()); + } + ShuffleDashboard.field.getObject("traj").setPoses(pathPoses); + } + + public void visualize() { + visualize(_autoChooser.getSelected().getName()); + } + + +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Camera/BaseCam.java b/src/main/java/frc/lib/Camera/BaseCam.java new file mode 100644 index 0000000..4239e5e --- /dev/null +++ b/src/main/java/frc/lib/Camera/BaseCam.java @@ -0,0 +1,91 @@ +package frc.team696.lib.Camera; + +import java.util.Optional; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N3; +import frc.team696.lib.Logging.BackupLogger; +import frc.team696.lib.Logging.PLog; + +/** + * Base Camera Object for handling how cameras will generally be handled + * + *

+ * Extend to add functionality for a specific camera + * + * @see LimeLightCam.java + * @see PhotonVisionCam.java + */ +public abstract class BaseCam { + public class AprilTagResult { + public Pose2d pose; + /* + * Time Should Be In Current Time, Same as Phoenix6 Swerve + */ + public double time; + + public double distToTag; + public int tagCount; + + public double ambiguity; + + public AprilTagResult(Pose2d pose, double time, double distToTag, int tagCount, double ambiguity) { + this.pose = pose; + this.time = time; + this.distToTag = distToTag; + this.tagCount = tagCount; + this.ambiguity = ambiguity; + } + } + + public abstract Optional getEstimate(); + + Vector stdDeviations = VecBuilder.fill(0.7, 0.7, 2); + + public void setStdDeviations(double x, double y, double r) { + stdDeviations = VecBuilder.fill(x, y, r); + } + + @FunctionalInterface + public static interface addVisionEstimate { + void accept(Pose2d p, double d, Vector v); + } + + @FunctionalInterface + public static interface acceptEstimate { + boolean test(AprilTagResult latestResult); + } + + // eventually switch this to taking in a addVisionEstimate + public boolean addVisionEstimate(addVisionEstimate addVisionMeasurement, acceptEstimate checkEstimation) { + Optional oEstimation = this.getEstimate(); + + if (oEstimation.isPresent()) { + AprilTagResult estimation = oEstimation.get(); + try { + if (!checkEstimation.test(estimation)) { + BackupLogger.addToQueue("696/Vision/Rejected Pose", estimation.pose); + return false; + } else { + BackupLogger.addToQueue("696/Vision/Accepted Pose", estimation.pose); + } + } catch (Exception e) { + PLog.fatalException("Camera", e.getMessage(), e); + } + addVisionMeasurement.accept( + estimation.pose, + estimation.time, + stdDeviations); + return true; + } + return false; + } + + public synchronized boolean addVisionEstimate(addVisionEstimate addVisionMeasurement) { + return addVisionEstimate(addVisionMeasurement, (latestResult) -> { + return true; + }); + } +} diff --git a/src/main/java/frc/lib/Camera/LimeLightCam.java b/src/main/java/frc/lib/Camera/LimeLightCam.java new file mode 100644 index 0000000..64ffa87 --- /dev/null +++ b/src/main/java/frc/lib/Camera/LimeLightCam.java @@ -0,0 +1,93 @@ +package frc.team696.lib.Camera; + +import java.util.Optional; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * LimeLight Camera Interface + */ +public class LimeLightCam extends BaseCam { + public String name = ""; + public static int LimeLightCount = 0; + + private NetworkTable _ntTable; + + private boolean useMegaTag2 = false; + + public LimeLightCam(String name, int[] TagsToCheck, boolean useMegaTag2) { + this.name = name; + + if (TagsToCheck.length > 0) { + LimelightHelpers.SetFiducialIDFiltersOverride(name, TagsToCheck); + } + + for (int port = 5800; port <= 5809; port++) { + PortForwarder.add(port + 10 * LimeLightCount, String.format("%s.local", this.name), port); + } + + _ntTable = NetworkTableInstance.getDefault().getTable(name); + + _ntTable.getEntry("ledMode").setNumber(1); // Example of how to use -> This turns off LEDS on the front + + this.useMegaTag2 = useMegaTag2; + + LimeLightCount++; + } + + public LimeLightCam(String name) { + this(name, new int[] {}, false); + } + + public LimeLightCam(String name, boolean useMegaTag2) { + this(name, new int[] {}, useMegaTag2); + } + + public int targetCount() { + double[] t2d = _ntTable.getEntry("t2d").getDoubleArray(new double[0]); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + public boolean hasTargets() { + return targetCount() > 0; + } + + public double tX() { + return -1 * _ntTable.getEntry("tx").getDouble(0); + } + + public void SetRobotOrientation(Rotation2d curYaw) { + LimelightHelpers.SetRobotOrientation(name, curYaw.getDegrees(), 0, 0, 0, 0, 0); + } + + public Optional getEstimate() { + LimelightHelpers.PoseEstimate latestEstimate; + if (!useMegaTag2 || DriverStation.isDisabled()) { + latestEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); + } else { + latestEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + } + + if (latestEstimate == null) + return Optional.empty(); + + if (latestEstimate.tagCount == 0) + return Optional.empty(); + + return Optional.of( + new AprilTagResult(latestEstimate.pose, + Utils.fpgaToCurrentTime(latestEstimate.timestampSeconds), + latestEstimate.avgTagDist, + latestEstimate.tagCount, + latestEstimate.rawFiducials[0].ambiguity)); // Probably not the best but good enough for now + } +} diff --git a/src/main/java/frc/lib/Camera/LimelightHelpers.java b/src/main/java/frc/lib/Camera/LimelightHelpers.java new file mode 100644 index 0000000..dd65ec5 --- /dev/null +++ b/src/main/java/frc/lib/Camera/LimelightHelpers.java @@ -0,0 +1,1268 @@ +//LimelightHelpers v1.9 (REQUIRES 2024.9.1) + +package frc.team696.lib.Camera; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * Default Library Supplied By LimeLight Themselves + * + *

Potentially will be removed in the future. + */ +@SuppressWarnings("unused") +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + /** + * Makes a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles. + * + * @param pose The Pose3d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles. + * + * @param pose The Pose2d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Camera/PhotonVisionCam.java b/src/main/java/frc/lib/Camera/PhotonVisionCam.java new file mode 100644 index 0000000..d794f96 --- /dev/null +++ b/src/main/java/frc/lib/Camera/PhotonVisionCam.java @@ -0,0 +1,77 @@ +package frc.team696.lib.Camera; +/* +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.net.PortForwarder; +import frc.team696.lib.Logging.PLog; + +/** + * PhotonVision Camera Interface + * */ +/* +public class PhotonVisionCam extends BaseCam { + + public String _name; + private Transform3d _position; + + private PhotonCamera _camera; + private PhotonPoseEstimator _estimator; + + public static AprilTagFieldLayout _aprilTagFieldLayout; + + public PhotonVisionCam(String name, Transform3d position) { + this._name = name; + this._position = position; + + _camera = new PhotonCamera(_name); + _estimator = new PhotonPoseEstimator( + _aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + _camera, + _position + ); + _estimator.setMultiTagFallbackStrategy( + PoseStrategy.LOWEST_AMBIGUITY + ); + } + + public Optional getEstimate() { + Optional oLatestEstimate = _estimator.update(); + + if (oLatestEstimate.isEmpty()) return Optional.empty(); + + EstimatedRobotPose latestEstimate = oLatestEstimate.get(); + + return Optional.of( + new AprilTagResult(latestEstimate.estimatedPose.toPose2d(), + latestEstimate.timestampSeconds, + latestEstimate.targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm(), + latestEstimate.targetsUsed.size(), + latestEstimate.targetsUsed.get(0).getPoseAmbiguity())); + } + + static { + try { + _aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + } catch (Exception e) { + PLog.fatalException( + "PhotonVisionCam", + "Failed to load april tag layout.", + e + ); + } + + PortForwarder.add(5800, "photonvision.local", 5800); + + PhotonCamera.setVersionCheckEnabled(false); + } +} +*/ \ No newline at end of file diff --git a/src/main/java/frc/lib/Dashboards/ShuffleDashboard.java b/src/main/java/frc/lib/Dashboards/ShuffleDashboard.java new file mode 100644 index 0000000..1d0d6c8 --- /dev/null +++ b/src/main/java/frc/lib/Dashboards/ShuffleDashboard.java @@ -0,0 +1,35 @@ +package frc.team696.lib.Dashboards; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; + +public class ShuffleDashboard { + private static ShuffleboardTab _mainTab; + + public static final Field2d field = new Field2d(); + + static { + _mainTab = Shuffleboard.getTab("Telemetry"); + + _mainTab.add(field).withPosition(0, 0).withSize(7, 6); + } + + public static ComplexWidget addObject(Sendable object) { + return _mainTab.add(object); + } + + public static ComplexWidget addObject(String name, Sendable object) { + return _mainTab.add(name, object); + } + + public static ShuffleboardTab Tab() { + return _mainTab; + } + + public static void addAutoChooser(Sendable chooser) { + _mainTab.add("Autos", chooser).withPosition(7, 0).withSize(2,1); + } +} diff --git a/src/main/java/frc/lib/Datatypes/InterpolatingTable.java b/src/main/java/frc/lib/Datatypes/InterpolatingTable.java new file mode 100644 index 0000000..942532d --- /dev/null +++ b/src/main/java/frc/lib/Datatypes/InterpolatingTable.java @@ -0,0 +1,39 @@ +package frc.team696.lib.Datatypes; + +import java.util.Map; +import java.util.TreeMap; + +import edu.wpi.first.math.interpolation.Interpolatable; +import frc.team696.lib.Util; + +/** + * Class For creating a table to interpolate between values automatically + * + *

To use must create your custom class that implements Interpolatable to handle how the table will interpolate between nearest datapoints. + */ +public class InterpolatingTable> { + public final TreeMap map = new TreeMap(); + + @SafeVarargs + public InterpolatingTable(Map.Entry... entries) { + for (Map.Entry entry : entries) { + map.putIfAbsent(entry.getKey(), entry.getValue()); + } + } + + /** + * + * @param key Gets the value from the map at this point + * @return Interpolated Value + */ + public T getValue(double key) { + double index = Util.clamp(key, map.firstKey() + 0.0000000001, map.lastKey() - 0.0000000001); + Map.Entry lower = map.floorEntry(index); + Map.Entry higher = map.ceilingEntry(index); + + double t = (index - lower.getKey()) / (higher.getKey() - lower.getKey()); + + return lower.getValue().interpolate(higher.getValue(), t); + } + +} diff --git a/src/main/java/frc/lib/Field.java b/src/main/java/frc/lib/Field.java new file mode 100644 index 0000000..4f9037a --- /dev/null +++ b/src/main/java/frc/lib/Field.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** + * Add Any Necessary Field Elements Below + * + * To Be Updated Each Year + */ +public class Field { + public static abstract class FieldSide { + public abstract Pose2d FieldElement(); + } + + public static final class RED extends FieldSide { + @Override + public final Pose2d FieldElement() { + return new Pose2d(); + } + } + public static final class BLUE extends FieldSide { + @Override + public final Pose2d FieldElement() { + return new Pose2d(); + } + } + + public static final RED redInstance = new RED(); + public static final BLUE blueInstance = new BLUE(); + + public static final FieldSide getSide() { + if (Util.getAlliance() == Alliance.Blue) { + return blueInstance; + } + return redInstance; + } + + public static final FieldSide getOppositeSide() { + if (Util.getAlliance() == Alliance.Blue) { + return redInstance; + } + return blueInstance; + } + +} diff --git a/src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java b/src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java new file mode 100644 index 0000000..bbbae81 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java @@ -0,0 +1,89 @@ +package frc.team696.lib.HardwareDevices; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Timer; +import frc.team696.lib.Logging.PLog; + +/** + * Class For creating and keeping track of cancoder device + * + *

Automatically logs name of the device if it fails to config + * + *

Resets device parameters if it drops out + */ +public class CANCoderFactory { + private final double TIMEOUT = 0.05; + private CANcoder _encoder; + private CANcoderConfiguration _config; + private String _name; + + private boolean _configured = false; + private double _lastConfiguration = -100; + + private Alert configurationAlert; + + public CANCoderFactory(int id, String canBus, CANcoderConfiguration config, String name) { + this._encoder = new CANcoder(id, canBus); + this._name = name; + this._config = config; + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public CANCoderFactory(int id, CANcoderConfiguration config, String name) { + this(id, "rio", config, name); + } + + private boolean configure() { + return configure(false); + } + + public boolean configure(boolean force) { + if (!force && _configured) return true; + if (!force && Timer.getFPGATimestamp() - _lastConfiguration < 3) return false; + + _lastConfiguration = Timer.getFPGATimestamp(); + StatusCode configCode = _encoder.getConfigurator().apply(this._config, TIMEOUT); + + if(configCode.isError()) { + PLog.unusual(_name, "Failed to configure"); + } else { + PLog.info(_name, "Configured"); + if (configCode.isWarning()) { + PLog.unusual(_name, "Config Warning: " + configCode.toString()); + } + + _configured = true; + } + + configurationAlert.set(!_configured); + + return _configured; + } + + + public Rotation2d getPosition() { + if (configure()) { + StatusSignal positionCode = _encoder.getAbsolutePosition(); + if(!positionCode.getStatus().isOK()) { + _configured = false; + return new Rotation2d(); + } + return Rotation2d.fromRotations(positionCode.getValueAsDouble()); + } else + return new Rotation2d(); + } + + + public CANcoder get() { + return _encoder; + } +} diff --git a/src/main/java/frc/lib/HardwareDevices/GyroInterface.java b/src/main/java/frc/lib/HardwareDevices/GyroInterface.java new file mode 100644 index 0000000..3ab6077 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/GyroInterface.java @@ -0,0 +1,15 @@ +package frc.team696.lib.HardwareDevices; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; + +/** + * Base interface for switching between different Gyro Devices + */ +public interface GyroInterface { + public abstract Rotation2d getYaw(); + + public abstract void resetYaw(); + + public abstract AngularVelocity getAngularVelocity(); +} diff --git a/src/main/java/frc/lib/HardwareDevices/PigeonFactory.java b/src/main/java/frc/lib/HardwareDevices/PigeonFactory.java new file mode 100644 index 0000000..c2fa762 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/PigeonFactory.java @@ -0,0 +1,143 @@ +package frc.team696.lib.HardwareDevices; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.team696.lib.Logging.PLog; + +/** + * Automatically configures and optimizes pigeon + */ +public class PigeonFactory implements GyroInterface { + + private final double TIMEOUT = 0.05; + private Pigeon2 _gyro; + private Pigeon2Configuration _config; + private String _name; + + private boolean _configured = false; + private double _lastConfiguration = -100; + + public StatusSignal _yawSignal; + public StatusSignal _yawVelocitySignal; + + private Alert configurationAlert; + + public PigeonFactory(int id, String canBus, Pigeon2Configuration config, String name) { + this._gyro = new Pigeon2(id, canBus); + this._name = name; + this._config = config; + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public PigeonFactory(int id, Pigeon2Configuration config, String name) { + this(id, "rio", config, name); + } + + private boolean configure() { + return configure(false); + } + + public boolean configure(boolean force) { + if (!force && _configured) return true; + if (!force && Timer.getFPGATimestamp() - _lastConfiguration < 3) return false; + + _lastConfiguration = Timer.getFPGATimestamp(); + StatusCode configCode = _gyro.getConfigurator().apply(this._config, TIMEOUT); + + _yawSignal = _gyro.getYaw(); + _yawVelocitySignal = _gyro.getAngularVelocityZWorld(); + + if(configCode.isError() || _yawSignal.getStatus().isError() || _yawVelocitySignal.getStatus().isError()) { + PLog.unusual(_name, "Failed to configure"); + } else { + PLog.info(_name, "Configured"); + if (configCode.isWarning()) { + PLog.unusual(_name, "Config Warning: " + configCode.toString()); + } + + _configured = true; + } + + // Actually Used In Swerve + _yawSignal.setUpdateFrequency(100); + _yawVelocitySignal.setUpdateFrequency(100); + + // Useful Other Commands That May Be Used In The Future + _gyro.getPitch().setUpdateFrequency(100); + _gyro.getRoll().setUpdateFrequency(100); + + _gyro.optimizeBusUtilization(); + + configurationAlert.set(!_configured); + + return _configured; + } + + private double getRawYaw(boolean refresh) { + if (configure()) { + if (refresh) { + StatusSignal code = _yawSignal.refresh(); + if(!code.getStatus().isOK()) { + _configured = false; + return 0; + } + } + return MathUtil.inputModulus(_yawSignal.getValueAsDouble(),-180,180); + } else + return 0; + } + + /* + * Angle Of the Gyro in -180, 180 + */ + @Override + public Rotation2d getYaw() { + return Rotation2d.fromDegrees(getRawYaw(false)); + } + + @Override + public void resetYaw() { + _gyro.reset(); + } + + public AngularVelocity getAngularVelocity(boolean refresh) { + if (configure()) { + if (refresh) { + StatusSignal code = _yawVelocitySignal.refresh(); + if(!code.getStatus().isOK()) { + _configured = false; + return RadiansPerSecond.of(0); + } + } + return _yawVelocitySignal.getValue(); + } else + return RadiansPerSecond.of(0); + } + + @Override + public AngularVelocity getAngularVelocity() { + return getAngularVelocity(false); + } + + public Pigeon2 get() { + return _gyro; + } + + public Rotation2d getLatencyAdjustedYaw() { + return Rotation2d.fromDegrees(MathUtil.inputModulus(BaseStatusSignal.getLatencyCompensatedValue(_yawSignal, _yawVelocitySignal).magnitude(),-180,180)); + } +} diff --git a/src/main/java/frc/lib/HardwareDevices/TalonFactory.java b/src/main/java/frc/lib/HardwareDevices/TalonFactory.java new file mode 100644 index 0000000..47c83a2 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/TalonFactory.java @@ -0,0 +1,197 @@ +package frc.team696.lib.HardwareDevices; + +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.team696.lib.Logging.PLog; + +/** + * Class For creating and keeping track of talon device + * + *

Automatically logs name of the device if it fails to config + * + *

Resets device parameters if it drops out + */ +public class TalonFactory { + private final double TIMEOUT = 0.05; + private TalonFX _motor; + private TalonFXConfiguration _config; + private String _name; + private DutyCycleOut _DutyCycleControl; + private VoltageOut _VoltageControl; + + private int _followID = -1; + private boolean _opposeDirection = false; + + private boolean _configured = false; + private double _lastConfiguration = -100; + + private Alert configurationAlert; + + public TalonFactory(int id, String canBus, TalonFXConfiguration config, String name) { + this._motor = new TalonFX(id, canBus); + this._name = name; + this._config = config; + _DutyCycleControl = new DutyCycleOut(0); + _VoltageControl = new VoltageOut(0); + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public TalonFactory(int id, CANBus canBus, TalonFXConfiguration config, String name) { + this._motor = new TalonFX(id, canBus); + this._name = name; + this._config = config; + _DutyCycleControl = new DutyCycleOut(0); + _VoltageControl = new VoltageOut(0); + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public TalonFactory(int id, TalonFXConfiguration config, String name) { + this(id, "rio", config, name); + } + + private boolean configure() { + return configure(false); + } + + public boolean configure(boolean force) { + if (!force && _configured) return true; + if (!force && Timer.getFPGATimestamp() - _lastConfiguration < 3) return false; + + _lastConfiguration = Timer.getFPGATimestamp(); + StatusCode configCode = _motor.getConfigurator().apply(this._config, TIMEOUT); + + if(configCode.isError()) { + PLog.unusual(_name, "Failed to configure"); + } else { + PLog.info(_name, "Configured"); + if (configCode.isWarning()) { + PLog.unusual(_name, "Config Warning: " + configCode.toString()); + } + + if (_followID != -1) + _motor.setControl(new Follower(_followID, _opposeDirection)); + + setPosition(0); + + _configured = true; + } + + configurationAlert.set(!_configured); + + return _configured; + } + + public void Follow(int id, boolean opposeDirection) { + _followID = id; + this._opposeDirection = opposeDirection; + if (configure()) + _motor.setControl(new Follower(id, opposeDirection)); + } + + public void Follow(TalonFX master, boolean opposeDirection) { + Follow(master.getDeviceID(), opposeDirection); + } + + public void Follow(TalonFactory master, boolean opposeDirection) { + Follow(master.getID(), opposeDirection); + } + + public int getID() { + return _motor.getDeviceID(); + } + + public String getName(){ + return _name; + } + + public void setPosition(double newPosition) { + if (configure()) + if(!_motor.setPosition(newPosition).isOK()) { + PLog.info(this._name, "Failed To Set Position"); + _configured = false; + } + } + + public void setPosition() { + setPosition(0); + } + + public double getPosition() { + if (configure()) { + StatusSignal positionCode = _motor.getPosition(); + if(!positionCode.getStatus().isOK()) { + _configured = false; + return 0; + } + return positionCode.getValueAsDouble(); + } else + return 0; + } + + public double getVelocity() { + if (configure()) { + StatusSignal velocityCode = _motor.getVelocity(); + if(!velocityCode.getStatus().isOK()) { + _configured = false; + return 0; + } + return velocityCode.getValueAsDouble(); + } else + return 0; + } + + public void PercentOutput(double percent) { + setControl(_DutyCycleControl.withOutput(percent)); + } + + /** @param voltage 0-12 input */ + public void VoltageOut(Voltage voltage) { + setControl(_VoltageControl.withOutput(voltage.in(Volts))); + } + + public void stop() { + _motor.stopMotor(); + } + + public void setControl(ControlRequest request) { + if (configure()) + if (!_motor.setControl(request).isOK()) { + _configured = false; + } + } + + public double getCurrent() { + if (configure()) { + StatusSignal currentCode = _motor.getStatorCurrent(); + if(!currentCode.getStatus().isOK()) { + _configured = false; + return 0; + } + return currentCode.getValueAsDouble(); + } else + return 0; + } + + public TalonFX get() { + return _motor; + } +} diff --git a/src/main/java/frc/lib/Logging/AdvantageKitLog.java b/src/main/java/frc/lib/Logging/AdvantageKitLog.java new file mode 100644 index 0000000..b942d1c --- /dev/null +++ b/src/main/java/frc/lib/Logging/AdvantageKitLog.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +/* +package frc.team696.lib.Logging; + +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.BuildConstants; + +/** Simple Setup for Advantage Kit */ /* +@SuppressWarnings("resource") +public class AdvantageKitLog { + public static final void setup() { + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + if (RobotBase.isReal()) { + new PowerDistribution(1, ModuleType.kRev); + Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + } + + Logger.start(); + } +} +*/ \ No newline at end of file diff --git a/src/main/java/frc/lib/Logging/BackupLogger.java b/src/main/java/frc/lib/Logging/BackupLogger.java new file mode 100644 index 0000000..e1f7635 --- /dev/null +++ b/src/main/java/frc/lib/Logging/BackupLogger.java @@ -0,0 +1,161 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib.Logging; + +import java.util.concurrent.ConcurrentLinkedQueue; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.hal.PowerJNI; +import edu.wpi.first.hal.can.CANJNI; +import edu.wpi.first.hal.can.CANStatus; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistribution; + +/** + * NOT DONE + * + * + * Sick Logger that uses generics to fuck shit up. Who knows how performant it + * is, who cares, it's 1 file! + * + * May Switch to this internally -> less reliance on other libraries, more + * freedom, doesn't hurt anything anyway + * + * Really Meant as a backup/alternative to other loggers, use it if you want, + * idgaf + */ +public class BackupLogger { + public static DataLog log; + + public static final boolean PUBLISH_TO_NT = true; + public static final boolean PUBLISH_TO_LOG = true; + + private static ConcurrentLinkedQueue> logQueue = new ConcurrentLinkedQueue>(); + + private static final CANStatus status = new CANStatus(); + private static PowerDistribution pdh; + + private static logThread lThread; + + static { + DataLogManager.start("/U/logs"); + + log = DataLogManager.getLog(); + + DataLogManager.logConsoleOutput(true); + DataLogManager.logNetworkTables(true); + DriverStation.startDataLog(log, true); + + lThread = new logThread(); + lThread.start(); + } + + public static void stop() { + lThread.isRunning = false; + DataLogManager.stop(); + + for (LogEntry.GenericPublisher p : LogEntry.publishers.values()) { + p.publisher.close(); + } + + for (LogEntry.GenericDataLog p : LogEntry.logEntries.values()) { + p.dataLog.finish(); + } + } + + public static void setPdh(PowerDistribution newPdh) { + pdh = newPdh; + } + + /** + * Call Periodically If Desired + */ + public static void logSystemInformation() { + addToQueue("SystemStats/FPGAVersion", HALUtil.getFPGAVersion()); + addToQueue("SystemStats/FPGARevision", HALUtil.getFPGARevision()); + addToQueue("SystemStats/SerialNumber", HALUtil.getSerialNumber()); + addToQueue("SystemStats/Comments", HALUtil.getComments()); + addToQueue("SystemStats/TeamNumber", HALUtil.getTeamNumber()); + addToQueue("SystemStats/FPGAButton", HALUtil.getFPGAButton()); + addToQueue("SystemStats/SystemActive", HAL.getSystemActive()); + addToQueue("SystemStats/BrownedOut", HAL.getBrownedOut()); + addToQueue("SystemStats/RSLState", HAL.getRSLState()); + addToQueue("SystemStats/SystemTimeValid", HAL.getSystemTimeValid()); + addToQueue("SystemStats/BatteryVoltage", PowerJNI.getVinVoltage()); + addToQueue("SystemStats/BatteryCurrent", PowerJNI.getVinCurrent()); + addToQueue("SystemStats/3v3Rail/Voltage", PowerJNI.getUserVoltage3V3()); + addToQueue("SystemStats/3v3Rail/Current", PowerJNI.getUserCurrent3V3()); + addToQueue("SystemStats/3v3Rail/Active", PowerJNI.getUserActive3V3()); + addToQueue("SystemStats/3v3Rail/CurrentFaults", PowerJNI.getUserCurrentFaults3V3()); + addToQueue("SystemStats/5vRail/Voltage", PowerJNI.getUserVoltage5V()); + addToQueue("SystemStats/5vRail/Current", PowerJNI.getUserCurrent5V()); + addToQueue("SystemStats/5vRail/Active", PowerJNI.getUserActive5V()); + addToQueue("SystemStats/5vRail/CurrentFaults", PowerJNI.getUserCurrentFaults5V()); + addToQueue("SystemStats/6vRail/Voltage", PowerJNI.getUserVoltage6V()); + addToQueue("SystemStats/6vRail/Current", PowerJNI.getUserCurrent6V()); + addToQueue("SystemStats/6vRail/Active", PowerJNI.getUserActive6V()); + addToQueue("SystemStats/6vRail/CurrentFaults", PowerJNI.getUserCurrentFaults6V()); + addToQueue("SystemStats/BrownoutVoltage", PowerJNI.getBrownoutVoltage()); + addToQueue("SystemStats/CPUTempCelcius", PowerJNI.getCPUTemp()); + + CANJNI.getCANStatus(status); + addToQueue("SystemStats/CANBus/Utilization", status.percentBusUtilization); + addToQueue("SystemStats/CANBus/OffCount", status.busOffCount); + addToQueue("SystemStats/CANBus/TxFullCount", status.txFullCount); + addToQueue("SystemStats/CANBus/ReceiveErrorCount", status.receiveErrorCount); + addToQueue("SystemStats/CANBus/TransmitErrorCount", status.transmitErrorCount); + addToQueue("SystemStats/EpochTimeMicros", HALUtil.getFPGATime()); + + if (pdh == null) + return; + + addToQueue("SystemStats/PowerDistribution/Temperature", pdh.getTemperature()); + addToQueue("SystemStats/PowerDistribution/Voltage", pdh.getVoltage()); + addToQueue("SystemStats/PowerDistribution/ChannelCurrent", pdh.getAllCurrents()); + addToQueue("SystemStats/PowerDistribution/TotalCurrent", pdh.getTotalCurrent()); + addToQueue("SystemStats/PowerDistribution/TotalPower", pdh.getTotalPower()); + addToQueue("SystemStats/PowerDistribution/TotalEnergy", pdh.getTotalEnergy()); + addToQueue("SystemStats/PowerDistribution/ChannelCount", pdh.getNumChannels()); + } + + public static void addToQueue(String name, T value) { + if (lThread.isRunning) + logQueue.add(new LogEntry(name, value)); + } + + public static class logThread extends Thread { + public logThread() { + this.setName("LoggerThread"); + this.setDaemon(true); + this.setPriority(MIN_PRIORITY); + } + + public volatile boolean isRunning = true; + + @Override + public void run() { + while (isRunning) { + LogEntry entry = BackupLogger.logQueue.poll(); + if (entry == null) + continue; + try { + if (BackupLogger.PUBLISH_TO_NT) { + LogEntry.GenericPublisher.classToPublisher.get(entry.value.getClass()) + .invoke(LogEntry.publishers.get(entry.name).publisher, entry.value); + } + if (BackupLogger.PUBLISH_TO_LOG) { + LogEntry.GenericDataLog.classToDatalog.get(entry.value.getClass()) + .invoke(LogEntry.logEntries.get(entry.name).dataLog, entry.value); + } + } catch (Exception e) { + PLog.info("Logger", "Publisher Failed to accept value: " + entry.name); + } + } + } + } +} diff --git a/src/main/java/frc/lib/Logging/LogEntry.java b/src/main/java/frc/lib/Logging/LogEntry.java new file mode 100644 index 0000000..40a45a1 --- /dev/null +++ b/src/main/java/frc/lib/Logging/LogEntry.java @@ -0,0 +1,192 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib.Logging; + +import java.lang.reflect.Method; +import java.util.concurrent.ConcurrentHashMap; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.Publisher; +import edu.wpi.first.util.datalog.BooleanArrayLogEntry; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLogEntry; +import edu.wpi.first.util.datalog.DoubleArrayLogEntry; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.IntegerArrayLogEntry; +import edu.wpi.first.util.datalog.IntegerLogEntry; +import edu.wpi.first.util.datalog.ProtobufLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.datalog.StructArrayLogEntry; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import us.hebi.quickbuf.ProtoMessage; + +@SuppressWarnings("unchecked") +/** Known Bug, Fails to print string the first time, caught in a try catch so it's fine, just don't know why it's hpapening + * + * Everything works but int[]s, because they are longs internally Sick + */ +public class LogEntry { + public final T value; + public final String name; + public final Long time; + public LogEntry(String name, T value) { + this.value = value; + this.name = name; + this.time = System.currentTimeMillis(); + + if (BackupLogger.log == null) return; + + if (publishers.containsKey(name)) return; + + if (value instanceof String) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getStringTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new StringLogEntry(BackupLogger.log, name))); + + return; + } + + if (value instanceof Integer || value instanceof Long || value instanceof Short) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getIntegerTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new IntegerLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof int[] || value instanceof long[] || value instanceof short[]) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getIntegerArrayTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new IntegerArrayLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof Double || value instanceof Float) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getDoubleTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new DoubleLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof double[] || value instanceof float[]) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getDoubleArrayTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new DoubleArrayLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof Boolean) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getBooleanTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new BooleanLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof boolean[]) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getBooleanArrayTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new BooleanArrayLogEntry(BackupLogger.log, name))); + return; + } + + + if(value instanceof StructSerializable) { + try { + Struct structImplementation = (Struct)value.getClass().getDeclaredField("struct").get(null); + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getStructTopic(name, structImplementation).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), StructLogEntry.create(BackupLogger.log, name, structImplementation))); + + } catch ( Exception e ){ + PLog.info("Logger", "Malformed Struct detected: " + name); + } + return; + } + + + if(value instanceof StructSerializable[]) { + try { + Struct structImplementation = (Struct)value.getClass().getComponentType().getDeclaredField("struct").get(null); + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getStructArrayTopic(name, structImplementation).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), StructArrayLogEntry.create(BackupLogger.log, name, structImplementation))); + + } catch ( Exception e ){ + PLog.info("Logger", "Malformed Struct detected: " + name); + } + return; + } + + if (value instanceof ProtobufSerializable) { + try { + Protobuf> protobufImplementation = (Protobuf>)value.getClass().getDeclaredField("proto").get(null); + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getProtobufTopic(name, protobufImplementation).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), ProtobufLogEntry.create(BackupLogger.log, name, protobufImplementation))); + } catch ( Exception e ){ + PLog.info("Logger", "Malformed Protobuf detected: " + name); + } + return; + } + + + + PLog.info("Logger", "failed to categorize logEntry: " + name); + + } + + + /** + * private final StructArrayPublisher swerveModuleDesiredStatePublisher = NetworkTableInstance.getDefault() +.getStructArrayTopic("696/Swerve/DesiredStates", SwerveModuleState.struct).publish(); + + */ + + public static class GenericPublisher { + public T publisher; + public static final ConcurrentHashMap, Method> classToPublisher = new ConcurrentHashMap<>(); + public GenericPublisher(Class type, T publisher) { + this.publisher = publisher; + + if (classToPublisher.containsKey(type)) return; + + Method[] methodsNT = publisher.getClass().getMethods(); + for (Method method : methodsNT) { + if (method.getName().equals("set") && method.getParameterCount() == 1) { + if (method.getParameterTypes()[0].isArray() != type.isArray()) continue; + + classToPublisher.put(type, method); + break; + } + } + } + } + + public static class GenericDataLog { + public T dataLog; + public static final ConcurrentHashMap, Method> classToDatalog = new ConcurrentHashMap<>(); + public GenericDataLog(Class type, T log) { + this.dataLog = log; + + if (classToDatalog.containsKey(type)) return; + + Method[] methodsLOG = dataLog.getClass().getMethods(); + for (Method method : methodsLOG) { + if (method.getName().equals("update") && method.getParameterCount() == 1) { + if (method.getParameterTypes()[0].isArray() != type.isArray()) continue; + // if (!((Object)type).getClass().isAssignableFrom(temp)) continue; + + classToDatalog.put(type, method); + break; + } + } + } + } + public static final ConcurrentHashMap> publishers = new ConcurrentHashMap<>(); + public static final ConcurrentHashMap> logEntries = new ConcurrentHashMap<>(); +} diff --git a/src/main/java/frc/lib/Logging/PLog.java b/src/main/java/frc/lib/Logging/PLog.java new file mode 100644 index 0000000..d97f00f --- /dev/null +++ b/src/main/java/frc/lib/Logging/PLog.java @@ -0,0 +1,91 @@ +package frc.team696.lib.Logging; + +import edu.wpi.first.wpilibj.DriverStation; + +/** Class for printing out to the console */ +public class PLog { + /** + * Log a FATAL error, after which the robot cannot (properly) function.
+ * + * @param category + * @param message + */ + public static void fatal(String category, T message) { + log("Fatal", category, String.valueOf(message)); + + // make it show up on the DS as well + DriverStation.reportError("Fatal Error: " + String.valueOf(message), true); + } + + /** + * Log a FATAL error due to an exception, after which the robot cannot + * (properly) function.
+ * Prints your message, and the exception's name, message, and stacktrace. + * + */ + public static void fatalException(String category, String userMessage, Exception exception) { + String exceptionMessage = String.format("%s -- %s: %s", userMessage, exception.getClass().getSimpleName(), exception.getMessage()); + for (StackTraceElement element : exception.getStackTrace()) { + exceptionMessage += "\n " + element; + } + log("Fatal", category, exceptionMessage); + + } + + public static void fatalException(String category, String userMessage, StackTraceElement[] elements) { + String exceptionMessage = String.format("%s ", userMessage); + for (StackTraceElement element : elements) { + exceptionMessage += "\n " + element; + } + log("Fatal", category, exceptionMessage); + } + + /** + * Log a failure which may kill one function or one thread, however the robot as + * a whole can keep functioning. + * + * @param category + * @param message + */ + public static void recoverable(String category, T message) { + log("Recoverable", category, String.valueOf(message)); + + DriverStation.reportError("Error: " + String.valueOf(message), true); + } + + /** + * Log something which should not happen under normal circumstances and probably + * is a bug, but does not cause anything to crash. + * + * @param category + * @param message + */ + public static void unusual(String category, T message) { + log("Unusual", category, String.valueOf(message)); + } + + /** + * Log a semi-important message which the user should probably see, but does not + * indicate anything is broken. + */ + public static void info(String category, T message) { + log("Info", category, String.valueOf(message)); + } + + /** + * Log a message which is not important during normal operation, but is useful + * if you're trying to debug the robot. + * + * @param category + * @param message + */ + public static void debug(String category, T message) { + log("Debug", category, String.valueOf(message)); + } + + private static void log(String severity, String category, String message) { + String output = String.format("[%s] [%s] %s", severity, category, message); + System.out.println(output); + BackupLogger.addToQueue("prints", output); + } +} diff --git a/src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java b/src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java new file mode 100644 index 0000000..a5f9ce3 --- /dev/null +++ b/src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java @@ -0,0 +1,165 @@ +package frc.team696.lib.Swerve.Commands; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team696.lib.Util; +import frc.team696.lib.Swerve.SwerveConstants; +import frc.team696.lib.Swerve.SwerveDriveSubsystem; + +/** + * Swerve command for driving around during teleop + * + *

Config once and create multiple objects to handle different driving styles + */ +public class TeleopSwerve extends Command { + protected static DoubleSupplier translation = ()->0; + protected static DoubleSupplier strafe = ()->0; + protected static DoubleSupplier rotation = ()->0; + protected static double deadband = 1; // deadband for controller -> defaulted to 1 so you must config swerve + protected static double rotationDeadband = 1; + protected static SwerveDriveSubsystem swerveSubsystem = null; + protected static double lastPeriodicSeconds = 0; + private static PIDController pidController = new PIDController(0.0056, 0.00, 0); + static { + pidController.enableContinuousInput(-180, 180); + pidController.setTolerance(1); + } + private static BooleanSupplier lockRotation; // should lock rotation -> usually a button + + private boolean fieldRelative; // should do fieldRelative controol + private boolean openLoop; // should do openLoop control + private Supplier rotationGoal; // Rotation to lock to once lockRotation has been activated + private DoubleSupplier multiplier = ()->1; // Multiplier to outputted Speed + + public static void config(SwerveDriveSubsystem s, DoubleSupplier x, DoubleSupplier y, DoubleSupplier r, BooleanSupplier rotationLock, double deadBand) { + strafe = x; + translation = y; + rotation = r; + + lockRotation = rotationLock; + + deadband = deadBand; + rotationDeadband = Math.sqrt(2 * Math.pow(deadband, 2)); + swerveSubsystem = s; + } + + public static TeleopSwerve New(){ + return new TeleopSwerve(); + } + + private TeleopSwerve() { + this.fieldRelative = true; + this.openLoop = true; + + this.rotationGoal = ()->null; + + this.multiplier = ()->1; + + addRequirements(swerveSubsystem); + } + + public TeleopSwerve withMultiplier(DoubleSupplier multiplier) { + this.multiplier = multiplier; + return this; + } + + public TeleopSwerve withMultiplier(double multiplier) { + this.multiplier = ()->multiplier; + return this; + } + + public TeleopSwerve withRotationGoal(Supplier goal) { + this.rotationGoal = goal; + return this; + } + + public TeleopSwerve withRotationGoal(Rotation2d goal) { + this.rotationGoal = ()->goal; + return this; + } + + public TeleopSwerve withRobotRelative(boolean robotRelative) { + this.fieldRelative = !robotRelative; + return this; + } + + public TeleopSwerve withOpenLoop(boolean openLoop) { + this.openLoop = openLoop; + return this; + } + + @Override + public void execute() { + double yAxis = translation.getAsDouble(); + double xAxis = strafe.getAsDouble(); + double rAxis = rotation.getAsDouble(); + + Rotation2d theta=Rotation2d.fromDegrees(0); + if(xAxis!=0||yAxis!=0) + theta = new Rotation2d(yAxis, xAxis); + double magnitude = Math.min(Math.sqrt((xAxis * xAxis) + (yAxis * yAxis)), 1); + if (magnitude < deadband) magnitude = 0; + Rotation2d goalRotation = rotationGoal.get(); + if (lockRotation != null && lockRotation.getAsBoolean() && goalRotation != null) { // Rotation Lock To Angle + double pid = pidController.calculate(swerveSubsystem.getPose().getRotation().getDegrees(), goalRotation.getDegrees()); + rAxis = Math.abs(pidController.getError()) > 1 ? Math.abs(Math.pow(pid, 2)) * 1.1 * Math.signum(pid) + pid * 2.2 : 0; + } else { + rAxis = (Math.abs(rAxis) > rotationDeadband) ? Util.map(rAxis * rAxis, rotationDeadband, 1, 0, 1) * Math.signum(rAxis) : 0; + } + + double outputPercent = Math.min(multiplier.getAsDouble(),1); + + double desiredRotation = rAxis * SwerveConstants.MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); + Translation2d desiredTranslation = new Translation2d(Math.pow(magnitude, 2), theta).times(SwerveConstants.MAX_VELOCITY.in(MetersPerSecond)).times(outputPercent); + + /* Untested Portion Begin + double deltaSeconds = Timer.getFPGATimestamp() - lastPeriodicSeconds; + + // Acceleration Limiting Start + Translation2d curVel = swerveSubsystem.getState().velocityXY(); + Translation2d desiredAcceleration = desiredTranslation.minus(curVel).div(deltaSeconds); + double accelerationLimit = SwerveConstants.MAX_ACCELERATION.in(MetersPerSecondPerSecond) * (1 - curVel.getNorm() / SwerveConstants.MAX_VELOCITY.in(MetersPerSecond)); + + desiredAcceleration = desiredAcceleration.div(desiredAcceleration.getNorm()).times(accelerationLimit); + + double accelerationXYScaling = Math.max(Math.max(1 , Math.abs(desiredAcceleration.getX()) / SwerveConstants.MAX_ACCELERATION_X.in(MetersPerSecondPerSecond) ), Math.abs(desiredAcceleration.getY()) / SwerveConstants.MAX_ACCELERATION_Y.in(MetersPerSecondPerSecond)); + desiredAcceleration = desiredAcceleration.div(desiredAcceleration.getNorm()).times(accelerationXYScaling); + + double maxSkidAcceleration = Math.max(1, desiredAcceleration.getNorm() / SwerveConstants.MAX_ACCELERATION_SKID.in(MetersPerSecondPerSecond)); + desiredAcceleration = desiredAcceleration.div(desiredAcceleration.getNorm()).times(maxSkidAcceleration); + + // Acceleration Limiting End + + // Jerk Limiting Start + // Might do Questionable Things because we are using our currentAcceleration which may not be accurate (Why it is in the untested Portion) + + Translation2d curAccel = swerveSubsystem.getState().accelerationXY(); + Translation2d desiredJerk = desiredAcceleration.minus(curAccel).div(deltaSeconds); + double jerkLimit = SwerveConstants.MAX_JERK.in(MetersPerSecondPerSecond.per(Seconds)); + + double jerkScaling = Math.min(jerkLimit, desiredJerk.getNorm()); + + Translation2d scaledJerk = desiredJerk.div(desiredJerk.getNorm()).times(jerkScaling); + + desiredAcceleration = curAccel.plus(scaledJerk.times(deltaSeconds)); + + // Jerk Limiting End + + desiredTranslation = curVel.plus(desiredAcceleration.times(deltaSeconds)); + + lastPeriodicSeconds = Timer.getFPGATimestamp(); + /* Untested Portion End */ + + swerveSubsystem.Drive(desiredTranslation, desiredRotation, fieldRelative, openLoop); + + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Swerve/SwerveConfigs.java b/src/main/java/frc/lib/Swerve/SwerveConfigs.java new file mode 100644 index 0000000..7cd502f --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveConfigs.java @@ -0,0 +1,107 @@ +package frc.team696.lib.Swerve; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + +/** + * + * Device Configs of the Swerve Drive + * + *

Only Update Device Configs if you know what you are doing. + */ +public final class SwerveConfigs { + public final static TalonFXConfiguration angle; + public final static TalonFXConfiguration drive; + public final static CANcoderConfiguration canCoder; + public final static Pigeon2Configuration pigeon; + public final static SwerveModuleConstants FRONT_LEFT; + public final static SwerveModuleConstants FRONT_RIGHT; + public final static SwerveModuleConstants BACK_LEFT; + public final static SwerveModuleConstants BACK_RIGHT; + static { + angle = new TalonFXConfiguration(); + drive = new TalonFXConfiguration(); + canCoder = new CANcoderConfiguration(); + pigeon = new Pigeon2Configuration(); + + FRONT_LEFT = new SwerveModuleConstants(); + FRONT_RIGHT = new SwerveModuleConstants(); + BACK_LEFT = new SwerveModuleConstants(); + BACK_RIGHT = new SwerveModuleConstants(); + + /** Swerve CANCoder Configuration */ + canCoder.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + canCoder.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + + /** Swerve Angle Motor Configuration */ + angle.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + angle.MotorOutput.NeutralMode = NeutralModeValue.Coast; + angle.Feedback.SensorToMechanismRatio = SwerveConstants.ANGLE_GEAR_RATIO; + angle.ClosedLoopGeneral.ContinuousWrap = true; + angle.CurrentLimits.SupplyCurrentLimitEnable = true; + angle.CurrentLimits.SupplyCurrentLimit = 25; + angle.CurrentLimits.SupplyCurrentLowerLimit = 60; + angle.CurrentLimits.SupplyCurrentLowerTime = 0.1; + angle.CurrentLimits.StatorCurrentLimitEnable = true; + angle.CurrentLimits.StatorCurrentLimit = 80; + angle.Slot0.kP = 150.0; + angle.Slot0.kI = 0.0; + angle.Slot0.kD = 0.0; + + angle.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0; + angle.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0; + angle.Voltage.PeakForwardVoltage = 12.; + angle.Voltage.PeakReverseVoltage = -12.; + + /** Swerve Drive Motor Configuration */ + drive.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + drive.MotorOutput.NeutralMode = NeutralModeValue.Brake; + drive.Feedback.SensorToMechanismRatio = SwerveConstants.DRIVE_GEAR_RATIO; + drive.CurrentLimits.SupplyCurrentLimitEnable = true; + drive.CurrentLimits.SupplyCurrentLimit = 25; + drive.CurrentLimits.SupplyCurrentLowerLimit = 90; + drive.CurrentLimits.SupplyCurrentLowerTime = 0.2; + drive.CurrentLimits.StatorCurrentLimitEnable = true; + drive.CurrentLimits.StatorCurrentLimit = 110; + drive.Voltage.PeakForwardVoltage = 12.; + drive.Voltage.PeakReverseVoltage = -12.; + + drive.Slot0.kP = 2.; + drive.Slot0.kI = 0.0; + drive.Slot0.kD = 0.0; + drive.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = 0.25; + drive.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.25; + drive.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.02; + drive.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.02; + + /** Individual Swerve Module Configurations */ + FRONT_LEFT.EncoderId = 0; + FRONT_LEFT.DriveMotorId = 1; + FRONT_LEFT.SteerMotorId = 0; + FRONT_LEFT.EncoderOffset = -0.24-.25; + + FRONT_RIGHT.EncoderId = 1; + FRONT_RIGHT.DriveMotorId = 3; + FRONT_RIGHT.SteerMotorId = 2; + FRONT_RIGHT.EncoderOffset = -0.393-.25; + + BACK_LEFT.EncoderId = 2; + BACK_LEFT.DriveMotorId = 5; + BACK_LEFT.SteerMotorId = 4; + + BACK_LEFT.EncoderOffset = -0.456-.25; + + BACK_RIGHT.EncoderId = 3; + BACK_RIGHT.DriveMotorId = 7; + BACK_RIGHT.SteerMotorId = 6; + BACK_RIGHT.EncoderOffset = -0.03-.25; + + /** Pigeon Configuration */ + pigeon.MountPose.MountPoseYaw = 0; + } +} diff --git a/src/main/java/frc/lib/Swerve/SwerveConstants.java b/src/main/java/frc/lib/Swerve/SwerveConstants.java new file mode 100644 index 0000000..f564408 --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveConstants.java @@ -0,0 +1,116 @@ +package frc.team696.lib.Swerve; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Kilogram; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Minute; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.LinearAccelerationUnit; +import edu.wpi.first.units.LinearVelocityUnit; +import edu.wpi.first.units.MassUnit; +import edu.wpi.first.units.MultUnit; +import edu.wpi.first.units.TimeUnit; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Force; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Mass; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Mult; +import edu.wpi.first.units.measure.Per; +import edu.wpi.first.units.measure.Torque; +import edu.wpi.first.units.measure.Voltage; + +/** + * Constants of the Swerve Drive + * + *

Should Be Updated For Each Robot Accordingly + */ +public class SwerveConstants { + public static final String canBus = "cv"; + + public static final int expectedModuleCount = 4; + + public static final Voltage drivekS = Volts.of(0.667); + public static final Per drivekV = Volts.of(2.44).div(MetersPerSecond.of(1)); + public static final Per drivekA = Volts.of(0.27).div(MetersPerSecondPerSecond.of(1)); + + public static final double DRIVE_GEAR_RATIO = (5.36 / 1.0); + public static final double ANGLE_GEAR_RATIO = (150.0 / 7.0 / 1.0); + + public static final Mass MASS = Pounds.of(140); + + // Distance From The Center To The Wheel, From The Distance Between Two Wheels divided By 2 + public static final Distance WHEELBASE_X = Inches.of(18.5).div(2); + public static final Distance WHEELBASE_Y = Inches.of(18.5).div(2); + public static final Distance DRIVEBASE_RADIUS = Inches.of(Math.sqrt(Math.pow(WHEELBASE_X.in(Inches), 2) + Math.pow(WHEELBASE_Y.in(Inches), 2))); + public static final Distance WHEEL_DIAMETER = Inches.of(3.5); + public static final Distance WHEEL_RADIUS = WHEEL_DIAMETER.div(2); + public static final Distance WHEEL_CIRCUM = WHEEL_DIAMETER.times(Math.PI); + public static final double WHEEL_COEFFICIENT_OF_FRICTION = 1.2; + + public static final Per DISTANCE_PER_ROTATION = WHEEL_CIRCUM.div(DRIVE_GEAR_RATIO).div(Rotations.of(1)); + + // Use Reca.lc to find Stall Torque At Motor Current Limit + // Values For Kraken X60 + public static final AngularVelocity MAX_FREESPIN_VELOCITY = Rotations.of(6000).per(Minute); + // Current Limit / Max Current / Torque At Max Current + public static final Torque MAX_MOTOR_STALL_TORQUE = NewtonMeters.of(SwerveConfigs.drive.CurrentLimits.StatorCurrentLimit / 366 * 7.09); + public static final Force MAX_WHEEL_FORCE = MAX_MOTOR_STALL_TORQUE.times(DRIVE_GEAR_RATIO).div(WHEEL_RADIUS).times(expectedModuleCount); + + public static final LinearVelocity THEORETICAL_MAX_SPEED = MetersPerSecond.of(MAX_FREESPIN_VELOCITY.in(RotationsPerSecond) * DISTANCE_PER_ROTATION.in(Meters.per(Rotation))); + public static final LinearAcceleration THEORETICAL_MAX_ACCELERATION = (MAX_WHEEL_FORCE.div(MASS)); + public static final LinearVelocity MAX_VELOCITY = THEORETICAL_MAX_SPEED.times(0.85); + public static final LinearAcceleration MAX_ACCELERATION = THEORETICAL_MAX_ACCELERATION.times(0.7); + public static final AngularVelocity THEORETICAL_MAX_ANGULAR_VELOCITY = RotationsPerSecond.of(MAX_VELOCITY.in(MetersPerSecond) / (DRIVEBASE_RADIUS.in(Meters) * 2 * Math.PI)); + public static final AngularVelocity MAX_ANGULAR_VELOCITY = THEORETICAL_MAX_ANGULAR_VELOCITY.times(0.95); + + /* Credit To 1690s Software Session Part 1 + * Forward limit -> Max Acceleration Magnitude -> MaxAcc * (1 - curVel / maxVel)-> see TeleopSwerve.java for usage + * Tilt limit -> Limits Magnitude of Accelx and Accely -> MaxFrontAccel & MaxSideAcc + * Skid Limit -> Limits Max Acceleration Of the robot to prevent skidding + */ + public static final LinearAcceleration MAX_ACCELERATION_X = MetersPerSecondPerSecond.of(10000000); + public static final LinearAcceleration MAX_ACCELERATION_Y = MetersPerSecondPerSecond.of(10000000); + public static final LinearAcceleration MAX_ACCELERATION_SKID = MetersPerSecondPerSecond.of(10000000); + + /** + * Jerk Control - My Own + */ + public static final Per MAX_JERK = MetersPerSecondPerSecond.per(Seconds).ofNative(100000000); + + //ASSUMES UNIFORM DISTRIBUTION, SHOULD BE CALCULATED EXPERIMENTALLY + public static final Mult, DistanceUnit> THEORETICAL_MOMENT_OF_INERTIA = MASS.times(DRIVEBASE_RADIUS).times(DRIVEBASE_RADIUS).times(1/12); + + /** + * Credit (https://pathplanner.dev/robot-config.html#module-config-options) + * + * Calculate Kangular and Klinear through SysID + * + * Mass * WheelBase_X * Kangular * Klinear + */ + public static final MomentOfInertia MOMENT_OF_INERTIA = MomentOfInertia.ofBaseUnits(THEORETICAL_MOMENT_OF_INERTIA.in(MultUnit.combine(MultUnit.combine(Kilogram, Meters), Meters)), KilogramSquareMeters); + + public static final Translation2d[] modPositions = { + new Translation2d(WHEELBASE_X, WHEELBASE_Y), // FL + new Translation2d(WHEELBASE_X, WHEELBASE_Y.times(-1)), // FR + new Translation2d(WHEELBASE_X.times(-1), WHEELBASE_Y), // BL + new Translation2d(WHEELBASE_X.times(-1), WHEELBASE_Y.times(-1)) // BR + }; + +} diff --git a/src/main/java/frc/lib/Swerve/SwerveDriveState.java b/src/main/java/frc/lib/Swerve/SwerveDriveState.java new file mode 100644 index 0000000..d837c2f --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveDriveState.java @@ -0,0 +1,164 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib.Swerve; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; + +/** + * Holds the current State of the robot and last update. + * + *

Should Not be manually updated + */ +public class SwerveDriveState implements StructSerializable { + public Pose2d pose = new Pose2d(); + public ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds(); + public ChassisSpeeds robotAcceleration = new ChassisSpeeds(); + public double timeStamp = 0; + public double timeSinceLastUpdate = -1; + + /** Use This to Scale trust of camera measurements, decreasing it as you get updates, up to you how you want to change it. */ + public double trust = 1e-9; // Never Set To 0 + + public SwerveDriveState(Pose2d pose, ChassisSpeeds speeds, ChassisSpeeds accelerations, double time, double trust) { + update(pose, speeds, accelerations, time, trust); + } + + public SwerveDriveState(Pose2d pose, ChassisSpeeds speeds, ChassisSpeeds acceleration, double time, double timeSinceLastUpdate, double trust) { + this.pose = pose; + this.robotRelativeSpeeds = speeds; + this.robotAcceleration = acceleration; + this.timeStamp = time; + this.timeSinceLastUpdate = timeSinceLastUpdate; + this.trust = trust; + } + + public SwerveDriveState(SwerveDriveState other) { + this(other.pose, other.robotRelativeSpeeds, other.robotAcceleration, other.timeStamp, other.trust); + this.timeSinceLastUpdate = other.timeSinceLastUpdate; + } + + public SwerveDriveState() { + this(new Pose2d(), new ChassisSpeeds(), new ChassisSpeeds(), 0, 1e-9); + } + + /** + * + * @return Current X,Y velocity of the robot in M/S + */ + public double speed() { + return Math.sqrt(robotRelativeSpeeds.vxMetersPerSecond * robotRelativeSpeeds.vxMetersPerSecond + robotRelativeSpeeds.vyMetersPerSecond * robotRelativeSpeeds.vyMetersPerSecond); + } + + /** + * + * @return Current X,Y velocity of robot in M/S as a Vector + */ + public Translation2d velocityXY() { + return new Translation2d(robotRelativeSpeeds.vxMetersPerSecond, robotRelativeSpeeds.vyMetersPerSecond); + } + + public Translation2d accelerationXY() { + return new Translation2d(robotAcceleration.vxMetersPerSecond, robotAcceleration.vyMetersPerSecond); + } + + /** + * + * @return Current Rotational Velocity of the robot in Rad/s + */ + public double angularVelocity() { + return Math.abs(robotRelativeSpeeds.omegaRadiansPerSecond); + } + + /** + * Used interally for updated estimation + * + * @param pose + * @param speeds + * @param time + * @return + */ + public SwerveDriveState update(Pose2d pose, ChassisSpeeds speeds, ChassisSpeeds accelerations, double time, double slippage) { + this.timeSinceLastUpdate = time - this.timeStamp; + if (timeSinceLastUpdate == 0) timeSinceLastUpdate = 1e-6; + + this.pose = pose; + this.robotRelativeSpeeds = speeds; + this.robotAcceleration = accelerations; + this.timeStamp = time; + + this.trust += slippage; + + return this; + } + + public static final SwerveDriveStateStruct struct = new SwerveDriveStateStruct(); + public static class SwerveDriveStateStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveDriveState.class; + } + + @Override + public String getTypeName() { + return "SwerveDriveState"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + ChassisSpeeds.struct.getSize() + ChassisSpeeds.struct.getSize() + Double.BYTES * 3; + } + + @Override + public String getSchema() { + return "Pose2d pose;ChassisSpeeds velocity;ChassisSpeeds acceleration;double timestamp;double timeSinceLastUpdate;double trust"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct, ChassisSpeeds.struct}; + } + + @Override + public SwerveDriveState unpack(ByteBuffer bb) { + Pose2d pose = Pose2d.struct.unpack(bb); + ChassisSpeeds velocity = ChassisSpeeds.struct.unpack(bb); + ChassisSpeeds acceleration = ChassisSpeeds.struct.unpack(bb); + double time = bb.getDouble(); + double timeSinceLastUpdate = bb.getDouble(); + double trust = bb.getDouble(); + return new SwerveDriveState(pose, velocity, acceleration, time, timeSinceLastUpdate, trust); + } + + @Override + public void pack(ByteBuffer bb, SwerveDriveState value) { + Pose2d.struct.pack(bb, value.pose); + ChassisSpeeds.struct.pack(bb, value.robotRelativeSpeeds); + ChassisSpeeds.struct.pack(bb, value.robotAcceleration); + bb.putDouble(value.timeStamp); + bb.putDouble(value.timeSinceLastUpdate); + bb.putDouble(value.trust); + } + + @Override + public boolean isImmutable() { + return true; + } + } +} + + +/** + * Potential Future work -> Incorporate Gyro Readings into Current Acceleration and Store Jerk, Might be cool + * + * Add system for keeping track of confidence of current pose (1690). Overtime accumulate error from odometry -> reset down when getting vision readings ect. + * + * Will help update vision faster and more reliably, don't need to trust shit vision poses when we have gotten a good one and barely moved! + */ \ No newline at end of file diff --git a/src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java b/src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java new file mode 100644 index 0000000..3960bde --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java @@ -0,0 +1,444 @@ +package frc.team696.lib.Swerve; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import java.util.concurrent.locks.ReadWriteLock; +import java.util.concurrent.locks.ReentrantReadWriteLock; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team696.lib.Util; +import frc.team696.lib.HardwareDevices.PigeonFactory; +import frc.team696.lib.Logging.BackupLogger; + +public abstract class SwerveDriveSubsystem extends SubsystemBase { + private final SwerveModulePosition[] _swervePositions = new SwerveModulePosition[4]; + private final SwerveDrivePoseEstimator _poseEstimator; + + protected final SwerveModule[] _modules; + protected final SwerveDriveKinematics _kinematics; + + protected final PigeonFactory _pigeon; + + protected Rotation2d yawOffset = new Rotation2d(0); + + protected final ReadWriteLock _stateLock; + protected final SwerveDriveState _cachedState; + + private final odometryThread _odometryThread; + + private SwerveModuleState[] swerveModuleDesiredStates = { new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState() }; + + public SwerveDriveSubsystem() { + this._stateLock = new ReentrantReadWriteLock(); + this._cachedState = new SwerveDriveState(); + + SwerveModule frontLeft = new SwerveModule(SwerveConfigs.FRONT_LEFT); + SwerveModule frontRight = new SwerveModule(SwerveConfigs.FRONT_RIGHT); + SwerveModule backLeft = new SwerveModule(SwerveConfigs.BACK_LEFT); + SwerveModule backRight = new SwerveModule(SwerveConfigs.BACK_RIGHT); + _modules = new SwerveModule[] { frontLeft, frontRight, backLeft, backRight }; + + _kinematics = new SwerveDriveKinematics(SwerveConstants.modPositions); + + for (int i = 0; i < 4; ++i) { + _swervePositions[i] = _modules[i].getPosition(); + } + + _pigeon = new PigeonFactory(0, SwerveConstants.canBus, SwerveConfigs.pigeon, "Pigeon"); + + _poseEstimator = new SwerveDrivePoseEstimator(_kinematics, getYaw(), _swervePositions, + new Pose2d(0, 0, new Rotation2d(0)), VecBuilder.fill(0.1, 0.1, 0.01), VecBuilder.fill(0.3, 0.3, 0.6)); + + zeroYaw(); + + _odometryThread = new odometryThread(this); + _odometryThread.start(); + } + + /** + * + * @return Current Estimated State Of The Robot + * + * @see SwerveDriveState.java + */ + public SwerveDriveState getState() { + try { + this._stateLock.readLock().lock(); + return this._cachedState; + } finally { + this._stateLock.readLock().unlock(); + } + } + + /** + * + * @return Current Estimated Pose2d Of The Robot + */ + public Pose2d getPose() { + return getState().pose; + } + + /** + * Resets The Pose2d Of The Robot To newPose + * + * @param newPose new Pose2d Of The Robot + */ + public void resetPose(Pose2d newPose) { + try { + this._stateLock.writeLock().lock(); + _poseEstimator.resetPosition(getYaw(), _swervePositions, newPose); + } finally { + this._stateLock.writeLock().unlock(); + } + } + + /** + * Resets The Pose Of the Robot to 0,0,0 + */ + public void resetPose() { + resetPose(new Pose2d()); + } + + /** + * Updated The Yaw Offset Of The Robot + *

+ * Used To 0 out the Gyro When Driving + */ + public void updateYawOffset() { + yawOffset = getPose().getRotation().minus(getYaw()); + } + + /** + * Periodically called 50 Hz + *

+ * Replaces the regular periodic function + */ + public abstract void onUpdate(); + + /** + * + * @return Current Yaw Of the Gyro + * + * @see latencyAdjustedYaw() + */ + public Rotation2d getYaw() { + return _pigeon.getYaw(); + } + + /** + * + * @return Latency Adjusted Yaw Of the Gyro + */ + public Rotation2d latencyAdjustedYaw() { + return _pigeon.getLatencyAdjustedYaw(); + } + + /** + * Zeros the gyro + */ + public void zeroYaw() { + yawOffset = getYaw(); + resetPose(new Pose2d(getPose().getTranslation(), + Rotation2d.fromDegrees(Util.getAlliance() == DriverStation.Alliance.Red ? 180 : 0))); + } + + /** + * + * @return Robot Relatives Speeds + * + *

    + *
  • x is forward
    • + *
  • y is left
    • + *
  • r is counterclock wise
    • + */ + public ChassisSpeeds getRobotRelativeSpeeds() { + return getState().robotRelativeSpeeds; + } + + /** + * + * @return Array Of Modules States + */ + protected SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (SwerveModule mod : _modules) { + states[mod.moduleNumber] = mod.getState(); + } + return states; + } + + /** + * + * @return Array of Module Positions + */ + protected SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (SwerveModule mod : _modules) { + states[mod.moduleNumber] = mod.getPosition(); + } + return states; + } + + /** + * Used To drive the robot during teleop + * + * @param translation in X, Y + * @param rotation in R + * @param fieldRelative should forward be based on the field or robot + * @param isOpenLoop closed or open loop control of the drive wheels. + * Typically openLoop for teleop and closed for autonomous + */ + public void Drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { + ChassisSpeeds desiredRobotSpeeds = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + if (fieldRelative) + desiredRobotSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotSpeeds, + getYaw().plus(yawOffset).rotateBy(Rotation2d.fromDegrees((Util.getAlliance() == Alliance.Red ? 180 : 0)))); + + SwerveModuleState[] swerveModuleStates = _kinematics.toSwerveModuleStates(desiredRobotSpeeds); + + setModuleStates(swerveModuleStates, isOpenLoop); + } + + /** + * Used to drive during auto + * + * @param c Chassis Speeds object containing the desired speeds of the robot + */ + public void Drive(ChassisSpeeds c) { + SwerveModuleState[] swerveModuleStates = _kinematics.toSwerveModuleStates(c); + setModuleStates(swerveModuleStates); + } + + public void voltageDriveForward(Voltage volts) { + for (SwerveModule mod : _modules) + mod.setDesiredVoltageForward(volts); + } + + public void voltageRotateForward(Voltage volts) { + for (SwerveModule mod : _modules) + mod.setDesiredVoltageRotation(volts); + } + + /** + * Stops driving + */ + public void doNothing() { + Drive(new ChassisSpeeds()); + } + + /** + * @param desiredStates the desired states of each module, in the order of the + * kinematics (FL, FR, BL, BR) + * @param openLoop Open or closed loop control + */ + public void setModuleStates(SwerveModuleState[] desiredStates, boolean openLoop) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_VELOCITY); + + for (SwerveModule mod : _modules) + mod.setDesiredState(desiredStates[mod.moduleNumber], openLoop); + + this.swerveModuleDesiredStates = desiredStates; + } + + /** + * Using closed loop control + * + * @param desiredStates the desired states of each module, in the order of the + * kinematics (FL, FR, BL, BR) + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + setModuleStates(desiredStates, false); + } + + /** + * + * @return List Of Swerve Modules + */ + public SwerveModule[] getModules() { + return _modules; + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Translation2d position) { + return getPose().getTranslation().getDistance(position); + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Pose2d position) { + return distTo(position.getTranslation()); + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Translation2d position) { + Translation2d delta = getPose().getTranslation().minus(position); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(delta.getY(), delta.getX())); + return rot; + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Pose2d position) { + return angleTo(position.getTranslation()); + } + + /** + * Don't override periodic, override onUpdate() + * + */ + @Override + public final void periodic() { + if (DriverStation.isDisabled()) { + this.updateYawOffset(); + } + + BackupLogger.addToQueue("Swerve/ModuleStates", getModuleStates()); + + BackupLogger.addToQueue("Swerve/DesiredModuleStates", swerveModuleDesiredStates); + + BackupLogger.addToQueue("Swerve/RobotState", getState()); + + onUpdate(); + } + + /** + * Adds a vision measurement to the current estimation + * + * @param visionPose Vision Pose + * @param visionTimestamp Vision Timestamp + * @param stdDeviations Standard Deviations of vision measurement + */ + public void addVisionMeasurement(Pose2d visionPose, double visionTimestamp, Vector stdDeviations) { + try { + this._stateLock.writeLock().lock(); + + _poseEstimator.addVisionMeasurement(visionPose, visionTimestamp, stdDeviations); + } finally { + this._stateLock.writeLock().unlock(); + } + } + + class odometryThread extends Thread { + private SwerveDriveSubsystem this0; + + private BaseStatusSignal[] _allSignals; + + odometryThread(SwerveDriveSubsystem this0) { + this.this0 = this0; + + this.setName("OdometryThread"); + this.setDaemon(true); + this.setPriority(MIN_PRIORITY + 1); // Slightly Above Minimum + } + + public void run() { + _allSignals = new BaseStatusSignal[this0._modules.length * SwerveModule.SIGNAL_COUNT + 2 + 3]; + for (SwerveModule mod : this0._modules) { + + BaseStatusSignal[] signals = mod.getSignals(); + + for (int i = 0; i < SwerveModule.SIGNAL_COUNT; i++) { + _allSignals[mod.moduleNumber * SwerveModule.SIGNAL_COUNT + i] = signals[i]; + } + } + _allSignals[_allSignals.length - 2] = this0._pigeon._yawSignal; + _allSignals[_allSignals.length - 1] = this0._pigeon._yawVelocitySignal; + + _allSignals[_allSignals.length - 5] = this0._pigeon.get().getAccelerationX(); + _allSignals[_allSignals.length - 4] = this0._pigeon.get().getAccelerationY(); + _allSignals[_allSignals.length - 3] = this0._pigeon.get().getAccelerationZ(); + + BaseStatusSignal.setUpdateFrequencyForAll(250, _allSignals); + ChassisSpeeds acceleration = new ChassisSpeeds(); + + while (true) { + try { + BaseStatusSignal.refreshAll(_allSignals); + + this.this0._stateLock.writeLock().lock(); + + for (int i = 0; i < 4; ++i) { + _swervePositions[i] = _modules[i].getPosition(); + } + + ChassisSpeeds speeds = _kinematics.toChassisSpeeds(getModuleStates()); + + double time = System.currentTimeMillis(); + double timeSinceLastUpdate = time - _cachedState.timeStamp; + if (timeSinceLastUpdate == 0) + timeSinceLastUpdate = 1e-9; + + Pose2d newPose = _poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), latencyAdjustedYaw(), + _swervePositions); + + double slippage = Math.abs(speeds.omegaRadiansPerSecond - _pigeon.getAngularVelocity().in(RadiansPerSecond)) + * Math.sqrt(speeds.vxMetersPerSecond * speeds.vxMetersPerSecond + + speeds.vyMetersPerSecond * speeds.vyMetersPerSecond); + + double measuredSensorAcceleration = Math + .sqrt(Math.pow(_allSignals[_allSignals.length - 3].getValueAsDouble(), 2) + + Math.pow(_allSignals[_allSignals.length - 4].getValueAsDouble(), 2) + + Math.pow(_allSignals[_allSignals.length - 5].getValueAsDouble(), 2)); + + acceleration.vxMetersPerSecond = (speeds.vxMetersPerSecond + - _cachedState.robotRelativeSpeeds.vxMetersPerSecond) / (timeSinceLastUpdate / 1000); + acceleration.vyMetersPerSecond = (speeds.vyMetersPerSecond + - _cachedState.robotRelativeSpeeds.vyMetersPerSecond) / (timeSinceLastUpdate / 1000); + acceleration.omegaRadiansPerSecond = (speeds.omegaRadiansPerSecond + - _cachedState.robotRelativeSpeeds.omegaRadiansPerSecond) / (timeSinceLastUpdate / 1000); + + double accelerationMagnitude = acceleration.vxMetersPerSecond * acceleration.vxMetersPerSecond + + acceleration.vyMetersPerSecond * acceleration.vyMetersPerSecond; + + BackupLogger.addToQueue("sensorAcceleration", + Units.Gs.of(measuredSensorAcceleration - 1).in(Units.MetersPerSecondPerSecond)); + BackupLogger.addToQueue("acceleration", accelerationMagnitude); + + this.this0._cachedState.update( + newPose, + speeds, + acceleration, + time, + slippage); + } finally { + this.this0._stateLock.writeLock().unlock(); + } + Timer.delay(1.0 / 250.0); // Limits To 250 Hz + } + } + } +} diff --git a/src/main/java/frc/lib/Swerve/SwerveModule.java b/src/main/java/frc/lib/Swerve/SwerveModule.java new file mode 100644 index 0000000..8b1aeee --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveModule.java @@ -0,0 +1,185 @@ +package frc.team696.lib.Swerve; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.PerUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import frc.team696.lib.Util; +import frc.team696.lib.HardwareDevices.CANCoderFactory; +import frc.team696.lib.HardwareDevices.TalonFactory; + +public class SwerveModule{ + public static int s_moduleCount = 0; + public static final String[] moduleNames = {"FL", "FR", "BL", "BR"}; + + private final StatusSignal _encoderSignal; + private final StatusSignal _anglePositionSignal; + private final StatusSignal _angleVelocitySignal; + private final StatusSignal _drivePositionSignal; + private final StatusSignal _driveVelocitySignal; + + public static final int SIGNAL_COUNT = 4; + + public final int moduleNumber; + private Rotation2d _angleOffset; + + private TalonFactory _angleMotor; + private TalonFactory _driveMotor; + private CANCoderFactory _encoder; + + private Rotation2d _lastAngle; + + private final SimpleMotorFeedforward _driveFeedForward = new SimpleMotorFeedforward(SwerveConstants.drivekS.in(Volts), SwerveConstants.drivekV.in(PerUnit.combine(Volts, MetersPerSecond)), SwerveConstants.drivekA.in(PerUnit.combine(Volts,MetersPerSecondPerSecond))); + + private final VelocityVoltage _driveVelocity = new VelocityVoltage(0); + + private final PositionVoltage _anglePosition = new PositionVoltage(0); + + private final double[] _sysidRotationPositions = new double[] {135, -135, -45, 45}; + + + public SwerveModule(SwerveModuleConstants moduleConstants){ + this.moduleNumber = s_moduleCount++; + this._angleOffset = Rotation2d.fromRotations(moduleConstants.EncoderOffset); + /* Angle Encoder Config */ + _encoder = new CANCoderFactory(moduleConstants.EncoderId, SwerveConstants.canBus, SwerveConfigs.canCoder, "Swerve Encoder " + moduleNumber); + + _encoderSignal = ( _encoder.get().getAbsolutePosition()); + + /* Angle Motor Config */ + _angleMotor = new TalonFactory(moduleConstants.SteerMotorId, SwerveConstants.canBus, SwerveConfigs.angle, "Swerve Angle " + moduleNumber); + resetToAbsolute(); + + /* Drive Motor Config */ + _driveMotor = new TalonFactory(moduleConstants.DriveMotorId, SwerveConstants.canBus, SwerveConfigs.drive, "Swerve Drive " + moduleNumber); + + _anglePositionSignal = (_angleMotor.get().getPosition()); + _angleVelocitySignal = (_angleMotor.get().getVelocity()); + _drivePositionSignal = (_driveMotor.get().getPosition()); + _driveVelocitySignal = (_driveMotor.get().getVelocity()); + + BaseStatusSignal.setUpdateFrequencyForAll(100, _encoderSignal, _anglePositionSignal, _angleVelocitySignal, _drivePositionSignal, _driveVelocitySignal); + + ParentDevice.optimizeBusUtilizationForAll(_encoder.get(), _angleMotor.get(), _driveMotor.get()); + + _lastAngle = getState().angle; + } + + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){ + desiredState.optimize(getState().angle); + desiredState.cosineScale(getState().angle); + Rotation2d angle = ((Math.abs(desiredState.speedMetersPerSecond) <= (SwerveConstants.MAX_VELOCITY.in(Units.MetersPerSecond) * 0.01)) ? _lastAngle : desiredState.angle); + _angleMotor.setControl(_anglePosition.withPosition(angle.getRotations())); + setSpeed(desiredState, isOpenLoop); + _lastAngle = angle; + } + + // For Use In SysID + public void setDesiredVoltageForward(Voltage volts) { + _driveMotor.VoltageOut(volts); + _angleMotor.setControl(_anglePosition.withPosition(0)); + } + public void setDesiredVoltageRotation(Voltage volts) { + _driveMotor.VoltageOut(volts); + _angleMotor.setControl(_anglePosition.withPosition(_sysidRotationPositions[moduleNumber])); + } + public Voltage getDriveOutputVoltage() { + return Volts.of(_driveMotor.get().get() * RobotController.getBatteryVoltage()); + } + + private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ + if(isOpenLoop){ + _driveMotor.VoltageOut(Volts.of(desiredState.speedMetersPerSecond / SwerveConstants.MAX_VELOCITY.in(Units.MetersPerSecond)*12)); + } else { + _driveVelocity.Velocity = Util.MPSToRPS(desiredState.speedMetersPerSecond, SwerveConstants.WHEEL_CIRCUM.in(Units.Meters)); + _driveVelocity.FeedForward = _driveFeedForward.calculate(desiredState.speedMetersPerSecond); + _driveMotor.setControl(_driveVelocity); + } + } + + public Rotation2d getCANCoderAngle(boolean refresh){ + if (refresh) { + _encoderSignal.refresh(); + } + + return Rotation2d.fromRotations(_encoderSignal.getValueAsDouble()); + } + + public Rotation2d getCANCoderAngle(){ + return getCANCoderAngle(true); + } + + public void resetToAbsolute(){ + Rotation2d absolutePosition = getCANCoderAngle(true).minus(_angleOffset); + _angleMotor.setPosition(absolutePosition.getRotations()); + } + + public SwerveModuleState getState(boolean refresh){ + if (refresh) { + _driveVelocitySignal.refresh(); + } + return new SwerveModuleState( + Util.RPSToMPS(_driveVelocitySignal.getValueAsDouble(), SwerveConstants.WHEEL_CIRCUM.in(Units.Meters)), + Rotation2d.fromRotations(getAngleMotorPosition(refresh).in(Rotations)) + ); + } + + public SwerveModuleState getState(){ + return getState(false); + } + + public SwerveModulePosition getPosition(){ + return new SwerveModulePosition( + Util.rotationsToMeters(getDriveMotorPosition().in(Rotations), SwerveConstants.WHEEL_CIRCUM.in(Units.Meters)), + Rotation2d.fromRotations(getAngleMotorPosition().in(Rotations)) + ); + } + + public Angle getDriveMotorPosition(boolean refresh) { + if (refresh) { + _drivePositionSignal.refresh(); + _driveVelocitySignal.refresh(); + } + return Rotations.of(BaseStatusSignal.getLatencyCompensatedValue(_drivePositionSignal, _driveVelocitySignal).magnitude()); + } + + public Angle getDriveMotorPosition() { + return getDriveMotorPosition(false); + } + + public Angle getAngleMotorPosition(boolean refresh) { + if (refresh) { + _anglePositionSignal.refresh(); + _angleVelocitySignal.refresh(); + } + return Rotations.of(BaseStatusSignal.getLatencyCompensatedValue(_anglePositionSignal, _angleVelocitySignal).magnitude()); + } + + public Angle getAngleMotorPosition() { + return getAngleMotorPosition(false); + } + + public BaseStatusSignal[] getSignals(){ + return new BaseStatusSignal[] {_anglePositionSignal, _angleVelocitySignal, _drivePositionSignal, _driveVelocitySignal, _encoderSignal}; + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Util.java b/src/main/java/frc/lib/Util.java new file mode 100644 index 0000000..4a7b1ea --- /dev/null +++ b/src/main/java/frc/lib/Util.java @@ -0,0 +1,208 @@ +package frc.team696.lib; + +import java.io.IOException; +import java.net.NetworkInterface; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Enumeration; +import java.util.HashMap; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Function; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.team696.lib.Logging.PLog; + +/** + * Useful functions which don't fit anywhere specific + */ +public class Util { + + /** + * Linear Interpolates between two numbers + * + * @param t Value from [0,1] + * @param min Lower value --> 0 + * @param max Upper value --> 1 + * @return Interpolated value between min and max + */ + public static double lerp(double t, double min, double max) { + return (max - min) * t + min; + } + + /** + * @param val Value to be clamped + * @param min Minimum Value + * @param max Maximum Value + * @return Value Clamped between min and max + */ + public static double clamp(double val, double min, double max) { + return Math.max(Math.min(max, val), min); + } + + public static double map(double x, double in_min, double in_max, double out_min, double out_max){ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + } + + /** + * @return Current Alliance through driverstation or FMS. Defaults to Blue. + */ + public static Alliance getAlliance() { + if (DriverStation.getAlliance().isPresent()) { + return DriverStation.getAlliance().get(); + } else { + return Alliance.Blue; + } + } + + /** + * @param wheelRPS Wheel Velocity: (in Rotations per Second) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Velocity: (in Meters per Second) + */ + public static double RPSToMPS(double wheelRPS, double circumference){ + double wheelMPS = wheelRPS * circumference; + return wheelMPS; + } + + /** + * @param wheelMPS Wheel Velocity: (in Meters per Second) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Velocity: (in Rotations per Second) + */ + public static double MPSToRPS(double wheelMPS, double circumference){ + double wheelRPS = wheelMPS / circumference; + return wheelRPS; + } + + /** + * @param wheelRotations Wheel Position: (in Rotations) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Distance: (in Meters) + */ + public static double rotationsToMeters(double wheelRotations, double circumference){ + double wheelMeters = wheelRotations * circumference; + return wheelMeters; + } + + /** + * @param wheelMeters Wheel Distance: (in Meters) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Position: (in Rotations) + */ + public static double metersToRotations(double wheelMeters, double circumference){ + double wheelRotations = wheelMeters / circumference; + return wheelRotations; + } + + /** + * + * @return List of connected MAC Addresses + * @throws IOException + */ + public static List getMacAddresses() throws IOException { + List macAddresses = new ArrayList<>(); + + Enumeration networkInterfaces = NetworkInterface.getNetworkInterfaces(); + NetworkInterface networkInterface; + while (networkInterfaces.hasMoreElements()) { + networkInterface = networkInterfaces.nextElement(); + + byte[] address = networkInterface.getHardwareAddress(); + if (address == null) { + continue; + } + + macAddresses.add(address); + } + return macAddresses; + } + + public static String macToString(byte[] address) { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < address.length; i++) { + if (i != 0) { + builder.append(':'); + } + builder.append(String.format("%02X", address[i])); + } + return builder.toString(); + } + + /** Returns the current detected robot based on map of robot name to mac address + *

    + *

      + *
    • -1 is simulation
      • + *
    • 0 is unknown
      • + *
    • 1 is first item in map... etc.
      • + *
    + *

    + * verbose will print to console, which robot connected, or the MAC if unknown + * + *

    + * ex: (RobotInit()): Util.setRobotType(new LinkedHashMap<>() { }, true); + */ + public static int setRobotType(LinkedHashMap nameToMac, boolean verbose) { + if (RobotBase.isSimulation()) { + if (verbose) { + PLog.info("Robot", "Simulation Detected"); + } + return -1; + } + + List macAddresses; + try { + macAddresses = Util.getMacAddresses(); + } catch (IOException e) { + PLog.fatalException("Robot", "Mac Address Attempt Unsuccessful", e); + macAddresses = List.of(); + return 0; + } + + for (byte[] macAddress : macAddresses) { + int i = 1; + for (Map.Entry robotEntry : nameToMac.entrySet()) { + if (Arrays.compare(robotEntry.getValue(), macAddress) == 0) { + if (verbose) { + PLog.info("Robot", String.format("%s Detected", robotEntry.getKey())); + } + return i; + } + i++; + } + } + + if (verbose) { + PLog.info("Robot", "Unknown Robot Detected"); + for (byte[] macAddress : macAddresses) { + PLog.info(" ", Util.macToString(macAddress)); + } + } + return 0; + } + public static Map transformMap(Map originalMap, Function transformer) { + Map resultMap = new HashMap<>(); + for (Map.Entry entry : originalMap.entrySet()) { + resultMap.put(entry.getKey(), transformer.apply(entry.getValue())); + } + return resultMap; + } + + public static Map> transformNestedMap( + Map> originalMap, Function transformer) { + Map> resultMap = new HashMap<>(); + + for (Map.Entry> outerEntry : originalMap.entrySet()) { + Map innerResultMap = new HashMap<>(); + for (Map.Entry innerEntry : outerEntry.getValue().entrySet()) { + innerResultMap.put(innerEntry.getKey(), transformer.apply(innerEntry.getValue())); + } + resultMap.put(outerEntry.getKey(), innerResultMap); + } + + return resultMap; + } +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 434f5cd..d9f8f48 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 71; - public static final String GIT_SHA = "6a31eb8e1fb82df01ab88bd7937a7c5a50413b5c"; - public static final String GIT_DATE = "2025-10-01 13:53:19 PDT"; + public static final int GIT_REVISION = 72; + public static final String GIT_SHA = "35eb49f4c59a322a36bed9578e58c02cd244ab9c"; + public static final String GIT_DATE = "2025-10-03 22:02:18 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-10-03 21:45:33 PDT"; - public static final long BUILD_UNIX_TIME = 1759553133948L; + public static final String BUILD_DATE = "2025-10-06 17:04:04 PDT"; + public static final long BUILD_UNIX_TIME = 1759795444883L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b25bb8f..8b96040 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -172,6 +172,9 @@ public Robot() { }))); NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); + + NamedCommands.registerCommand("L1", new AutoMoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back),-0.6,0.0,true).asProxy()); + NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); NamedCommands.registerCommand("AfterIntake", diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 deleted file mode 160000 index a985f86..0000000 --- a/src/main/java/frc/team696 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a985f86cfae3df59e09714ec1704636f6853e11a From ac4eefe6d687d83e3dfd98318c0bf391b5d91312 Mon Sep 17 00:00:00 2001 From: atoorian Date: Mon, 13 Oct 2025 14:34:00 -0700 Subject: [PATCH 37/37] canged max v back to 3 and max a back to 2.4 --- src/main/deploy/pathplanner/settings.json | 4 ++-- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 9e8028d..e20e2f0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,8 +8,8 @@ "RightSide" ], "autoFolders": [], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 2.5, + "defaultMaxVel": 3, + "defaultMaxAccel": 2.4, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d9f8f48..dfd3494 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 72; - public static final String GIT_SHA = "35eb49f4c59a322a36bed9578e58c02cd244ab9c"; - public static final String GIT_DATE = "2025-10-03 22:02:18 PDT"; + public static final int GIT_REVISION = 73; + public static final String GIT_SHA = "23d68b0cc9b2b5f702ed325f4ee68414f572fd28"; + public static final String GIT_DATE = "2025-10-06 18:41:54 PDT"; public static final String GIT_BRANCH = "postLA"; - public static final String BUILD_DATE = "2025-10-06 17:04:04 PDT"; - public static final long BUILD_UNIX_TIME = 1759795444883L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2025-10-13 14:32:18 PDT"; + public static final long BUILD_UNIX_TIME = 1760391138186L; + public static final int DIRTY = 0; private BuildConstants(){} }