From 0d38f0a0ebd235e69dbea93a48def65819519faa Mon Sep 17 00:00:00 2001 From: DriverStation3 <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 29 May 2025 20:04:09 -0700 Subject: [PATCH 01/19] Added Mechanism2D display of claw, arm pivot, and elevator --- src/main/java/frc/robot/Robot.java | 22 ++++++++++++++++++- .../java/frc/robot/subsystems/ArmPivot.java | 6 +++++ .../robot/subsystems/ElevatorSubsystem.java | 5 +++++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a401ee3b..a4ab10a2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,8 @@ import au.grapplerobotics.CanBridge; import com.pathplanner.lib.commands.FollowPathCommand; + +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; @@ -14,7 +16,13 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Subsystems.SubsystemConstants; import frc.robot.subsystems.SuperStructure; @@ -37,11 +45,15 @@ public static Robot getInstance() { private final RobotType robotType; public final Controls controls; public final Subsystems subsystems; - public final Sensors sensors; public final SuperStructure superStructure; private final PowerDistribution PDH; + Mechanism2d mech; + MechanismRoot2d root; + MechanismLigament2d m_elevator; + MechanismLigament2d m_wrist; + protected Robot() { // non public for singleton. Protected so test class can subclass @@ -51,6 +63,12 @@ protected Robot() { PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); + mech = new Mechanism2d(1,2); + root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5)); + SmartDashboard.putData("Mechanism", mech); + m_elevator = root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed))); + var pivot = m_elevator.append(new MechanismLigament2d("pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed))); + m_wrist = pivot.append(new MechanismLigament2d("wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed))); sensors = new Sensors(); subsystems = new Subsystems(sensors); @@ -115,6 +133,8 @@ public void robotPeriodic() { if (subsystems.visionSubsystem != null) { subsystems.visionSubsystem.update(); } + m_elevator.setLength(subsystems.elevatorSubsystem.getHeightMeters()); + m_wrist.setAngle(subsystems.armPivotSubsystem.getAngle()); } @Override diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 3755ca3a..f1386df9 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; @@ -14,6 +15,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; @@ -128,6 +130,10 @@ private double getCurrentPosition() { var curPos = motor.getPosition(); return curPos.getValueAsDouble(); } + public Rotation2d getAngle() { // 0 is forward, + is up + return Rotation2d.fromRotations(getCurrentPosition()); + + } // preset command placeholder public Command moveToPosition(double position) { diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index bf7c6140..6b4fea25 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -52,6 +52,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; + private static final double MOTOR_ROTATIONS_PER_METER = 40; //Inaccurate public static final double MANUAL = 0.1; private static final double POS_TOLERANCE = 0.1; private final double ELEVATOR_KP = 7.804; @@ -242,6 +243,10 @@ private double getCurrentPosition() { return curPos; } + public double getHeightMeters() { // Elevator height converted to Meters + return getCurrentPosition() / MOTOR_ROTATIONS_PER_METER; + } + private void setCurrentPosition(double pos) { m_motor.setPosition(pos); } From 517f0d6fd3968cb1aec03fde696da272475907b6 Mon Sep 17 00:00:00 2001 From: DriverStation3 <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 29 May 2025 20:10:56 -0700 Subject: [PATCH 02/19] Spotless --- src/main/java/frc/robot/Robot.java | 17 +++++++++++------ .../java/frc/robot/subsystems/ArmPivot.java | 3 +-- .../frc/robot/subsystems/ElevatorSubsystem.java | 2 +- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a4ab10a2..a114836f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,6 @@ import au.grapplerobotics.CanBridge; import com.pathplanner.lib.commands.FollowPathCommand; - import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -16,7 +15,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -63,12 +61,19 @@ protected Robot() { PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); - mech = new Mechanism2d(1,2); + mech = new Mechanism2d(1, 2); root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5)); SmartDashboard.putData("Mechanism", mech); - m_elevator = root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed))); - var pivot = m_elevator.append(new MechanismLigament2d("pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed))); - m_wrist = pivot.append(new MechanismLigament2d("wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed))); + m_elevator = + root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed))); + var pivot = + m_elevator.append( + new MechanismLigament2d( + "pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed))); + m_wrist = + pivot.append( + new MechanismLigament2d( + "wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed))); sensors = new Sensors(); subsystems = new Subsystems(sensors); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index f1386df9..d78504f0 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -1,6 +1,5 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; @@ -130,9 +129,9 @@ private double getCurrentPosition() { var curPos = motor.getPosition(); return curPos.getValueAsDouble(); } + public Rotation2d getAngle() { // 0 is forward, + is up return Rotation2d.fromRotations(getCurrentPosition()); - } // preset command placeholder diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 6b4fea25..742fef5d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -52,7 +52,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; - private static final double MOTOR_ROTATIONS_PER_METER = 40; //Inaccurate + private static final double MOTOR_ROTATIONS_PER_METER = 40; // Inaccurate public static final double MANUAL = 0.1; private static final double POS_TOLERANCE = 0.1; private final double ELEVATOR_KP = 7.804; From 83839ad466419af1e7bfb8041cbb56bfeb111aea Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 2 Oct 2025 18:46:04 -0700 Subject: [PATCH 03/19] Added mechanism config file to robot code for easy access WIP --- .../java/frc/robot/subsystems/config.json | 144 ++++++++++++++++++ 1 file changed, 144 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/config.json diff --git a/src/main/java/frc/robot/subsystems/config.json b/src/main/java/frc/robot/subsystems/config.json new file mode 100644 index 00000000..93f75c4e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/config.json @@ -0,0 +1,144 @@ +{ + "name": "PelicanAuburnV2", + "disableSimplification": true, + "rotations": [ + {"axis":"x", "degrees": 90}, + {"axis": "z", "degrees": -90} + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [], + "components": [ { + "_comment": "model_0", + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { "_comment": "model_1", + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { "_comment": "model_2", + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + + { "_comment": "model_3", + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { "_comment": "model_4", + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, { "_comment": "model_5", + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + } + + + ] +} \ No newline at end of file From 5c5fb1f2dc569c79e1cbe5bea0385f26762437fd Mon Sep 17 00:00:00 2001 From: DriverStation3 <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 2 Oct 2025 19:33:58 -0700 Subject: [PATCH 04/19] Added networktables entry for elevator pose3d WIP --- .../robot/subsystems/ElevatorSubsystem.java | 13 +- .../java/frc/robot/subsystems/config.json | 144 ------------------ 2 files changed, 12 insertions(+), 145 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/config.json diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 7217e2db..dc5e81ef 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -9,6 +9,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +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.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Voltage; @@ -91,7 +96,8 @@ public class ElevatorSubsystem extends SubsystemBase { new Alert("Elevator", "Motor 2 not connected", AlertType.kError); private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth); private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); - + private final StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); + // Creates a SysIdRoutine SysIdRoutine routine = new SysIdRoutine( @@ -108,6 +114,8 @@ public ElevatorSubsystem() { motorConfigs(); Shuffleboard.getTab("Elevator").addDouble("Motor Current Position", () -> getCurrentPosition()); + //Elevator pose test + Shuffleboard.getTab("Elevator").addDouble("Target Position", () -> getTargetPosition()); Shuffleboard.getTab("Elevator") .addDouble("M1 supply current", () -> m_motor.getSupplyCurrent().getValueAsDouble()); @@ -134,6 +142,7 @@ public ElevatorSubsystem() { "M2 at reverse softstop", () -> m_motor2.getFault_ReverseSoftLimit().getValue()); Shuffleboard.getTab("Elevator") .addDouble("Elevator Speed", () -> m_motor.getVelocity().getValueAsDouble()); + } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -375,6 +384,8 @@ public void periodic() { if (RobotBase.isSimulation()) { m_motorOneSimState.setRawRotorPosition(targetPos); m_motorTwoSimState.setRawRotorPosition(targetPos); + + elevatorPose3d.set(new Pose3d(new Pose2d(0.0, getHeightMeters(), new Rotation2d(0.0,0.0)))); } } } diff --git a/src/main/java/frc/robot/subsystems/config.json b/src/main/java/frc/robot/subsystems/config.json deleted file mode 100644 index 93f75c4e..00000000 --- a/src/main/java/frc/robot/subsystems/config.json +++ /dev/null @@ -1,144 +0,0 @@ -{ - "name": "PelicanAuburnV2", - "disableSimplification": true, - "rotations": [ - {"axis":"x", "degrees": 90}, - {"axis": "z", "degrees": -90} - ], - "position": [ - 0, - 0, - 0 - ], - "cameras": [], - "components": [ { - "_comment": "model_0", - "zeroedRotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 0 - } - ], - "zeroedPosition": [ - 0, - 0, - 0 - ] - }, - { "_comment": "model_1", - "zeroedRotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 0 - } - ], - "zeroedPosition": [ - 0, - 0, - 0 - ] - }, - { "_comment": "model_2", - "zeroedRotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 0 - } - ], - "zeroedPosition": [ - 0, - 0, - 0 - ] - }, - - { "_comment": "model_3", - "zeroedRotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 0 - } - ], - "zeroedPosition": [ - 0, - 0, - 0 - ] - }, - { "_comment": "model_4", - "zeroedRotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 0 - } - ], - "zeroedPosition": [ - 0, - 0, - 0 - ] - }, { "_comment": "model_5", - "zeroedRotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 0 - } - ], - "zeroedPosition": [ - 0, - 0, - 0 - ] - } - - - ] -} \ No newline at end of file From 7a86d58709aa6b52cf3a7a3450cab8d3dab93bde Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 08:56:02 -0700 Subject: [PATCH 05/19] elastic camera fix --- src/main/deploy/elastic-layout.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 54ccf5f3..7e3a60b3 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -100,7 +100,7 @@ "height": 512.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/left", + "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", "period": 0.06, "rotation_turns": 0 } @@ -113,7 +113,7 @@ "height": 512.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/right", + "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", "period": 0.06, "rotation_turns": 0 } From c8fcedc0e2d4372629db33a51b9d79a6c620dda7 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 09:12:29 -0700 Subject: [PATCH 06/19] autoScore stuff --- src/main/java/frc/robot/Controls.java | 6 +- .../frc/robot/subsystems/SuperStructure.java | 83 +++++++++++++++++++ 2 files changed, 86 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 2ba35c91..8eb77a0c 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -463,13 +463,13 @@ private Command getSoloCoralBranchHeightCommand() { .coralLevelFour(soloController.rightBumper()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper()) + .coralLevelThree(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper()) + .coralLevelTwo(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper()) + .coralLevelOne(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..d7dc5092 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -67,6 +67,17 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) } } + private Command repeatPrescoreScoreSwing( + Command command, BooleanSupplier score, BooleanSupplier ipScore) { + // repeats scoring sequence if the coral is still in the claw + if (armSensor == null) { + return Commands.sequence( + command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score)); + } else { + return command.repeatedly().onlyWhile(armSensor.inClaw()); + } + } + public Command coralLevelFour(BooleanSupplier score) { if (branchSensors != null) { // checks if sensor enabled then use for faster scoring score = branchSensors.withinScoreRange().or(score); @@ -168,6 +179,78 @@ public Command coralLevelOne(BooleanSupplier score) { .withName("Coral Level 1"); } + public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.repeatingSequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .withTimeout(1.5) + .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + score, + ipScore), + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) + .withName("Coral Level 3"); + } + + public Command coralLevelTwo( + BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.sequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L2) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .withTimeout(1.5) + .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + score, + ipScore), + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) + .withName("Coral Level 2"); + } + + public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { + return Commands.sequence( + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), + spinnyClaw.stop()) // holds coral without wearing flywheels + .withTimeout(0.5) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral + Commands.waitSeconds(1), // Wait to clear the reef + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) + .withName("Coral Level 1"); + } + // quickGroundIntake() is used instead since it's faster and still reliable. // (This one moves the coral intake the normal intake position, then does the normal intake. // quickGroundIntake() instead hands the coral directly to the claw.) From 1014c22fbde2c2ec2f1d11347244cffbaa5ef34d Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 09:51:18 -0700 Subject: [PATCH 07/19] l1 auto align --- src/main/java/frc/robot/Controls.java | 10 ++++ .../frc/robot/subsystems/auto/AutoAlign.java | 55 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 8eb77a0c..51a8a39c 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -969,6 +969,11 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -1006,6 +1011,11 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); // Ground Intake soloController .leftBumper() diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 0f0ad2d4..757e6287 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,6 +23,10 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } + public static Command autoAlignL1(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1(drivebaseSubsystem, controls).withName("Auto Align"); + } + public static Command autoAlignLeft( CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandLeft(drivebaseSubsystem, controls).withName("Auto Align"); @@ -138,6 +142,31 @@ public static boolean isCloseEnough() { private static final Pose2d redBranchL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d(); + private static final Pose2d blueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d(); + private static final Pose2d blueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d(); + private static final Pose2d blueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d(); + private static final Pose2d blueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d(); + private static final Pose2d blueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d(); + + private static final Pose2d redReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d(); + private static final Pose2d redReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d(); + private static final Pose2d redReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d(); + private static final Pose2d redReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d(); + private static final Pose2d redReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d(); + private static final Pose2d redReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d(); private static final List blueBranchPoses = List.of( blueBranchA, @@ -294,4 +323,30 @@ public void initialize() { pidRotate.setSetpoint(branchPose.getRotation().getRadians()); } } + + private static class AutoAlignCommandL1 extends AutoAlignCommand { + private static final List blueReefFaces = + List.of(blueReefFaceAB, blueReefFaceCD, blueReefFaceEF, blueReefFaceGH, blueReefFaceIJ, blueReefFaceKL); + + private static final List redReefFaces = + List.of(redReefFaceAB, redReefFaceCD, redReefFaceEF, redReefFaceGH, redReefFaceIJ, redReefFaceKL); + + public static Pose2d getNearestReefFace(Pose2d p) { + List branchPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(branchPose2ds); + } + + public AutoAlignCommandL1(CommandSwerveDrivetrain drive, Controls controls) { + super(drive, controls); + } + + @Override + public void initialize() { + Pose2d robotPose = drive.getState().Pose; + branchPose = getNearestReefFace(robotPose); + pidX.setSetpoint(branchPose.getX()); + pidY.setSetpoint(branchPose.getY()); + pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + } + } } From 0d2405ebd833eb64d03ca512cf682ec13a6d246c Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 09:53:49 -0700 Subject: [PATCH 08/19] ee --- src/main/java/frc/robot/Controls.java | 12 ++++++------ .../java/frc/robot/subsystems/auto/AutoAlign.java | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 8eb77a0c..3de11c8d 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -463,13 +463,13 @@ private Command getSoloCoralBranchHeightCommand() { .coralLevelFour(soloController.rightBumper()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } @@ -1095,11 +1095,11 @@ private void configureSoloControllerBindings() { .startMovingVoltage( () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) .withName("Elevator Manual Control")); - // Ready to score rumble - Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace()); + // Ready to score rumble (Removal for autoscoring) + /* Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace()); readyToScore.onTrue( Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0))); readyToScore.onFalse( - Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); + Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); */ } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 0f0ad2d4..35758bf7 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -83,10 +83,10 @@ public static boolean isCloseEnough() { // left and right offsets from the april tags () private static final Transform2d leftOfTag = new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(36 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); private static final Transform2d rightOfTag = new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(36 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); From e00f8279b09a2908d8e709852aa728a650763ae8 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 10:12:22 -0700 Subject: [PATCH 09/19] c --- src/main/java/frc/robot/Controls.java | 2 +- .../frc/robot/subsystems/SuperStructure.java | 2 +- .../frc/robot/subsystems/auto/AutoAlign.java | 50 ++++++++++++------- 3 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 6330f955..f3c2b3ef 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -970,7 +970,7 @@ private void configureSoloControllerBindings() { .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); soloController - .rightTrigger() + .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d7dc5092..0e4e28ea 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -244,7 +244,7 @@ public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral + spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 19f16e5f..b76bc5e6 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -87,10 +87,12 @@ public static boolean isCloseEnough() { // left and right offsets from the april tags () private static final Transform2d leftOfTag = new Transform2d( - Units.inchesToMeters(36 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(34 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); private static final Transform2d rightOfTag = new Transform2d( - Units.inchesToMeters(36 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + private static final Transform2d middleOfReef = + new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -143,30 +145,30 @@ public static boolean isCloseEnough() { aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); private static final Pose2d blueReefFaceAB = - aprilTagFieldLayout.getTagPose(18).get().toPose2d(); + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceCD = - aprilTagFieldLayout.getTagPose(17).get().toPose2d(); + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceEF = - aprilTagFieldLayout.getTagPose(22).get().toPose2d(); + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceGH = - aprilTagFieldLayout.getTagPose(21).get().toPose2d(); + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceIJ = - aprilTagFieldLayout.getTagPose(20).get().toPose2d(); + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceKL = - aprilTagFieldLayout.getTagPose(19).get().toPose2d(); + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceAB = - aprilTagFieldLayout.getTagPose(7).get().toPose2d(); + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceCD = - aprilTagFieldLayout.getTagPose(8).get().toPose2d(); + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceEF = - aprilTagFieldLayout.getTagPose(9).get().toPose2d(); + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceGH = - aprilTagFieldLayout.getTagPose(10).get().toPose2d(); + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceIJ = - aprilTagFieldLayout.getTagPose(11).get().toPose2d(); + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceKL = - aprilTagFieldLayout.getTagPose(6).get().toPose2d(); + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(middleOfReef); private static final List blueBranchPoses = List.of( blueBranchA, @@ -326,14 +328,26 @@ public void initialize() { private static class AutoAlignCommandL1 extends AutoAlignCommand { private static final List blueReefFaces = - List.of(blueReefFaceAB, blueReefFaceCD, blueReefFaceEF, blueReefFaceGH, blueReefFaceIJ, blueReefFaceKL); + List.of( + blueReefFaceAB, + blueReefFaceCD, + blueReefFaceEF, + blueReefFaceGH, + blueReefFaceIJ, + blueReefFaceKL); private static final List redReefFaces = - List.of(redReefFaceAB, redReefFaceCD, redReefFaceEF, redReefFaceGH, redReefFaceIJ, redReefFaceKL); + List.of( + redReefFaceAB, + redReefFaceCD, + redReefFaceEF, + redReefFaceGH, + redReefFaceIJ, + redReefFaceKL); public static Pose2d getNearestReefFace(Pose2d p) { - List branchPose2ds = isBlue() ? blueReefFaces : redReefFaces; - return p.nearest(branchPose2ds); + List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(reefFacesPose2ds); } public AutoAlignCommandL1(CommandSwerveDrivetrain drive, Controls controls) { From 088c91719e95f31fca68d9498bd860d081b812b8 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 10:37:42 -0700 Subject: [PATCH 10/19] super jank code to get auto l1 auto extake --- src/main/java/frc/robot/Controls.java | 29 +++- .../frc/robot/subsystems/auto/AutoAlign.java | 146 ++++++++++++++---- 2 files changed, 141 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index f3c2b3ef..a616dda5 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -457,7 +457,7 @@ private Command getCoralBranchHeightCommand() { }; } - private Command getSoloCoralBranchHeightCommand() { + private Command getSoloCoralBranchHeightCommandL() { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) @@ -469,7 +469,24 @@ private Command getSoloCoralBranchHeightCommand() { .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } + + private Command getSoloCoralBranchHeightCommandR() { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } @@ -926,7 +943,7 @@ private void configureSoloControllerBindings() { switch (soloScoringMode) { case CORAL_IN_CLAW -> { scoreCommand = - getSoloCoralBranchHeightCommand() + getSoloCoralBranchHeightCommandL() .until( () -> soloController.a().getAsBoolean() @@ -973,7 +990,7 @@ private void configureSoloControllerBindings() { .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlignL1L(s.drivebaseSubsystem, this)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -982,7 +999,7 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand() + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommandR() .until( () -> soloController.a().getAsBoolean() @@ -1015,7 +1032,7 @@ private void configureSoloControllerBindings() { .rightTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlignL1R(s.drivebaseSubsystem, this)); // Ground Intake soloController .leftBumper() diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index b76bc5e6..43fe7cf6 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,8 +23,12 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandL1(drivebaseSubsystem, controls).withName("Auto Align"); + public static Command autoAlignL1L(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); + } + + public static Command autoAlignL1R(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); } public static Command autoAlignLeft( @@ -56,6 +60,14 @@ public static boolean poseInPlace() { return isStationary() && isCloseEnough(); } + public static boolean poseInPlaceL1L() { + return isStationary() && isCloseEnoughL1L(); + } + + public static boolean poseInPlaceL1R() { + return isStationary() && isCloseEnoughL1R(); + } + public static boolean isStationary() { var speeds = AutoLogic.s.drivebaseSubsystem.getState().Speeds; return MathUtil.isNear(0, speeds.vxMetersPerSecond, 0.01) @@ -75,6 +87,18 @@ public static boolean isCloseEnough() { return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } + public static boolean isCloseEnoughL1L() { + var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var branchPose = AutoAlignCommandL1L.getNearestReefFace(currentPose); + return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + } + + public static boolean isCloseEnoughL1R() { + var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var branchPose = AutoAlignCommandL1R.getNearestReefFace(currentPose); + return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + } + public static boolean oneSecondLeft() { // THIS WILL ONLY WORK ON THE REAL FIELD AND IN PRACTICE MODE! @@ -93,6 +117,7 @@ public static boolean isCloseEnough() { Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Transform2d middleOfReef = new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = new Transform2d(Units.inchesToMeters(34/2), Units.inchesToMeters(13.5/2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -144,31 +169,58 @@ public static boolean isCloseEnough() { private static final Pose2d redBranchL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); - private static final Pose2d blueReefFaceAB = + private static final Pose2d lBlueReefFaceAB = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceCD = + private static final Pose2d lBlueReefFaceCD = aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceEF = + private static final Pose2d lBlueReefFaceEF = aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceGH = + private static final Pose2d lBlueReefFaceGH = aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceIJ = + private static final Pose2d lBlueReefFaceIJ = aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceKL = + private static final Pose2d lBlueReefFaceKL = aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceAB = + private static final Pose2d rBlueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(l1RightOfReef); + + private static final Pose2d lRedReefFaceAB = aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceCD = + private static final Pose2d lRedReefFaceCD = aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceEF = + private static final Pose2d lRedReefFaceEF = aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceGH = + private static final Pose2d lRedReefFaceGH = aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceIJ = + private static final Pose2d lRedReefFaceIJ = aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceKL = + private static final Pose2d lRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(middleOfReef); + + private static final Pose2d rRedReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); + private static final List blueBranchPoses = List.of( blueBranchA, @@ -326,31 +378,69 @@ public void initialize() { } } - private static class AutoAlignCommandL1 extends AutoAlignCommand { + private static class AutoAlignCommandL1L extends AutoAlignCommand { + private static final List blueReefFaces = + List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL); + + private static final List redReefFaces = + List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); + + public static Pose2d getNearestReefFace(Pose2d p) { + List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(reefFacesPose2ds); + } + + public AutoAlignCommandL1L(CommandSwerveDrivetrain drive, Controls controls) { + super(drive, controls); + } + + @Override + public void initialize() { + Pose2d robotPose = drive.getState().Pose; + branchPose = getNearestReefFace(robotPose); + pidX.setSetpoint(branchPose.getX()); + pidY.setSetpoint(branchPose.getY()); + pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + } + } + + private static class AutoAlignCommandL1R extends AutoAlignCommand { private static final List blueReefFaces = List.of( - blueReefFaceAB, - blueReefFaceCD, - blueReefFaceEF, - blueReefFaceGH, - blueReefFaceIJ, - blueReefFaceKL); + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL); private static final List redReefFaces = List.of( - redReefFaceAB, - redReefFaceCD, - redReefFaceEF, - redReefFaceGH, - redReefFaceIJ, - redReefFaceKL); + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); public static Pose2d getNearestReefFace(Pose2d p) { List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; return p.nearest(reefFacesPose2ds); } - public AutoAlignCommandL1(CommandSwerveDrivetrain drive, Controls controls) { + public AutoAlignCommandL1R(CommandSwerveDrivetrain drive, Controls controls) { super(drive, controls); } From e1b832af1657e7c9197c20291518ed1e1f37a492 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 15:15:04 -0700 Subject: [PATCH 11/19] c --- src/main/java/frc/robot/Controls.java | 9 ++------- .../java/frc/robot/subsystems/SuperStructure.java | 4 +++- .../java/frc/robot/subsystems/auto/AutoAlign.java | 12 ++++++++---- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index a616dda5..b0b3fd96 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -371,7 +371,7 @@ private void configureSuperStructureBindings() { .and(RobotModeTriggers.teleop()) .onTrue( superStructure - .coralIntake() + .coralIntake().alongWith(Commands.runOnce(() -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW)) .alongWith( s.elevatorLEDSubsystem != null ? s.elevatorLEDSubsystem @@ -394,12 +394,7 @@ private void configureSuperStructureBindings() { case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; } }) - .withName("Set solo scoring mode")); - - sensors - .armSensor - .inClaw() - .and(RobotModeTriggers.teleop()) + .withName("Set solo scoring mode")) .onFalse( Commands.runOnce( () -> { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 0e4e28ea..570839d1 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -244,7 +244,9 @@ public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.5 /* this time could be shorter */), // spits out coral + spinnyClaw + .coralLevelOneHoldExtakePower() + .withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 43fe7cf6..902e580b 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,11 +23,13 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1L(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1L( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1R(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1R( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); } @@ -117,7 +119,9 @@ public static boolean isCloseEnoughL1R() { Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Transform2d middleOfReef = new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); - private static final Transform2d l1RightOfReef = new Transform2d(Units.inchesToMeters(34/2), Units.inchesToMeters(13.5/2), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(13.5 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -220,7 +224,7 @@ public static boolean isCloseEnoughL1R() { aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); - + private static final List blueBranchPoses = List.of( blueBranchA, From c1835195e3ea09cd794094ac48a2a190ba916407 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sun, 19 Oct 2025 13:36:06 -0700 Subject: [PATCH 12/19] ds changes --- src/main/deploy/elastic-layout.json | 1170 +++++------------ src/main/java/frc/robot/Controls.java | 8 + .../frc/robot/subsystems/SuperStructure.java | 4 +- .../frc/robot/subsystems/auto/AutoAlign.java | 12 +- 4 files changed, 339 insertions(+), 855 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 7e3a60b3..40397540 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -3,130 +3,33 @@ "grid_size": 128, "tabs": [ { - "name": "Match", + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "ArmSensor", "grid_layout": { "layouts": [], "containers": [ { - "title": "has arm sensor", + "title": "inTrough", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Match/has arm sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has ground intake sensor", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has ground intake sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has left branch sensor", - "x": 0.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has left branch sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has right branch sensor", - "x": 128.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has right branch sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has recent vision measurements", - "x": 384.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has recent vision measurements", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "left cam", - "x": 256.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "right cam", - "x": 768.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "Is Zeroed", - "x": 640.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/Is Zeroed", + "topic": "/Shuffleboard/ArmSensor/inTrough", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -139,465 +42,44 @@ } }, { - "name": "Autos", + "name": "IntakeSensor", "grid_layout": { "layouts": [], "containers": [ { - "title": "Auto Delay", - "x": 640.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Autos/Auto Delay", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Starting Position", - "x": 512.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Starting Position", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Game Objects", - "x": 768.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Game Objects", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Available Auto Variants", - "x": 512.0, - "y": 128.0, - "width": 512.0, - "height": 256.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Available Auto Variants", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Close Enough?", - "x": 1152.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/Close Enough?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Level?", - "x": 1280.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/Level?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Stationary?", - "x": 1408.0, + "title": "In Intake", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Autos/Stationary?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "readyToScore?", - "x": 1152.0, - "y": 128.0, - "width": 384.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/readyToScore?", + "topic": "/Shuffleboard/IntakeSensor/In Intake", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Selected auto", - "x": 0.0, - "y": 256.0, - "width": 512.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Shuffleboard/Field/Selected auto", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "Start pose", - "x": 1024.0, - "y": 256.0, - "width": 512.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Shuffleboard/Field/Start pose", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "Auto display speed", - "x": 0.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Shuffleboard/Field/Auto display speed", - "period": 0.06, - "data_type": "double", - "min_value": -0.5, - "max_value": 3.0, - "divisions": 5, - "update_continuously": false - } - } - ] - } - }, - { - "name": "AprilTags", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Last raw timestamp", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Last raw timestamp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Num targets", - "x": 0.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Num targets", - "period": 0.06, - "data_type": "int", - "show_submit_button": false - } - }, - { - "title": "Last timestamp", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Last timestamp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "april tag distance meters", - "x": 128.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/april tag distance meters", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "time since last reading", - "x": 256.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/time since last reading", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Disable vision", - "x": 512.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Toggle Button", - "properties": { - "topic": "/Shuffleboard/AprilTags/Disable vision", - "period": 0.06, - "data_type": "boolean" - } - } - ] - } - }, - { - "name": "Elevator", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Elevator Speed", - "x": 640.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/Elevator Speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Is Zeroed", - "x": 512.0, - "y": 128.0, - "width": 512.0, - "height": 512.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/Is Zeroed", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 at forward softstop", - "x": 0.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 at forward softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 at reverse softstop", - "x": 0.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 at reverse softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 output voltage", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 output voltage", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M1 supply current", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 supply current", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 at forward softstop", - "x": 1280.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 at forward softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M2 at reverse softstop", - "x": 1280.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 at reverse softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M2 output voltage", - "x": 1408.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 output voltage", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 supply current", - "x": 1280.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 supply current", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 temp", - "x": 1152.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 temp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false + "false_icon": "None" } - }, + } + ] + } + }, + { + "name": "BranchSensor", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "M1 temp", - "x": 256.0, + "title": "LeftDistance(m)", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Elevator/M1 temp", + "topic": "/Shuffleboard/BranchSensor/LeftDistance(m)", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -607,179 +89,132 @@ } }, { - "name": "End Effector", + "name": "AprilTags", "grid_layout": { "layouts": [], "containers": [ { - "title": "Pivot Target Pos", - "x": 512.0, + "title": "Last raw timestamp", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Target Pos", + "topic": "/Shuffleboard/AprilTags/Last raw timestamp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Speed", - "x": 256.0, - "y": 0.0, + "title": "Num targets", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "topic": "/Shuffleboard/AprilTags/Num targets", "period": 0.06, - "data_type": "double", + "data_type": "int", "show_submit_button": false } }, { - "title": "Pivot Position", + "title": "Last timestamp", "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Position", + "topic": "/Shuffleboard/AprilTags/Last timestamp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Motor rotor Pos", - "x": 0.0, - "y": 0.0, + "title": "april tag distance meters", + "x": 128.0, + "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Motor rotor Pos", + "topic": "/Shuffleboard/AprilTags/april tag distance meters", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Motor Temperature", - "x": 384.0, + "title": "time since last reading", + "x": 256.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Motor Temperature", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Distance(m)", - "x": 640.0, - "y": 512.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/ArmSensor/Distance(m)", + "topic": "/Shuffleboard/AprilTags/time since last reading", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "inClaw", + "title": "Disable vision", "x": 512.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/ArmSensor/inClaw", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "inTrough", - "x": 768.0, - "y": 256.0, - "width": 256.0, + "y": 0.0, + "width": 384.0, "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/ArmSensor/inTrough", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Claw Motor Temperature", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Claw/Claw Motor Temperature", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Claw Speed", - "x": 1024.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", + "type": "Toggle Button", "properties": { - "topic": "/Shuffleboard/Claw/Claw Speed", + "topic": "/Shuffleboard/AprilTags/Disable vision", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Elevator", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Last Set Power", - "x": 1152.0, - "y": 128.0, + "title": "Motor Current Position", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Claw/Last Set Power", + "topic": "/Shuffleboard/Elevator/Motor Current Position", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "Arm Pivot", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Motor Voltage", - "x": 1408.0, - "y": 128.0, + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Claw/Motor Voltage", + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -795,9 +230,9 @@ "containers": [ { "title": "Is Climb OUT?", - "x": 640.0, - "y": 128.0, - "width": 256.0, + "x": 0.0, + "y": 0.0, + "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -809,137 +244,68 @@ "true_icon": "None", "false_icon": "None" } - }, + } + ] + } + }, + { + "name": "Claw", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Is Climb STOWED?", - "x": 640.0, + "title": "Claw Speed", + "x": 0.0, "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Is Climb STOWED?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Motor Position", - "x": 1152.0, - "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Motor Position", + "topic": "/Shuffleboard/Claw/Claw Speed", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "GroundIntake", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Motor Speed", - "x": 1152.0, + "title": "Speed", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Motor Speed", + "topic": "/Shuffleboard/GroundIntake/Speed", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, - { - "title": "Move Complete?", - "x": 384.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Move Complete?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, + } + ] + } + }, + { + "name": "Ground Arm", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Set speed", - "x": 1280.0, + "title": "Pivot Speed", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Set speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Where Move next?", - "x": 640.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/Where Move next?", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Where moving?", - "x": 640.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/Where moving?", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Within Tolerance?", - "x": 896.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Within Tolerance?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "targetPos", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/targetPos", + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -949,212 +315,316 @@ } }, { - "name": "GroundIntake", + "name": "Controls", "grid_layout": { "layouts": [], "containers": [ { - "title": "Speed", + "title": "Swerve Coast Mode", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Toggle Button", "properties": { - "topic": "/Shuffleboard/GroundIntake/Speed", + "topic": "/Shuffleboard/Controls/Swerve Coast Mode", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Autos", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Pivot Motor Temperature", + "title": "readyToScore?", "x": 0.0, - "y": 384.0, + "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Motor Temperature", + "topic": "/Shuffleboard/Autos/readyToScore?", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Pivot Motor rotor Pos", - "x": 128.0, - "y": 384.0, - "width": 128.0, + "title": "Starting Position", + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Motor rotor Pos", + "topic": "/Shuffleboard/Autos/Starting Position", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Position", - "x": 256.0, - "y": 384.0, + "title": "Launch Type", + "x": 512.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Position", + "topic": "/Shuffleboard/Autos/Launch Type", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Speed", - "x": 384.0, - "y": 384.0, + "title": "Game Objects", + "x": 640.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Speed", + "topic": "/Shuffleboard/Autos/Game Objects", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Target Pos", + "title": "Available Auto Variants", "x": 512.0, - "y": 384.0, + "y": 256.0, "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Target Pos", + "topic": "/Shuffleboard/Autos/Available Auto Variants", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Last Set Power", - "x": 0.0, - "y": 128.0, + "title": "Auto Delay", + "x": 512.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/GroundIntake/Last Set Power", + "topic": "/Shuffleboard/Autos/Auto Delay", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Motor Temperature", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", + "title": "photonvision_Port_1182_Output_MJPEG_Server", + "x": 256.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", "properties": { - "topic": "/Shuffleboard/GroundIntake/Motor Temperature", + "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "rotation_turns": 0 } }, { - "title": "Motor Voltage", - "x": 128.0, - "y": 128.0, - "width": 128.0, + "title": "photonvision_Port_1186_Output_MJPEG_Server", + "x": 640.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + }, + { + "name": "Field", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Auto display speed", + "x": 1280.0, + "y": 640.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Number Slider", "properties": { - "topic": "/Shuffleboard/GroundIntake/Motor Voltage", + "topic": "/Shuffleboard/Field/Auto display speed", "period": 0.06, "data_type": "double", - "show_submit_button": false + "min_value": -1.0, + "max_value": 1.0, + "divisions": 5, + "update_continuously": false } }, { - "title": "Distance(m)", - "x": 640.0, - "y": 0.0, + "title": "Est. Time (s)", + "x": 1280.0, + "y": 512.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/IntakeSensor/Distance(m)", + "topic": "/Shuffleboard/Field/Est. Time (s)", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "In Intake", - "x": 768.0, + "title": "Selected auto", + "x": 0.0, "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", + "width": 1280.0, + "height": 768.0, + "type": "Field", "properties": { - "topic": "/Shuffleboard/IntakeSensor/In Intake", + "topic": "/Shuffleboard/Field/Selected auto", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 } } ] } }, { - "name": "Controls", + "name": "Match", "grid_layout": { "layouts": [], "containers": [ { - "title": "Elevator Coast Mode", - "x": 384.0, + "title": "has arm sensor", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Elevator Coast Mode", + "topic": "/Shuffleboard/Match/has arm sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Swerve Coast Mode", - "x": 512.0, + "title": "has ground intake sensor", + "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Swerve Coast Mode", + "topic": "/Shuffleboard/Match/has ground intake sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Climb Coast Mode", - "x": 640.0, - "y": 0.0, + "title": "has left branch sensor", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has left branch sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has right branch sensor", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Climb Coast Mode", + "topic": "/Shuffleboard/Match/has right branch sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has recent vision measurements", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has recent vision measurements", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "left cam", + "x": 256.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/left", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "right cam", + "x": 768.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/right", + "period": 0.06, + "rotation_turns": 0 } } ] diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index a616dda5..7ec94515 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -112,6 +112,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureGroundSpinnyBindings(); configureGroundArmBindings(); configureSoloControllerBindings(); + Shuffleboard.getTab("Elevator") + .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE); + // Shuffleboard.getTab("Elevator") + // .addString("Scoring Mode", () -> getSoloScoringMode().toString()); + } + + public SoloScoringMode getSoloScoringMode() { + return soloScoringMode; } private Trigger connected(CommandXboxController controller) { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 0e4e28ea..570839d1 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -244,7 +244,9 @@ public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.5 /* this time could be shorter */), // spits out coral + spinnyClaw + .coralLevelOneHoldExtakePower() + .withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 43fe7cf6..726558d8 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,11 +23,13 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1L(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1L( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1R(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1R( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); } @@ -117,7 +119,9 @@ public static boolean isCloseEnoughL1R() { Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Transform2d middleOfReef = new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); - private static final Transform2d l1RightOfReef = new Transform2d(Units.inchesToMeters(34/2), Units.inchesToMeters(13.5/2), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(26 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -220,7 +224,7 @@ public static boolean isCloseEnoughL1R() { aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); - + private static final List blueBranchPoses = List.of( blueBranchA, From 0bcbd20a956cb096e48deeaf1470a234b0520dc2 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sun, 19 Oct 2025 13:36:55 -0700 Subject: [PATCH 13/19] quick hand off fix --- src/main/java/frc/robot/Controls.java | 2 +- src/main/java/frc/robot/subsystems/SuperStructure.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b0b3fd96..7267be6b 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -371,7 +371,7 @@ private void configureSuperStructureBindings() { .and(RobotModeTriggers.teleop()) .onTrue( superStructure - .coralIntake().alongWith(Commands.runOnce(() -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW)) + .coralIntake() .alongWith( s.elevatorLEDSubsystem != null ? s.elevatorLEDSubsystem diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 570839d1..b1b5624e 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -301,11 +301,11 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Make the big sequence. return Commands.sequence( // Initial setup- Move elevator high enough for ground arm to be clear, start moving - // arm pivot, stop the spinny claw, and start spinning the ground intake + // arm pivot, and start spinning the ground intake Commands.parallel( elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), - spinnyClaw.stop(), // just as a backup in case things are silly + spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), From b2d668730f7a954b966a611709acb575b9518cac Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 00:41:48 -0700 Subject: [PATCH 14/19] log + spot --- src/main/java/frc/robot/Controls.java | 4 ++-- src/main/java/frc/robot/subsystems/SuperStructure.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 6cd226dc..36eb85d2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -114,8 +114,8 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureSoloControllerBindings(); Shuffleboard.getTab("Elevator") .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE); - // Shuffleboard.getTab("Elevator") - // .addString("Scoring Mode", () -> getSoloScoringMode().toString()); + Shuffleboard.getTab("Elevator") + .addString("Scoring Mode", () -> getSoloScoringMode().toString()); } public SoloScoringMode getSoloScoringMode() { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index b1b5624e..217675a6 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -305,7 +305,7 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph Commands.parallel( elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), - spinnyClaw.coralIntakePower(), + spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), From 1ce5960ba17738da9495b715cc2b6b1bc0b96fa4 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 21 Oct 2025 23:07:45 -0700 Subject: [PATCH 15/19] Added smoothing factor to claw position(DEBUG AND TESTING WIP) --- .../robot/subsystems/ElevatorSubsystem.java | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index dc5e81ef..431ea58a 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -12,15 +12,18 @@ 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.Rotation3d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; @@ -29,6 +32,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Hardware; +import jdk.jfr.Timestamp; + +import java.util.concurrent.TimeUnit; import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -97,6 +103,8 @@ public class ElevatorSubsystem extends SubsystemBase { private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth); private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); private final StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); + private final StructPublisher TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish(); + private final StructPublisher TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish(); // Creates a SysIdRoutine SysIdRoutine routine = @@ -385,7 +393,17 @@ public void periodic() { m_motorOneSimState.setRawRotorPosition(targetPos); m_motorTwoSimState.setRawRotorPosition(targetPos); - elevatorPose3d.set(new Pose3d(new Pose2d(0.0, getHeightMeters(), new Rotation2d(0.0,0.0)))); + elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d())); + + + + double smoothing = 0.01; + TESTpose.set(new Pose3d( + 0.0, 0.0, getHeightMeters() + (getTargetPosition() - getCurrentPosition()) * smoothing, + new Rotation3d(Math.PI / 2, Math.PI / 2, -Math.PI / 2))); + TESTpose2.set(new Pose3d(0.0,0.0,0.0,new Rotation3d(0.0,Math.sin((Timer.getFPGATimestamp())),0.0))); + + } } } From 6aab30c29005ec24665d70e7659ef7675089c3b2 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 28 Oct 2025 20:47:07 -0700 Subject: [PATCH 16/19] Claw position fixed, elevator height offset kinda working WIP --- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 431ea58a..803ac38c 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -399,8 +399,8 @@ public void periodic() { double smoothing = 0.01; TESTpose.set(new Pose3d( - 0.0, 0.0, getHeightMeters() + (getTargetPosition() - getCurrentPosition()) * smoothing, - new Rotation3d(Math.PI / 2, Math.PI / 2, -Math.PI / 2))); + 0.2, 0.0, (getCurrentPosition() / 6)+ 0.4 , + new Rotation3d(0.0,0.0,-135))); TESTpose2.set(new Pose3d(0.0,0.0,0.0,new Rotation3d(0.0,Math.sin((Timer.getFPGATimestamp())),0.0))); From 5fa4bb0ef309bd88b836a32c5deda43607e06b0f Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 6 Nov 2025 19:41:27 -0800 Subject: [PATCH 17/19] Added positional smoothing --- .../robot/subsystems/ElevatorSubsystem.java | 36 ++++++++++++------- .../frc/robot/subsystems/auto/AutoLogic.java | 5 +++ 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 803ac38c..773fdd65 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -32,6 +32,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Hardware; +import frc.robot.Robot; +import frc.robot.sensors.ArmSensor; +import frc.robot.subsystems.auto.AutoLogic; import jdk.jfr.Timestamp; import java.util.concurrent.TimeUnit; @@ -63,7 +66,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; - private static final double MOTOR_ROTATIONS_PER_METER = 40; // Inaccurate + private static final double MOTOR_ROTATIONS_PER_METER = 3; // Inaccurate public static final double MANUAL = 0.1; private static final double POS_TOLERANCE = 0.1; private final double ELEVATOR_KP = 7.804; @@ -102,9 +105,9 @@ public class ElevatorSubsystem extends SubsystemBase { new Alert("Elevator", "Motor 2 not connected", AlertType.kError); private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth); private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); - private final StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); - private final StructPublisher TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish(); - private final StructPublisher TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish(); + private StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); + public StructPublisher TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish(); + public StructPublisher TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish(); // Creates a SysIdRoutine SysIdRoutine routine = @@ -382,7 +385,8 @@ public Command stop() { .ignoringDisable(true) .withName("ElevatorStop"); } - + double smoothedAngleZ = 0.4; + double smoothingFactor = 0.1; @Override public void periodic() { NotConnectedError.set( @@ -390,20 +394,28 @@ public void periodic() { NotConnectedError2.set( notConnectedDebouncerTwo.calculate(!m_motor2.getMotorVoltage().hasUpdated())); if (RobotBase.isSimulation()) { + if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) { + double targetZ = (CORAL_STOWED/ MOTOR_ROTATIONS_PER_METER);} m_motorOneSimState.setRawRotorPosition(targetPos); m_motorTwoSimState.setRawRotorPosition(targetPos); - elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d())); - - - - double smoothing = 0.01; + + double targetZ = (getCurrentPosition()/ MOTOR_ROTATIONS_PER_METER); + smoothedAngleZ += (targetZ - smoothedAngleZ) * smoothingFactor; + TESTpose.set(new Pose3d( - 0.2, 0.0, (getCurrentPosition() / 6)+ 0.4 , - new Rotation3d(0.0,0.0,-135))); + 0.2, 0.0, smoothedAngleZ, + new Rotation3d(0.0, 0.0, -135))); + //TESTpose.set(new Pose3d( + //0.2, 0.0, (getHeightMeters() + getTargetPosition() - getCurrentPosition()), + // new Rotation3d(0.0, 0.0, -135))); + + + TESTpose2.set(new Pose3d(0.0,0.0,0.0,new Rotation3d(0.0,Math.sin((Timer.getFPGATimestamp())),0.0))); } + } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index 2f0a7a54..b6b01456 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -9,7 +9,9 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.FileVersionException; 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.Rotation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; @@ -25,6 +27,8 @@ import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; +import frc.robot.subsystems.ElevatorSubsystem; + import java.io.IOException; import java.util.HashMap; import java.util.List; @@ -299,6 +303,7 @@ public static Command scoreCommand() { // Condition: () -> ARMSENSOR_ENABLED && r.sensors.armSensor.booleanInClaw()); } + return AutoAlign.autoAlign(s.drivebaseSubsystem, controls) .withName("scoreCommand-noSuperstructure"); } From db5ca1e54054f0dd4b4f389b20acbc9b5d994960 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 13 Nov 2025 21:58:51 -0800 Subject: [PATCH 18/19] Sort of decent claw presets? --- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 773fdd65..481ef404 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -66,7 +66,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; - private static final double MOTOR_ROTATIONS_PER_METER = 3; // Inaccurate + private static final double MOTOR_ROTATIONS_PER_METER = 19.68; // Inaccurate public static final double MANUAL = 0.1; private static final double POS_TOLERANCE = 0.1; private final double ELEVATOR_KP = 7.804; @@ -395,7 +395,7 @@ public void periodic() { notConnectedDebouncerTwo.calculate(!m_motor2.getMotorVoltage().hasUpdated())); if (RobotBase.isSimulation()) { if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) { - double targetZ = (CORAL_STOWED/ MOTOR_ROTATIONS_PER_METER);} + } m_motorOneSimState.setRawRotorPosition(targetPos); m_motorTwoSimState.setRawRotorPosition(targetPos); elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d())); @@ -404,7 +404,7 @@ public void periodic() { smoothedAngleZ += (targetZ - smoothedAngleZ) * smoothingFactor; TESTpose.set(new Pose3d( - 0.2, 0.0, smoothedAngleZ, + 0.2, 0.0, smoothedAngleZ * 5.5, new Rotation3d(0.0, 0.0, -135))); //TESTpose.set(new Pose3d( //0.2, 0.0, (getHeightMeters() + getTargetPosition() - getCurrentPosition()), From c59dc102cb12d9bb0eb1d9f0f2d4c1034d5bfb12 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 14 Nov 2025 22:22:30 -0800 Subject: [PATCH 19/19] Better debug elevator presets --- .../robot/subsystems/ElevatorSubsystem.java | 61 +++++++++++-------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 481ef404..a7e512d6 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.Units; @@ -107,8 +108,10 @@ public class ElevatorSubsystem extends SubsystemBase { private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); private StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); public StructPublisher TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish(); - public StructPublisher TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish(); - + //public StructPublisher TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish(); + + + // Creates a SysIdRoutine SysIdRoutine routine = new SysIdRoutine( @@ -126,7 +129,7 @@ public ElevatorSubsystem() { Shuffleboard.getTab("Elevator").addDouble("Motor Current Position", () -> getCurrentPosition()); //Elevator pose test - + Shuffleboard.getTab("Elevator").addDouble("Target Position", () -> getTargetPosition()); Shuffleboard.getTab("Elevator") .addDouble("M1 supply current", () -> m_motor.getSupplyCurrent().getValueAsDouble()); @@ -153,7 +156,16 @@ public ElevatorSubsystem() { "M2 at reverse softstop", () -> m_motor2.getFault_ReverseSoftLimit().getValue()); Shuffleboard.getTab("Elevator") .addDouble("Elevator Speed", () -> m_motor.getVelocity().getValueAsDouble()); - + + // Test commands + Shuffleboard.getTab("Elevator") + .add("Move to Level Four", setLevel(CORAL_LEVEL_FOUR_PRE_POS)); + Shuffleboard.getTab("Elevator") + .add("Move to Level Three", setLevel(CORAL_LEVEL_THREE_PRE_POS)); + Shuffleboard.getTab("Elevator") + .add("Move to Level Two", setLevel(CORAL_LEVEL_TWO_PRE_POS)); + Shuffleboard.getTab("Elevator") + .add("Move to Level One", setLevel(CORAL_LEVEL_ONE_POS)); } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -295,6 +307,7 @@ public Command setLevel(double pos) { return runOnce( () -> { if (hasBeenZeroed) { + System.out.println("Setting elevator level to: " + pos); m_motor.setControl(m_request.withPosition(pos)); m_motor2.setControl(new Follower(m_motor.getDeviceID(), true)); targetPos = pos; @@ -394,28 +407,24 @@ public void periodic() { NotConnectedError2.set( notConnectedDebouncerTwo.calculate(!m_motor2.getMotorVoltage().hasUpdated())); if (RobotBase.isSimulation()) { - if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) { - } - m_motorOneSimState.setRawRotorPosition(targetPos); - m_motorTwoSimState.setRawRotorPosition(targetPos); - elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d())); - - double targetZ = (getCurrentPosition()/ MOTOR_ROTATIONS_PER_METER); - smoothedAngleZ += (targetZ - smoothedAngleZ) * smoothingFactor; - - TESTpose.set(new Pose3d( - 0.2, 0.0, smoothedAngleZ * 5.5, - new Rotation3d(0.0, 0.0, -135))); - //TESTpose.set(new Pose3d( - //0.2, 0.0, (getHeightMeters() + getTargetPosition() - getCurrentPosition()), - // new Rotation3d(0.0, 0.0, -135))); - - - - TESTpose2.set(new Pose3d(0.0,0.0,0.0,new Rotation3d(0.0,Math.sin((Timer.getFPGATimestamp())),0.0))); - - + if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) { + } + m_motorOneSimState.setRawRotorPosition(targetPos); + m_motorTwoSimState.setRawRotorPosition(targetPos); + //elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d())); + + double curPos = getCurrentPosition(); + double smoothingFactor = 0.5;// Percentage Scaler + double bottomZ = 0.2; + double topZ = 1.55; + double minPos = 0.0; + double maxPos = 37.5; + double targetZ = (bottomZ + ((curPos - minPos) / (maxPos - minPos)) * (topZ - bottomZ)); + + TESTpose.set(new Pose3d( + 0.2, 0.0, targetZ, + new Rotation3d(0.0, 0.0, -135))); } - + } }