diff --git a/elastic-layout.json b/elastic-layout.json index 53d53bb..8a56808 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 384.0, + "x": 896.0, "y": 0.0, "width": 256.0, - "height": 256.0, + "height": 384.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -55,27 +55,11 @@ } ], "containers": [ - { - "title": "MatchTime", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/DriverStation/MatchTime", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, { "title": "RetractedLimit", - "x": 0.0, - "y": 384.0, - "width": 128.0, + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -90,9 +74,9 @@ }, { "title": "Auto Chooser", - "x": 1024.0, - "y": 256.0, - "width": 384.0, + "x": 896.0, + "y": 384.0, + "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -103,9 +87,9 @@ }, { "title": "Roller Percent Output", - "x": 0.0, + "x": 512.0, "y": 256.0, - "width": 256.0, + "width": 384.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -120,8 +104,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 128.0, - "y": 384.0, + "x": 768.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -134,8 +118,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 256.0, - "y": 256.0, + "x": 512.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -151,8 +135,8 @@ }, { "title": "Mode", - "x": 256.0, - "y": 384.0, + "x": 512.0, + "y": 128.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -164,30 +148,49 @@ } }, { - "title": "Time to rev", - "x": 768.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Large Text Display", + "title": "IsActive", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Boolean Box", "properties": { - "topic": "/AdvantageKit/RealOutputs/Time to rev", + "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", "period": 0.06, - "data_type": "double" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Arm angle extended", - "x": 1152.0, - "y": 128.0, - "width": 256.0, + "title": "RemainingInShift", + "x": 0.0, + "y": 256.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Arm angle extended", + "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", "period": 0.06, "data_type": "double", - "show_submit_button": true + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Phase", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "period": 0.06, + "data_type": "string" } } ] @@ -527,8 +530,8 @@ }, { "title": "Shooter Idle max current", - "x": 384.0, - "y": 512.0, + "x": 1024.0, + "y": 384.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -555,8 +558,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 640.0, - "y": 512.0, + "x": 1024.0, + "y": 256.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -575,12 +578,40 @@ "grid_layout": { "layouts": [], "containers": [ + { + "title": "Stalled Motor: ", + "x": 512.0, + "y": 128.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter idle % output", + "x": 128.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shooter idle % output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, { "title": "Auto Chooser", - "x": 0.0, - "y": 0.0, - "width": 640.0, - "height": 768.0, + "x": 512.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, "type": "ComboBox Chooser", "properties": { "topic": "/SmartDashboard/Auto Chooser", diff --git a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto new file mode 100644 index 0000000..8765ce9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot driving across" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot Driving" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto new file mode 100644 index 0000000..43867c9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump test" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "BumpToCenter" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump test.path b/src/main/deploy/pathplanner/paths/Bump test.path new file mode 100644 index 0000000..dfb810c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump test.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4908076923076936, + "y": 6.633345195729537 + }, + "prevControl": null, + "nextControl": { + "x": 2.3977564102564113, + "y": 6.0868195547038955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4908076923076936, + "y": 5.6745769230769225 + }, + "prevControl": { + "x": 2.6330609248104158, + "y": 5.743196664476699 + }, + "nextControl": { + "x": 3.7815128205128214, + "y": 5.651320512820514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.967615384615385, + "y": 5.709461538461539 + }, + "prevControl": { + "x": 4.746653846153847, + "y": 5.430384615384616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5211640211640187, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.24536426676825 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.59775645089275 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BumpToCenter.path b/src/main/deploy/pathplanner/paths/BumpToCenter.path new file mode 100644 index 0000000..7051d4f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BumpToCenter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.935255041518388, + "y": 5.6650177935943065 + }, + "prevControl": null, + "nextControl": { + "x": 6.073930090574693, + "y": 5.873030367178768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.559893238434164, + "y": 7.547876631079477 + }, + "prevControl": { + "x": 7.1940806642941855, + "y": 7.182064056939502 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.98776039963968 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.19348942398209 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path index 7325176..dd06473 100644 --- a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path +++ b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path index cbd5c03..3307f61 100644 --- a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path @@ -104,7 +104,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path index cf8bda0..c2d5d1a 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path index 2cab886..ae1d06a 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path index ddc66c0..79a2251 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path index 0d931a0..ab1ab91 100644 --- a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path @@ -64,7 +64,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path index 61b8049..992c038 100644 --- a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path +++ b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/ReadyPush.path b/src/main/deploy/pathplanner/paths/ReadyPush.path index 3c8bced..4d01629 100644 --- a/src/main/deploy/pathplanner/paths/ReadyPush.path +++ b/src/main/deploy/pathplanner/paths/ReadyPush.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Shoot driving across.path b/src/main/deploy/pathplanner/paths/Shoot driving across.path new file mode 100644 index 0000000..4af7874 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot driving across.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": null, + "nextControl": { + "x": 1.9932621113872893, + "y": 6.3390412980405895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.936134248900721, + "y": 1.9128215679513527 + }, + "nextControl": { + "x": 1.9291221613556924, + "y": 1.4128707397409563 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.9198835421653975, + "y": 6.331901987278464 + }, + "nextControl": { + "x": 1.9453728680910158, + "y": 6.8312518588753814 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.9337461053582308, + "y": 1.912843654431812 + }, + "nextControl": { + "x": 1.9315103048981825, + "y": 1.4128486532604971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.8878500514131944, + "y": 6.335619778537727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.8, + "maxAcceleration": 10.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index de52ec1..42c6f2c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,11 +15,14 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.HubShiftTimer; +import frc4388.utility.compute.HubShiftTimer.ShiftInfo; import frc4388.utility.compute.RobotTime; import frc4388.utility.compute.Trim; import frc4388.utility.status.FaultReporter; @@ -113,6 +116,7 @@ public class Robot extends LoggedRobot { m_autonomousCommand.schedule(); } m_robotTime.startMatchTime(); + HubShiftTimer.initializeAuto(); } /** @@ -138,6 +142,7 @@ public class Robot extends LoggedRobot { } m_robotTime.startMatchTime(); + HubShiftTimer.initializeTeleop(); } /** @@ -145,7 +150,12 @@ public class Robot extends LoggedRobot { */ @Override public void teleopPeriodic() { - // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); + var info = HubShiftTimer.getShiftInfo(); + + double rumble = (info.remainingInShift() < 5. && info.remainingInShift() > 0.1) ? 1 : 0; + + // m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble); + // m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd47223..2559999 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,9 +5,14 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package frc4388.robot; import java.io.File; +import java.util.Optional; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -24,24 +29,27 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc4388.robot.commands.Swerve.StayInPosition; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; -// Subsystems -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.FieldPositions; @@ -61,7 +69,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* RobotMap */ - public final RobotMap m_robotMap = new RobotMap(Mode.REAL); + public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM); /*Limit Switch */ // public final DigitalInput m_armLimitSwitch = new DigitalInput(9); @@ -69,6 +77,7 @@ public class RobotContainer { /* Subsystems */ // public final Lidar m_lidar = new Lidar(); public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID); + public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim(); //Testing of Colors public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); @@ -84,156 +93,212 @@ public class RobotContainer { // private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); // public List subsystems = new ArrayList<>(); - - // ! Teleop Commands - public void stop() { - new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); - m_robotSwerveDrive.stopModules(); - Constants.AutoConstants.Y_OFFSET_TRIM.set(0); - } - - // ! /* Autos */ - private SendableChooser autoChooser; - private Command autoCommand; - - - private Command IntakeExtended = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) - ); - - // private Command LidarIntake = new SequentialCommandGroup( - // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball - // // RobotIntakeDown, - // // new WaitCommand(1), - // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), - // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) - - // // new RunCommand( - // // () -> m_robotSwerveDrive.driveWithInput( - // // m_lidar.getClosestBall(), - // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), - // // false - // // ), - // // m_robotSwerveDrive - // // ) - // // .withTimeout(5.0) - // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) - // ); - - private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), - IntakeExtended, - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) - ); - - private Command IntakeRetracted = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) - ); - - private Command RobotShoot = new SequentialCommandGroup( - // TEST NEW AUTO ALIGN - //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), - new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), - new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(2), - IntakeRetracted, - new WaitCommand(5), - new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), - new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) - ); - + private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive); - public RobotContainer() { - - configureButtonBindings(); - - // Called on first robot enable - DeferredBlock.addBlock(() -> { - m_robotSwerveDrive.resetGyro(); - }, false); - - // Called on every robot enable - DeferredBlock.addBlock(() -> { - TimesNegativeOne.update(); - FieldPositions.update(); - - m_robotIntake.io.updateGains(); - m_robotShooter.io.updateGains(); - }, true); - - NamedCommands.registerCommand("Robot Rev Up", RobotRev); - NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); - NamedCommands.registerCommand("Robot Shoot", RobotShoot); - // NamedCommands.registerCommand("Lidar Intake", LidarIntake); - NamedCommands.registerCommand("Intake Extended", IntakeExtended); - - - DriverStation.silenceJoystickConnectionWarning(true); - - // Drive normally - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(),true); - - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - - m_robotSwerveDrive.setToSlow(); - - makeAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - - } + private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d()); + // ! Teleop Commands + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + } + // ! /* Autos */ + private SendableChooser autoChooser; + private Command autoCommand; + + + private Command IntakeExtended = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) + ); + + // private Command LidarIntake = new SequentialCommandGroup( + // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball + // // RobotIntakeDown, + // // new WaitCommand(1), + // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) + + // // new RunCommand( + // // () -> m_robotSwerveDrive.driveWithInput( + // // m_lidar.getClosestBall(), + // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), + // // false + // // ), + // // m_robotSwerveDrive + // // ) + // // .withTimeout(5.0) + // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) + // ); + + private Command RobotRev = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), + IntakeExtended, + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) + ); - - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - - //Driver controls - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - + private Command RobotShootDriving = new SequentialCommandGroup( + new RunCommand(() -> + m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION) + ).withTimeout(20) + ).finallyDo((interrupted) -> + m_robotSwerveDrive.disableRotationOverride() + ); + + private Command IntakeRetracted = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) + ); + + private Command RobotShoot = new SequentialCommandGroup( + // TEST NEW AUTO ALIGN + //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), + new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), + new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), + new WaitCommand(5), + IntakeRetracted, + new WaitCommand(10), + new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), + new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) + ); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> { + public RobotContainer() { + + configureSINGLEBindings(); + + // Called on first robot enable + DeferredBlock.addBlock(() -> { + m_robotSwerveDrive.resetGyro(); + }, false); + + // Called on every robot enable + DeferredBlock.addBlock(() -> { + TimesNegativeOne.update(); + FieldPositions.update(); + m_robotIntake.io.updateGains(); m_robotShooter.io.updateGains(); + }, true); + + NamedCommands.registerCommand("Robot Rev Up", RobotRev); + NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); + NamedCommands.registerCommand("Robot Shoot", RobotShoot); + // NamedCommands.registerCommand("Lidar Intake", LidarIntake); + NamedCommands.registerCommand("Intake Extended", IntakeExtended); + NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); + + NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed)); + NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter)); + NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)); + NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter)); + NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter)); + + NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> { + if (TimesNegativeOne.isRed) { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED); + } else { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE); + } + })); + + NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> { + if (!TimesNegativeOne.isRed) { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED); + } else { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE); + } + })); + + + DriverStation.silenceJoystickConnectionWarning(true); + + // Drive normally + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); + + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); + + m_robotSwerveDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + } + + + + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + + //Driver controls + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); + })); + + + // TEST-> the driver is holding the left trigger, drive slow and rotation up + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + m_robotSwerveDrive.shiftUpRot(); + })) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.setToFast(); + m_robotSwerveDrive.shiftDownRot(); + })); + + //TEST - > X positino on wheels + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + })); + + //TEST - > PID positinon + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + })); - // IF the driver is holding the left trigger, intake driving - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveIntakeOrientation( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight() - - ); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); - })); // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + })) + .whileTrue(new RunCommand(() -> { m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), FieldPositions.HUB_POSITION, @@ -326,6 +391,126 @@ public class RobotContainer { } + + private void configureSINGLEBindings() { + + //Driver controls + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED))); + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + + + // TEST-> the driver is holding the left trigger, drive slow and rotation up + // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + // .onTrue(new InstantCommand(() -> { + // m_robotSwerveDrive.setToSlow(); + // m_robotSwerveDrive.shiftUpRot(); + // })) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.setToFast(); + // m_robotSwerveDrive.shiftDownRot(); + // })); + + //TEST - > X positino on wheels + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + })); + + //TEST - > PID positinon + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + + })); + + + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + })) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get() + ); + }, m_robotSwerveDrive) + ); + + // D-PAD fine alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + + //allow shooting with right trigger + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotShooter.allowShooting(); + })).onFalse(new InstantCommand(() -> { + m_robotShooter.denyShooting(); + })); + + + + //set shooter ready (rev) with left trigger hold + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + m_robotIntake.rollerStop(); + m_robotShooter.spinUpShooting(); + })) + .onFalse(new InstantCommand(() -> { + m_robotShooter.spinUpIdle(); + })); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extending); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracting); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); + + } + //.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9791edb..b3dc909 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -25,6 +25,7 @@ import frc4388.robot.subsystems.intake.IntakeReal; import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.robot.subsystems.shooter.ShooterIO; import frc4388.robot.subsystems.shooter.ShooterReal; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; // import frc4388.robot.subsystems.elevator.ElevatorIO; // import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; @@ -133,8 +134,15 @@ public class RobotMap { FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); break; - // case SIM: - // break; + case SIM: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + + swerveDrivetrain = new SimpleSwerveSim() {}; + + shooterIO = new ShooterIO() {}; + intakeIO = new IntakeIO() {}; + break; default: leftCamera = new VisionIO() {}; rightCamera = new VisionIO() {}; diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java deleted file mode 100644 index 3a4e043..0000000 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc4388.robot.commands; - -import java.time.Instant; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class MoveForTimeCommand extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final long duration; - private final boolean robotRelative; - - private Instant startTime; - - public MoveForTimeCommand( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - long millis, - boolean robotRelative) { - - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.duration = millis; - this.robotRelative = robotRelative; - } - - @Override - public void initialize() { - startTime = Instant.now(); - } - - @Override - public void execute() { - swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); - } - - @Override - public boolean isFinished() { - return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java deleted file mode 100644 index c5b5e53..0000000 --- a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc4388.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class MoveUntilSuply extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final Supplier truth; - private final boolean robotRelative; - - public MoveUntilSuply( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - Supplier truth, - boolean robotRelative) { - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.truth = truth; - this.robotRelative = robotRelative; - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); - } - - @Override - public boolean isFinished() { - return truth.get(); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java deleted file mode 100644 index a3610d1..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -public class RotateToAngle extends PID { - - SwerveDrive drive; - double targetAngle; - - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.targetAngle = targetAngle; - - addRequirements(drive); - } - - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java new file mode 100644 index 0000000..7b826a7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Swerve; + +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.AutoLogOutput; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PID; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +public class StayInPosition extends PID { + SwerveDrive drive; + Translation2d driveTranslation = new Translation2d(); + + public StayInPosition(SwerveDrive drive) { + super(0.3, 0.0, 0.0, 0.0, 1); + this.drive = drive; + addRequirements(drive); + } + + public void goToTargetPose(Pose2d targetPose) { + Pose2d currentPose = drive.getCurrentPose(); + double translationX = targetPose.getX() - currentPose.getX(); + double translationY = targetPose.getY() - currentPose.getY(); + if (translationX > 0.2){ + //If below is changed make this change too so it never goes over 1 + translationX = 0.2; + } + if (translationY > 0.2){ + //Same here + translationY = 0.2; + } + if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) { + driveTranslation = new Translation2d(); + } else { + //TODO: Tweak for best corrector against another bot + driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); + } + + drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); + } + + @Override + public double getError() { + return 0; + } + + @Override + public void runWithOutput(double output) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java deleted file mode 100644 index ba3c6ce..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ /dev/null @@ -1,197 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileInputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.compute.DataUtils; -import frc4388.utility.controller.VirtualController; -import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; - - -/** - * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickPlayback extends Command { - private final SwerveDrive swerve; - private final VirtualController[] controllers; - private final ArrayList frames = new ArrayList<>(); - private final Supplier filenameGetter; - private String filename; - private int frame_index = 0; - // private long startTime = 0; - // private long playbackTime = 0; - private boolean m_finished = false; // ! There is no better way. - private boolean m_shouldfree = false; // should free memory on ending - - private byte m_numAxes = 0; - private byte m_numPOVs = 0; - private byte m_numControllers = 0; - private short m_numFrames = -1; - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree Unloads the auto on compleation or intruption. - * @param instantload Load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this.swerve = swerve; - this.filenameGetter = filenameGetter; - this.controllers = controllers; - this.m_shouldfree = shouldfree; - - if (instantload) loadAuto(); - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree unloads the auto on compleation or intruption. - * @param instantload load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this(swerve, () -> filename, controllers, shouldfree, instantload); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { - this(swerve, filenameGetter, controllers, true, false); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { - this(swerve, () -> filename, controllers, true, false); - } - - /** - * Load the auto file from disk into memory - * @return Returns true if loading was successful, else wise; return false - * @implNote if the auto is already loaded, it will return true. - */ - public boolean loadAuto() { - filename = filenameGetter.get(); - try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { - if (m_numFrames != -1 && m_numFrames == frames.size()) { - System.out.println("AUTOPLAYBACK: Auto Already loaded."); - return true; - } - - m_numAxes = stream.readNBytes(1)[0]; - m_numPOVs = stream.readNBytes(1)[0]; - m_numControllers = stream.readNBytes(1)[0]; - m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); - - if (m_numControllers > controllers.length) { - System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers - + " virtual controllers but only " + controllers.length + " were given"); - return false; - } - - for (int i = 0; i < m_numFrames; i++) { - AutoRecordingFrame frame = new AutoRecordingFrame(); - for (int j = 0; j < m_numControllers; j++) { - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = new double[m_numAxes]; - for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. - axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - } - short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); - short[] POV = new short[m_numPOVs]; - for (int k = 0; k < m_numPOVs; k++) { - POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); - } - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[j] = controllerFrame; - } - frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); - frames.add(frame); - } - - System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); - return true; - - } catch (Exception e) { - e.printStackTrace(); - System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); - return false; - } - } - - /** - * Unloads the auto. - */ - public void unloadAuto() { - System.out.println("AUTOPLAYBACK: Auto unloaded"); - frames.clear(); - } - - @Override - public void initialize() { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - frame_index = 0; - - m_finished = !loadAuto(); - } - - @Override - public void execute() { - if (frame_index >= m_numFrames) m_finished = true; - if (m_finished) return; - - // if (frame_index == 0) { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - // } else { - // playbackTime = System.currentTimeMillis() - startTime; - // } - - AutoRecordingFrame frame = frames.get(frame_index); - for (int i = 0; i < controllers.length; i++) { - AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; - controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); - if (i == 0) { - this.swerve.driveWithInput( - new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), - new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), - true); - } - } - frame_index++; - } - - @Override - public void end(boolean interrupted) { - for (VirtualController controller : controllers) controller.zeroControls(); - swerve.stopModules(); - if (m_shouldfree) unloadAuto(); - } - - @Override - public boolean isFinished() { - return m_finished; - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java deleted file mode 100644 index 9aa9283..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileOutputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.compute.DataUtils; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; - -/** - * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickRecorder extends Command { - private final SwerveDrive swerve; - private final XboxController[] controllers; - private String filename; - private final Supplier filenameGetter; - private long startTime = -1; - private final ArrayList frames = new ArrayList<>(); - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { - this.swerve = swerve; - this.controllers = controllers; - this.filenameGetter = filenameGetter; - this.filename = ""; - - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filename a String containing the name of the auto file you wish to playback. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { - this(swerve, controllers, () -> filename); - } - - @Override - public void initialize() { - frames.clear(); - - this.startTime = System.currentTimeMillis(); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; - frames.add(frame); - this.filename = this.filenameGetter.get(); - } - - - @Override - public void execute() { - System.out.println("AUTORECORD: RECORDING"); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.timeStamp = (int) (System.currentTimeMillis() - startTime); - for (int i = 0; i < controllers.length; i++) { - XboxController controller = controllers[i]; - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = {controller.getLeftX(), controller.getLeftY(), - controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), - controller.getRightX(), controller.getRightY()}; - short button = 0; - for (int j = 0; j < 10; j++) - if (controller.getRawButton(j+1)) - button |= 1 << j; - short[] POV = {(short)(controller.getPOV())}; - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[i] = controllerFrame; - } - - frames.add(frame); - - swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), - new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), - true); // Really jank way of doing this. - - } - @Override - public void end(boolean interrupted) { - try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { - // header: size of 0x5 - // byte Number of axes per controller - // byte Number of POVs per controller - // byte Number of controllers - // short Number of frames - stream.write(new byte[]{6, 1, (byte) controllers.length}); - stream.write(DataUtils.shortToByteArray((short) frames.size())); - - // frame - // controller frame * number of controllers - // int unix time stamp. - for (AutoRecordingFrame frame : frames) { - // controller frame - // double axis * Number of axes per controller - // short button states - // short POV * Number of POVs per controller - for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { - for (double axis: controllerFrame.axes) { - stream.write(DataUtils.doubleToByteArray(axis)); - } - stream.write(DataUtils.shortToByteArray(controllerFrame.button)); - for (short POV: controllerFrame.POV) { - stream.write(DataUtils.shortToByteArray(POV)); - } - } - stream.write(DataUtils.intToByteArray(frame.timeStamp)); - } - System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); - } catch (Exception e) { - e.printStackTrace(); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java deleted file mode 100644 index 2a0d045..0000000 --- a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc4388.robot.commands; - -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; - -import java.util.function.BooleanSupplier; - -/** - * A command composition that runs one of two commands, depending on the value of the given - * condition when this command is initialized. - * - *

The rules for command compositions apply: command instances that are passed to it cannot be - * added to any other composition or scheduled individually, and the composition requires all - * subsystems its components require. - * - *

This class is provided by the NewCommands VendorDep - */ -public class WhileTrueCommand extends Command { - private final Command m_whileTrue; - private final BooleanSupplier m_condition; - - /** - * Creates a new WhileTrueCommand. - * - * @param whileTrue the command to run while the condition is true - * @param condition the condition to determine which command to run - */ - @SuppressWarnings("this-escape") - public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { - m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); - m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); - - //CommandScheduler.getInstance().registerComposedCommands(whileTrue); - - // addRequirements(whileTrue.getRequirements()); - } - - @Override - public void initialize() { - if(m_condition.getAsBoolean()) - m_whileTrue.initialize(); - } - - @Override - public void execute() { - m_whileTrue.execute(); - - System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); - - if(!m_whileTrue.isFinished()) - return; - - if(m_condition.getAsBoolean()){ - m_whileTrue.end(false); - m_whileTrue.initialize(); - } - } - - @Override - public void end(boolean interrupted) { - m_whileTrue.end(interrupted); - } - - @Override - public boolean isFinished() { - return !m_condition.getAsBoolean() && m_whileTrue.isFinished(); - } - - @Override - public boolean runsWhenDisabled() { - return m_whileTrue.runsWhenDisabled(); - } - - @Override - public InterruptionBehavior getInterruptionBehavior() { - if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { - return InterruptionBehavior.kCancelSelf; - } else { - return InterruptionBehavior.kCancelIncoming; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addStringProperty("whileTrue", m_whileTrue::getName, null); - builder.addStringProperty( - "selected", - () -> { - if (m_whileTrue == null) { - return "null"; - } else { - return m_whileTrue.getName(); - } - }, - null); - } -} diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 770280d..5a1d2d3 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; import frc4388.utility.structs.Gains; diff --git a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java b/src/main/java/frc4388/robot/commands/alignment/RotTo45.java deleted file mode 100644 index 197cf8b..0000000 --- a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc4388.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -public class RotTo45 extends Command { - - SwerveDrive m_SwerveDrive; - Rotation2d targetAngle; - - - public RotTo45(SwerveDrive swerveDrive) { - m_SwerveDrive = swerveDrive; - - addRequirements(swerveDrive); - } - - @Override - public void initialize() { - targetAngle = new Rotation2d(); - } - - @Override - public void execute() { - m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle); - } - - @Override - public boolean isFinished() { - Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); - // TODO: Tune - return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5; - } - -} diff --git a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java deleted file mode 100644 index a23db69..0000000 --- a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.wait; - -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class waitSupplier extends Command { - /** Creates a new waitSupplier. */ - private final Supplier truth; - public waitSupplier(Supplier truth) { - this.truth = truth; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return truth.get(); - } -} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9ff0c32..b2c6e49 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6"; - public static final String GIT_DATE = "2026-03-07 20:08:15 MST"; - public static final String GIT_BRANCH = "PikesPeak"; - public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT"; - public static final long BUILD_UNIX_TIME = 1773001639109L; + public static final int GIT_REVISION = 182; + public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5"; + public static final String GIT_DATE = "2026-03-21 13:38:34 MDT"; + public static final String GIT_BRANCH = "MiraOrg"; + public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT"; + public static final long BUILD_UNIX_TIME = 1774129559544L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 5c2a083..3c54d38 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -117,6 +117,11 @@ public final class Constants { // Operator ready to shoot, but the driver conditions are bad public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE; + // LED color for when the indexer was stuck on ball and going in reverse + public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET; + + // LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled + public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED; public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE; // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java index f5a7245..49c75c0 100644 --- a/src/main/java/frc4388/robot/constants/FieldConstants.java +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -1,18 +1,27 @@ package frc4388.robot.constants; -import java.util.Arrays; +import static edu.wpi.first.units.Units.Meters; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; public final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + public static final double BUMP_OFFSET = 0.4; + public static final Transform2d BUMP_OFFSET_RED = new Transform2d( + Meters.of(BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d( + Meters.of(-BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + // Test april tag field layout // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( // Arrays.asList(new AprilTag[] { diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 8425c54..d293bd8 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -45,6 +45,13 @@ public class Intake extends SubsystemBase { io.setRollerOutput(state, 0); } + public double getRollerTarget() { + return state.rollerTargetOutput; + } + + public double getRollerSpeed() { + return state.rollerOutput; + } // public enum FieldZone { // // The robot should aim at the hub diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/led/LED.java similarity index 98% rename from src/main/java/frc4388/robot/subsystems/LED.java rename to src/main/java/frc4388/robot/subsystems/led/LED.java index 58a4591..07f2876 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/led/LED.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.led; import java.util.concurrent.TimeUnit; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 15f0edf..a131d10 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -9,16 +9,21 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; -import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.HubShiftTimer; +import frc4388.utility.compute.HubShiftTimer.ShiftInfo; +import frc4388.utility.compute.TimesNegativeOne; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -31,6 +36,9 @@ public class Shooter extends SubsystemBase { // Supplier m_swervePoseSupplier; public boolean badShooterVelocity; + public double distanceToHub = 5; + public double chassisXSpeed = 0; + public Shooter( @@ -76,16 +84,22 @@ public class Shooter extends SubsystemBase { this.mode = ShooterMode.Idle; } - + public double getDistanceToHub(){ + return distanceToHub; + } public void allowShooting() { shooterButtonReady = true; - } + } public void denyShooting() { shooterButtonReady = false; } + public double getBallVelocity() { + return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI; + //Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS) + } @AutoLogOutput public ShooterMode getMode() { @@ -102,40 +116,45 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter - + //Hub Shift logs + ShiftInfo info = HubShiftTimer.getShiftInfo(); + Logger.recordOutput("HubShift/IsActive", info.isActive()); + Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift()); + Logger.recordOutput("HubShift/Phase", info.phase().name()); Logger.processInputs("Shooter", state); + io.updateInputs(state); // Get robot positon and speeds ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI)); - - Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - - - // Calculate aim lead - // Get the current speed of the robot Translation2d robotSpeed = new Translation2d( chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond ); + Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION); + Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle().minus(m_SwerveDrive.getPose2d().getRotation()); + Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - // Calculate a point to aim ahead of the actual position. - Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); + if (TimesNegativeOne.isRed) { + chassisXSpeed = chassisSpeeds.vxMetersPerSecond; + } else { + chassisXSpeed = -chassisSpeeds.vxMetersPerSecond; + } + Translation2d fieldPos = robotPose2d.getTranslation(); // Get the robot's aim distance to hub - double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm()); - //Center of hub to cameras in inches + //Center of hub to cameras in meters Logger.recordOutput("Hub Dist", distanceToHub); boolean driverError = // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | - distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get() | + Math.abs(ang.getDegrees()) > ShooterConstants.AIM_ANGLE.get(); double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; @@ -145,14 +164,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); + double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond); double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT)); // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend)); Logger.recordOutput("Time to rev", revTime); switch (mode) { case Shooting: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); int bitmask = ( (shooterButtonReady ? 1 : 0) + @@ -166,7 +185,7 @@ public class Shooter extends SubsystemBase { m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; - case 0b001: // No errors and shoot button is pressed + case 0b001: // No errors and shoot button is pressed io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; @@ -179,13 +198,13 @@ public class Shooter extends SubsystemBase { case 0b100: // Driver error, button is not pressed case 0b101: // Driver error, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); break; case 0b110: // Driver error, bad shooter vel, button is not pressed case 0b111: // Driver error, bad shooter vel, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); break; } @@ -200,7 +219,7 @@ public class Shooter extends SubsystemBase { switch (bitmask2) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); break; @@ -211,7 +230,7 @@ public class Shooter extends SubsystemBase { case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); break; @@ -237,12 +256,11 @@ public class Shooter extends SubsystemBase { // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) ); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - - } - + + io.motorStalled(state, m_Intake, m_robotLED); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index d55e93b..fdf0af5 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -15,30 +15,37 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; - public static final double INDEXER_GEAR_RATIO = 1.; + public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out -- 0.77 or 1.29 + public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31 public static final double T_CONSTANT = 2; - + public static final double SHOOTER_RADIUS = 2/39.37; + public static final double INDEXER_RADIUS = 0.625/39.37; public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42); // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15); // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); - public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); - public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5); + public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2); + public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); - public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.); + public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.); + + + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); + public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 15); public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); @@ -47,20 +54,23 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { - // Model derived from points - // double speed = - // 1.11576*hubDistMeters*hubDistMeters + - // 0.318464*hubDistMeters + - // 30.6293; - double speed = - 5.6939*hubDistMeters + - 22.76545 + MODEL_TRIM.get(); + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { + double speed = 0; - // double speed = - // 0.00610938*hubDistMeters*hubDistMeters - // 5.65235*hubDistMeters + - // 22.82825; + if (Math.abs(chassisXSpeed) < 0.1){ + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + MODEL_TRIM.get(); + } else if (chassisXSpeed > 0){ + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get(); + + } else { // Negative is closer to hub + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get(); + } double max = SHOOTER_MAX_VELOCITY.get(); @@ -79,7 +89,7 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.08) + .withKP(0.02) .withKI(0.15) .withKD(0.0); @@ -88,7 +98,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index c7d3369..96cbd19 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; public interface ShooterIO { @AutoLog @@ -49,6 +51,7 @@ public interface ShooterIO { public default void updateInputs(ShooterState state) {} + public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {} public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 6ca3ce4..f92a760 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -3,11 +3,16 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Timer; +import frc4388.robot.constants.Constants; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; public class ShooterReal implements ShooterIO { @@ -19,6 +24,14 @@ public class ShooterReal implements ShooterIO { VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); // VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); + private final Timer m_stallTimerShooter = new Timer(); + private final Timer m_stallTimerIndexer = new Timer(); + private final Timer m_stallTimerRoller = new Timer(); + private boolean m_shooterStalling = false; + private boolean m_indexerStalling = false; + private boolean m_rollerStalling = false; + public String motorStall = ""; + public ShooterReal( TalonFX shooter1Motor, @@ -41,6 +54,59 @@ public class ShooterReal implements ShooterIO { // m_indexerVelocity.Slot = 0; } + @Override + public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { + motorStall = ""; + if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { + if (!m_shooterStalling) { + m_shooterStalling = true; + m_stallTimerShooter.restart(); + } + if (m_stallTimerShooter.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Shooter Stalled"; + System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)))); + System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))); + System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond))); + } + } else { + m_shooterStalling = false; + m_stallTimerShooter.reset(); + } + + if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) { + if (!m_indexerStalling) { + m_indexerStalling = true; + m_stallTimerIndexer.restart(); + } + if (m_stallTimerIndexer.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Indexer Stalled"; + System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput))); + } + } else { + m_indexerStalling = false; + m_stallTimerIndexer.reset(); + } + + if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) { + if (!m_rollerStalling) { + m_rollerStalling = true; + m_stallTimerRoller.restart(); + } + if (m_stallTimerRoller.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Roller Stalled"; + System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()))); + } + } else { + m_rollerStalling = false; + m_stallTimerRoller.reset(); + } + Logger.recordOutput("Stalled Motor: ", motorStall); + + } + @Override public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target; @@ -75,6 +141,8 @@ public class ShooterReal implements ShooterIO { // Math.abs(currentLimit.in(Amps)) > current && // Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity // ) { + state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput); + state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput); m_shooter1Motor.set(percentOutput); m_shooter2Motor.set(percentOutput); // } else { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java new file mode 100644 index 0000000..44937da --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -0,0 +1,204 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public class SimpleSwerveSim implements SwerveIO { + private Pose2d pose = new Pose2d(); + private Pose2d lastPose = pose; + private double vx = 0.0; + private double vy = 0.0; + private double omega = 0.0; + + private long lastTimeNs = System.nanoTime(); + + public SimpleSwerveSim() { + } + + public synchronized void setControl(SwerveRequest ctrl) { + if (ctrl == null) return; + + if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) { + vx = facingAngle.VelocityX; + vy = facingAngle.VelocityY; + + double currentAngle = pose.getRotation().getRadians(); + double targetAngle = facingAngle.TargetDirection.getRadians(); + double error = targetAngle - currentAngle; + + error = Math.atan2(Math.sin(error), Math.cos(error)); + + double kP = 5.0; + omega = error * kP; + return; + } + + if (ctrl instanceof SwerveRequest.FieldCentric fc) { + vx = fc.VelocityX; + vy = fc.VelocityY; + omega = fc.RotationalRate; + return; + } + + if (ctrl instanceof SwerveRequest.RobotCentric rc) { + double cos = pose.getRotation().getCos(); + double sin = pose.getRotation().getSin(); + double vxRobot = rc.VelocityX; + double vyRobot = rc.VelocityY; + vx = vxRobot * cos - vyRobot * sin; + vy = vxRobot * sin + vyRobot * cos; + omega = rc.RotationalRate; + return; + } + + if (ctrl instanceof SwerveRequest.SwerveDriveBrake) { + vx = 0; vy = 0; omega = 0; + return; + } + + ChassisSpeeds cs = tryGetSpeedsField(ctrl); + if (cs != null) { + vx = cs.vxMetersPerSecond; + vy = cs.vyMetersPerSecond; + omega = cs.omegaRadiansPerSecond; + } + } + + private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) { + try { + java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds"); + f.setAccessible(true); + Object o = f.get(ctrl); + if (o instanceof ChassisSpeeds) { + return (ChassisSpeeds) o; + } + } catch (NoSuchFieldException nsf) { + } catch (IllegalAccessException iae) { + } catch (SecurityException se) { + } + return null; + } + + private double tryGetDoubleField(Object obj, Class cls, String... names) { + for (String n : names) { + try { + java.lang.reflect.Field f = cls.getDeclaredField(n); + f.setAccessible(true); + Object val = f.get(obj); + if (val instanceof Number) { + return ((Number) val).doubleValue(); + } + } catch (NoSuchFieldException nsf) { + } catch (IllegalAccessException iae) { + } catch (Throwable t) { + } + } + return 0.0; + } + + + public synchronized void pointAtXY(double x, double y) { + Translation2d target = new Translation2d(x, y); + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + } + + @Override + public synchronized void updateInputs(SwerveState state) { + if (state == null) return; + + long now = System.nanoTime(); + double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9); + lastTimeNs = now; + + lastPose = pose; + double dxField; + double dyField; + + if (DriverStation.isAutonomous()) { + double dxRobot = vx * dt; + double dyRobot = vy * dt; + + Rotation2d r = pose.getRotation(); + double cos = r.getCos(); + double sin = r.getSin(); + + dxField = dxRobot * cos - dyRobot * sin; + dyField = dxRobot * sin + dyRobot * cos; + + } else { + dxField = vx * dt; + dyField = vy * dt; + + } + + + double dTheta = omega * dt; + + Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField)); + Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta)); + pose = new Pose2d(newTrans, newRot); + + state.lastPose = lastPose; + state.currentPose = pose; + state.speeds = new ChassisSpeeds(vx, vy, omega); + state.odometryRate = dt; + } + + @Override + public synchronized void resetPose(Pose2d p) { + if (p == null) return; + pose = p; + lastPose = p; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void tareEverything() { + pose = new Pose2d(); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void addVisionMeasurement(List poses) { + if (poses == null || poses.isEmpty()) return; + Pose2d visPose = poses.get(poses.size() - 1).pose(); + if (visPose != null) { + pose = visPose; + lastPose = visPose; + } + } + + public synchronized void pointAt(Translation2d target) { + if (target == null) return; + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + omega = 0.0; + } + + @Override + public synchronized void setLimits(double limitInAmps) { + } +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 9cb77d1..5523eec 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -5,7 +5,9 @@ package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -19,10 +21,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -63,6 +68,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private final PIDController m_rotationOverridePID = new PIDController( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + private boolean m_useRotationOverride = false; + private Translation2d m_rotationOverrideTarget = new Translation2d(); + /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain @@ -81,6 +94,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Handle exception as needed config = null; } + + PPHolonomicDriveController driveController = new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), // Translation PID + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF) + ); + driveController.setRotationTargetOverride(() -> { + if (!m_useRotationOverride) return Optional.empty(); + Rotation2d targetAngle = getPose2d() + .getTranslation() + .minus(m_rotationOverrideTarget) + .getAngle(); + return Optional.of(targetAngle); + }); + // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { @@ -92,11 +119,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. // Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for - // holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), + driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...) config, // The robot configuration () -> { // Boolean supplier that controls when the path will be mirrored for the red @@ -148,14 +171,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setInitalPose(Pose2d startingAutoPose){ initalPose2d = startingAutoPose; } - // MIRA public void setOdoPose(Pose2d pose) { - // if (pose == null) return; - // io.tareEverything(); - // initalPose2d = pose; - // io.resetPose(pose); - // robotKnowsWhereItIs = true; - // rotTarget = pose.getRotation().getDegrees(); - // } // public void oneModuleTest(SwerveModule module, Translation2d leftStick, @@ -172,6 +187,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // module.setDesiredState(state); // } + public double chassisXSpeeds(){ + if (TimesNegativeOne.isRed) { + return chassisSpeeds.vxMetersPerSecond; + } else { + return -chassisSpeeds.vxMetersPerSecond; + } + } + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve @@ -233,6 +256,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } + + + public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) { + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withTargetDirection(ang); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + io.setControl(ctrl); + } public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical // reason to have a robot @@ -301,7 +345,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // SmartDashboard.putBoolean("drift correction", true); } - + public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) { + + rotTarget = heading.getDegrees(); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + io.setControl(ctrl); + } public void driveIntake(Translation2d leftStick, boolean invertRotation){ // if (invert){ @@ -360,23 +417,57 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Drive with the robot facing towards a specific position public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { - - // Get the current speed of the robot - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - // Calculate a point to aim ahead of the actual position. - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - - // Calculate the angle between the current position and the lead position + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY()); + if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){ + if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) { + if (TimesNegativeOne.isRed){ + robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond); + } else { + robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond); + } + } } + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d()); + Logger.recordOutput("Lead Aim", fieldPosLeadLog); + driveFieldAngle(leftStick, ang); + } + public void offsetOdoPosition(Transform2d offset) { + // Manually performing an addittion on the pose + // WHY doesn't WPILIB have the ability to not transform poses + Pose2d new_pose = new Pose2d( + new Translation2d( + state.currentPose.getX() + offset.getX(), + state.currentPose.getY() + offset.getY() + ), + state.currentPose.getRotation() + ); + this.io.resetPose(new_pose); + } + + public void defenseXPosition(){ + io.setControl(new SwerveRequest.SwerveDriveBrake()); + } + + public void stopDefenseXPosition(){ + stopModules(); + } + + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + // Calculate the angle between the current position and the lead position + //Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle(); + Rotation2d ang = new Rotation2d(0,1); + System.out.println(ang); driveFieldAngle(leftStick, ang); } + public Pose2d getCurrentPose(){ + return state.currentPose; + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading); @@ -464,6 +555,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { softStop(); } + public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) { + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); + m_rotationOverrideTarget = fieldPosLead; + m_useRotationOverride = true; + } + + public void disableRotationOverride() { + m_useRotationOverride = false; + } + @Override public void periodic() { // This method will be called once per scheduler run\ @@ -476,6 +579,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable { vision.setLastOdomPose(state.currentPose); setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + if (state.speeds != null) { + this.chassisSpeeds = state.speeds; + } else { + this.chassisSpeeds = new ChassisSpeeds(); + } + if (vision.isTag()) { Pose2d pose = vision.getPose2d(); if (!robotKnowsWhereItIs) { @@ -485,6 +594,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } io.addVisionMeasurement(vision.getPosesToAdd()); + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); + + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); } // if(e.isPresent()) @@ -515,6 +629,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { gear_index = i; } + public void setPercentOutput(double speed) { speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; @@ -589,5 +704,4 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return status; } -} - +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index e295b7f..597b7a3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -73,9 +73,10 @@ public final class SwerveDriveConstants { public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; + public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); - private static final class ModuleSpecificConstants { //2025 + public static final class ModuleSpecificConstants { //2026 //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index fa7de1a..25db223 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; public interface SwerveIO { @AutoLog diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index c4ab4d6..9574699 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; public class SwerveReal implements SwerveIO { SwerveDrivetrain swerveDriveTrain; @@ -54,7 +56,7 @@ public class SwerveReal implements SwerveIO { swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime())); } } - + @Override public void setLimits(double limitInAmps) { for (SwerveModule module : swerveDriveTrain.getModules()) { diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java similarity index 98% rename from src/main/java/frc4388/robot/subsystems/Lidar.java rename to src/main/java/frc4388/robot/subsystems/vision/Lidar.java index fdbf2a6..d859102 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java @@ -1,4 +1,4 @@ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.vision; import java.util.ArrayList; import java.util.LinkedList; @@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.RPLidarA1.PolarPoint; -import frc4388.robot.subsystems.RPLidarA1.ScanListener; +import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint; +import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.FaultA1M8; diff --git a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java similarity index 99% rename from src/main/java/frc4388/robot/subsystems/RPLidarA1.java rename to src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java index fa5aa32..41ad20a 100644 --- a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java +++ b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java @@ -1,4 +1,4 @@ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.vision; import com.fazecast.jSerialComm.SerialPort; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc4388/utility/compute/HubShiftTimer.java b/src/main/java/frc4388/utility/compute/HubShiftTimer.java new file mode 100644 index 0000000..6f91284 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/HubShiftTimer.java @@ -0,0 +1,154 @@ + +package frc4388.utility.compute; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +public class HubShiftTimer { + + public enum ShiftPhase { + DISABLED, + AUTO, + TRANSITION, // 0 – 10 s + SHIFT1, // 10 – 35 s + SHIFT2, // 35 – 60 s + SHIFT3, // 60 – 85 s + SHIFT4, // 85 – 110 s + ENDGAME // 110 – 140 s + } + + public record ShiftInfo( + ShiftPhase phase, + double elapsedInShift, + double remainingInShift, + boolean isActive) {} +//total teleop time + public static final double TELEOP_DURATION = 140.0; +//total auto time + public static final double AUTO_DURATION = 20.0; + +//shift start and end times for calculations + private static final double[] SHIFT_STARTS = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] SHIFT_ENDS = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + +//hub active schedule, true is active and false is inactive +//starts always as active becasue transition is first and is active, but then is inactive for winner or active for loser + private static final boolean[] WINNER_SCHEDULE = {true, false, true, false, true, true}; + private static final boolean[] LOSER_SCHEDULE = {true, true, false, true, false, true}; + +//shift phase names during teleop + private static final ShiftPhase[] SHIFT_PHASES = { + ShiftPhase.TRANSITION, + ShiftPhase.SHIFT1, + ShiftPhase.SHIFT2, + ShiftPhase.SHIFT3, + ShiftPhase.SHIFT4, + ShiftPhase.ENDGAME + }; + +//timer to track time + private static final Timer teleopTimer = new Timer(); + private static double timerOffset = 0.0; + +//fms syncing idk other team did it too + private static final double RESYNC_THRESHOLD = 3.0; + + +//call at start of auto to start timer + public static void initializeAuto() { + teleopTimer.restart(); + } + +//call at start of teleop to start timer again because sometimes delay between auto and telop + public static void initializeTeleop() { + timerOffset = 0.0; + teleopTimer.restart(); + } + + + +//returns the updated shift info based on the winner of auto + public static ShiftInfo getShiftInfo() { + if (!DriverStation.isEnabled()) { + return new ShiftInfo(ShiftPhase.DISABLED, 0.0, 0.0, false); + } + if (DriverStation.isAutonomousEnabled()) { + double autoElapsed = teleopTimer.get(); // timer restarts in initialize() + return new ShiftInfo( + ShiftPhase.AUTO, + autoElapsed, + Math.max(0.0, AUTO_DURATION - teleopTimer.get()), + true); + } + return computeTeleopShift(); + } + +//find auto winner, R = red wins, B = blue wins + public static Alliance autoWinnerAlliance() { + String msg = DriverStation.getGameSpecificMessage(); + if (msg != null && msg.length() > 0) { + char c = msg.charAt(0); + if (c == 'R') return Alliance.Red; + if (c == 'B') return Alliance.Blue; + } + // backup if no msg, returns auto winner as opposite of our alliance. if we red -> blue wins auto + Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue); + return (ours == Alliance.Blue) ? Alliance.Red : Alliance.Blue; + } + + + //return our schedule for the shifts + private static boolean[] getSchedule() { + Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue); + Alliance winner = autoWinnerAlliance(); + return (ours == winner) ? WINNER_SCHEDULE : LOSER_SCHEDULE; + } + +//time since start of teleop + private static double getTeleopElapsed() { + double localTime = teleopTimer.get() - timerOffset; + + // Re-sync to FMS time if we drift too far (only when FMS is attached) + if (DriverStation.isFMSAttached()) { + double fmsElapsed = TELEOP_DURATION - DriverStation.getMatchTime(); + if (fmsElapsed <= TELEOP_DURATION - 5.0 // ignore the first few seconds of jitter + && Math.abs(localTime - fmsElapsed) >= RESYNC_THRESHOLD) { + timerOffset += localTime - fmsElapsed; + localTime = fmsElapsed; + } + } + return Math.max(0.0, Math.min(TELEOP_DURATION, localTime)); + } + + private static ShiftInfo computeTeleopShift() { + boolean[] schedule = getSchedule(); + double elapsed = getTeleopElapsed(); + + // Find which shift we're in + int phaseIndex = SHIFT_STARTS.length - 1; // default to last shift if past all bounds + for (int i = 0; i < SHIFT_STARTS.length; i++) { + if (elapsed >= SHIFT_STARTS[i] && elapsed < SHIFT_ENDS[i]) { + phaseIndex = i; + break; + } + } + + double shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex]; + double shiftRemaining = SHIFT_ENDS[phaseIndex] - elapsed; + + // merge time for elapsed if same active/inactive + if (phaseIndex > 0 && schedule[phaseIndex] == schedule[phaseIndex - 1]) { + shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex - 1]; + } + + // merge time for remaining time if same active/inactive status + if (phaseIndex < SHIFT_ENDS.length - 1 && schedule[phaseIndex] == schedule[phaseIndex + 1]) { + shiftRemaining = SHIFT_ENDS[phaseIndex + 1] - elapsed; + } + + return new ShiftInfo( + SHIFT_PHASES[phaseIndex], + shiftElapsed, + shiftRemaining, + schedule[phaseIndex]); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/status/CanDevice.java b/src/main/java/frc4388/utility/status/CanDevice.java index ef72647..ed49c4f 100644 --- a/src/main/java/frc4388/utility/status/CanDevice.java +++ b/src/main/java/frc4388/utility/status/CanDevice.java @@ -49,5 +49,10 @@ public class CanDevice { return s; } + + public Object getId() { + return id; + } + } diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java index aedb866..e99eb3e 100644 --- a/src/main/java/frc4388/utility/status/FaultA1M8.java +++ b/src/main/java/frc4388/utility/status/FaultA1M8.java @@ -1,7 +1,7 @@ package frc4388.utility.status; -import frc4388.robot.subsystems.RPLidarA1; -import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus; +import frc4388.robot.subsystems.vision.RPLidarA1; +import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus; import frc4388.utility.status.Status.ReportLevel; // Fault reporter for the RPLidar A1M8 Revolving lidar sensor