diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f0f03625..461215d3 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.85, + "robotLength": 0.952, "holonomicMode": true, "pathFolders": [], "autoFolders": [], diff --git a/simgui-ds.json b/simgui-ds.json index ba7e2e64..def7ba7b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,21 +4,16 @@ "visible": true } }, - "System Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ { - "decKey": 65, - "incKey": 68 + "decKey": 68, + "incKey": 65 }, { - "decKey": 87, - "incKey": 83 + "decKey": 83, + "incKey": 87 }, { "decKey": 69, @@ -103,6 +98,9 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index d078f7a2..bdde3437 100644 --- a/simgui.json +++ b/simgui.json @@ -14,8 +14,17 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/Shuffleboard/Pre-match/Alliance": "String Chooser", "/Shuffleboard/Pre-match/Mode": "String Chooser", + "/SmartDashboard/AllianceColor": "String Chooser", "/SmartDashboard/AutonomousMode": "String Chooser" + }, + "windows": { + "/Shuffleboard/Pre-match/Mode": { + "window": { + "visible": true + } + } } }, "NetworkTables": { @@ -26,6 +35,18 @@ "AdvantageKit": { "open": true }, + "Shuffleboard": { + "Pre-match": { + "Mode": { + "open": true, + "string[]##v_/Shuffleboard/Pre-match/Mode/options": { + "open": true + } + }, + "open": true + }, + "open": true + }, "SmartDashboard": { "TunableNumbers": { "Flywheel": { @@ -41,6 +62,24 @@ } }, "NetworkTables Info": { + "AdvantageScope@1": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "Clients": { + "open": true + }, + "Server": { + "open": true + }, "visible": true + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path b/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path new file mode 100644 index 00000000..4b8a0e53 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path @@ -0,0 +1,170 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.741887381054855, + "y": 6.695925443427855 + }, + "prevControl": null, + "nextControl": { + "x": 1.3249004714516954, + "y": 6.550174838486252 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.511115021072635, + "y": 6.9832349513151 + }, + "prevControl": { + "x": 2.212410438221992, + "y": 6.990643205161632 + }, + "nextControl": { + "x": 2.824296168340601, + "y": 6.975467660245216 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5082724716954066, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 2.125198127424631, + "y": 5.366730902594161 + }, + "nextControl": { + "x": 1.4444384407793702, + "y": 5.501367852634432 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.388440350339902, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 1.6754803456796465, + "y": 5.471219489931063 + }, + "nextControl": { + "x": 2.7106112717723123, + "y": 5.496661640675712 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5082724716954066, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 2.7225892863967442, + "y": 5.521583593948438 + }, + "nextControl": { + "x": 1.4220056676836053, + "y": 5.486410069694874 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.786221898253982, + "y": 7.42479630901102 + }, + "prevControl": { + "x": 7.15588177827108, + "y": 7.266533230042734 + }, + "nextControl": { + "x": 8.416562018236883, + "y": 7.583059387979306 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5082724716954066, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 2.672514522173267, + "y": 5.695384322269494 + }, + "nextControl": { + "x": 1.3904431950573972, + "y": 5.467829597262204 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.786221898253982, + "y": 5.7846202012571855 + }, + "prevControl": { + "x": 6.538274967970654, + "y": 7.594698262132346 + }, + "nextControl": { + "x": 9.03416882853731, + "y": 3.9745421403820247 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.834244879861288, + "y": 4.06537564992086 + }, + "prevControl": { + "x": 6.9342068541992985, + "y": 4.043525214419497 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4NoteAuto.path b/src/main/deploy/pathplanner/paths/4NoteAuto.path new file mode 100644 index 00000000..2c01b0e6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4NoteAuto.path @@ -0,0 +1,138 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.31, + "y": 5.501318674946378 + }, + "prevControl": null, + "nextControl": { + "x": 1.867126604928507, + "y": 5.503274647896266 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4, + "y": 6.975077486323955 + }, + "prevControl": { + "x": 1.8081527395083707, + "y": 6.967733500350662 + }, + "nextControl": { + "x": 2.6463913517236755, + "y": 6.978134853911492 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3116855915786485, + "y": 5.501318674946378 + }, + "prevControl": { + "x": 1.6315317275186487, + "y": 5.491396511661543 + }, + "nextControl": { + "x": 1.273899737015866, + "y": 5.50249085549415 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.337981361851478, + "y": 5.501318674946378 + }, + "prevControl": { + "x": 1.7239291602064108, + "y": 5.488891565963109 + }, + "nextControl": { + "x": 2.7674829050953003, + "y": 5.510010871590367 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.48, + "y": 5.501318674946378 + }, + "prevControl": { + "x": 1.8923419239969768, + "y": 5.493805623244927 + }, + "nextControl": { + "x": 1.1300580828207984, + "y": 5.507694771004687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.41, + "y": 4.13064739682429 + }, + "prevControl": { + "x": 2.2297949844380596, + "y": 4.460855194207495 + }, + "nextControl": { + "x": 2.441636277389571, + "y": 4.072677059863143 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3116855915786485, + "y": 5.501318674946378 + }, + "prevControl": { + "x": 1.3949049103972053, + "y": 4.901336962468485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 67490b70..691c318d 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 1.4706390139415118, + "y": 6.328270263890066 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 2.4706390139415104, + "y": 5.828270263890066 }, "isLocked": false, "linkedName": null @@ -55,12 +55,50 @@ "x": 5.380848911075716, "y": 2.0910493199386275 }, + "nextControl": { + "x": 1.4118781121676838, + "y": 1.1917246647355006 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.819079116601521, + "y": 6.328270263890066 + }, + "prevControl": { + "x": 4.117083609492741, + "y": 4.035665082434044 + }, + "nextControl": { + "x": 7.521074623710301, + "y": 8.620875445346087 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.2230432314597275, + "y": 4.91798685745809 + }, + "prevControl": { + "x": 6.628203460598891, + "y": 7.094390818899554 + }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 30.0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -71,7 +109,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 173.99592794454193, + "rotation": 127.90464256381038, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 00000000..5d0ddcf5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.5369885437759883, + "y": 6.346372292498636 + }, + "prevControl": null, + "nextControl": { + "x": 0.8982933063884068, + "y": 6.990846822333112 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8964426341396086, + "y": 6.764852148700247 + }, + "prevControl": { + "x": -1.7366713444726078, + "y": 7.130405677984391 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.78899295289287, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -179.490584237178, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt index 07f4b7cd..e866bb4b 100644 --- a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt +++ b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt @@ -38,8 +38,10 @@ fun Rotation3d.toDoubleArray(): Array { return arrayOf(this.x.inRadians, this.y.inRadians, this.z.inRadians) } -fun Transform2d.toDoubleArray(): Array { - return arrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians) +fun Transform2d.toDoubleArray(): DoubleArray { + return doubleArrayOf( + this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians + ) } fun Transform3d.toDoubleArray(): Array { diff --git a/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCamera.kt b/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCamera.kt index e5ff228e..08b2ba42 100644 --- a/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCamera.kt +++ b/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCamera.kt @@ -32,6 +32,9 @@ import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.Timer import org.photonvision.PhotonVersion import org.photonvision.common.dataflow.structures.Packet +import org.photonvision.common.dataflow.structures.PacketSerde +import org.photonvision.targeting.PNPResult.APacketSerde +import org.photonvision.targeting.PhotonTrackedTarget /** Represents a camera that is connected to PhotonVision. */ class PhotonCamera(instance: NetworkTableInstance, val name: String?) { @@ -75,12 +78,14 @@ class PhotonCamera(instance: NetworkTableInstance, val name: String?) { packet.clear() // Create latest result. - val ret = PhotonPipelineResult() + var ret = PhotonPipelineResult() + val photonPacketSerde = PhotonPipelineResult.APacketSerde() // Populate packet and create result. packet.data = rawBytesEntry.getRaw(byteArrayOf()) if (packet.size < 1) return ret - ret.createFromPacket(packet) + + ret = photonPacketSerde.unpack(packet) // Set the timestamp of the result. // getLatestChange returns in microseconds so we divide by 1e6 to convert to seconds. diff --git a/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCameraSim.kt b/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCameraSim.kt index fe03f336..c43b72e8 100644 --- a/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCameraSim.kt +++ b/src/main/kotlin/com/team4099/lib/sim/utils/sim/PhotonCameraSim.kt @@ -59,6 +59,7 @@ import edu.wpi.first.wpilibj.Timer import org.opencv.core.Size import org.photonvision.PhotonVersion import org.photonvision.common.dataflow.structures.Packet +import org.photonvision.targeting.PhotonPipelineResult.APacketSerde import java.lang.Exception import java.util.ArrayList @@ -377,7 +378,8 @@ class PhotonCameraSim @JvmOverloads constructor( fun submitProcessedFrame(result: PhotonPipelineResult) { latencyMillisEntry.setDouble(result.latencyMillis) val newPacket = Packet(result.packetSize) - result.populatePacket(newPacket) + val photonPipelineSerde = APacketSerde() + photonPipelineSerde.pack(newPacket, result) camera.rawBytesEntry.setRaw(newPacket.data) val hasTargets = result.hasTargets() hasTargetEntry.setBoolean(hasTargets) diff --git a/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt b/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt index 98e8caab..af5ee6bc 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt @@ -22,7 +22,7 @@ abstract class Waypoint { constructor( translation: Translation2d, heading: Rotation2d? = null, - holonomicRotation: Rotation2d? = null, + holonomicRotation: Rotation2d? = null ) { this.translation = ErrorMessages.requireNonNullParam(translation, "translation", "Waypoint") this.driveRotation = heading diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 50ab43e5..cfe0e8e8 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -3,6 +3,7 @@ package com.team4099.robot2023 import com.team4099.lib.hal.Clock import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.auto.PathStore +import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.util.Alert @@ -10,10 +11,14 @@ import com.team4099.robot2023.util.Alert.AlertType import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.NTSafePublisher import edu.wpi.first.hal.AllianceStationID +import edu.wpi.first.networktables.GenericEntry +import edu.wpi.first.networktables.NetworkTableValue import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.PowerDistribution import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj.livewindow.LiveWindow +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.simulation.DriverStationSim import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler @@ -43,6 +48,7 @@ object Robot : LoggedRobot() { val logSimulationAlert = Alert("Running in simulation", AlertType.INFO) val logTuningModeEnabled = Alert("Tuning Mode Enabled. Expect loop times to be greater", AlertType.WARNING) + lateinit var allianceSelected: GenericEntry /* val port0 = AnalogInput(0) val port1 = AnalogInput(1) @@ -129,16 +135,27 @@ object Robot : LoggedRobot() { CommandScheduler.getInstance().onCommandInterrupt { command: Command -> Logger.recordOutput("/ActiveCommands/${command.name}", false) } + + val autoTab = Shuffleboard.getTab("Pre-match") + allianceSelected = + autoTab + .add("Alliance Selected", "No alliance") + .withPosition(0, 1) + .withWidget(BuiltInWidgets.kTextView) + .entry } override fun autonomousInit() { - RobotContainer.zeroSensors() - FMSData.allianceColor = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + RobotContainer.zeroSensors(isInAutonomous = true) RobotContainer.setDriveBrakeMode() RobotContainer.setSteeringBrakeMode() RobotContainer.getAutonomousCommand().schedule() } + override fun disabledPeriodic() { + FMSData.allianceColor = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + } + override fun disabledInit() { RobotContainer.getAutonomousCommand().cancel() RobotContainer.setSteeringCoastMode() @@ -175,6 +192,17 @@ object Robot : LoggedRobot() { Logger.recordOutput("LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds) + ControlBoard.rumbleConsumer.accept(RobotContainer.rumbleState) + + val currentAlliance = + try { + DriverStation.getAlliance().get().toString() + } catch (_: NoSuchElementException) { + "No alliance" + } + + allianceSelected.set(NetworkTableValue.makeString(currentAlliance)) + /* Logger.recordOutput("LoggedRobot/port0", port0.voltage) Logger.recordOutput("LoggedRobot/port1", port1.voltage) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index e9a34644..386130da 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand +import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -27,14 +28,17 @@ import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision -import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar +import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision import com.team4099.robot2023.subsystems.wrist.Wrist -import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim +import com.team4099.robot2023.subsystems.wrist.WristIOTalon import com.team4099.robot2023.util.driver.Ryan +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import org.team4099.lib.smoothDeadband import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.degrees import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { @@ -48,37 +52,26 @@ object RobotContainer { private val wrist: Wrist val superstructure: Superstructure + val rumbleState + get() = feeder.rumbleTrigger + init { if (RobotBase.isReal()) { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) - vision = - Vision( - // object: CameraIO {} - // CameraIONorthstar("northstar"), - CameraIONorthstar("northstar_1"), - CameraIONorthstar("northstar_2"), - CameraIONorthstar("northstar_3"), - // CameraIONorthstar("right"), - // CameraIONorthstar("backward") - ) + vision = Vision(CameraIOPhotonvision("parakeet_1")) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIONEO) feeder = Feeder(FeederIONeo) elevator = Elevator(ElevatorIONEO) flywheel = Flywheel(FlywheelIOTalon) - wrist = Wrist(object : WristIO {}) + wrist = Wrist(WristIOTalon) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - vision = - Vision( - CameraIONorthstar("northstar_1"), - CameraIONorthstar("northstar_2"), - CameraIONorthstar("northstar_3"), - ) + vision = Vision(CameraIOPhotonvision("parakeet_1")) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOSim) feeder = Feeder(FeederIOSim) @@ -87,7 +80,7 @@ object RobotContainer { wrist = Wrist(WristIOSim) } - superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel) + superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain) vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }) limelight.poseSupplier = { drivetrain.odomTRobot } } @@ -97,8 +90,8 @@ object RobotContainer { drivetrain.defaultCommand = TeleopDriveCommand( driver = Ryan(), - { -ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { -ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain @@ -120,8 +113,8 @@ object RobotContainer { drivetrain.zeroSteering() } - fun zeroSensors() { - drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() + fun zeroSensors(isInAutonomous: Boolean = false) { + drivetrain.currentRequest = DrivetrainRequest.ZeroSensors(isInAutonomous) } fun zeroAngle(toAngle: Angle) { @@ -150,16 +143,91 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) - ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) + ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand()) + ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand()) + + ControlBoard.prepHighProtected.whileTrue( + ParallelCommandGroup( + superstructure.prepSpeakerMidCommand(), + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + 30.degrees, + ) + ) + ) + ControlBoard.prepHigh.whileTrue(superstructure.prepSpeakerHighCommand()) + ControlBoard.score.whileTrue(superstructure.scoreCommand()) + ControlBoard.extendClimb.whileTrue(superstructure.climbExtendCommand()) + ControlBoard.retractClimb.whileTrue(superstructure.climbRetractCommand()) + ControlBoard.forceIdle.whileTrue(superstructure.requestIdleCommand()) + ControlBoard.prepLow.whileTrue(superstructure.prepSpeakerLowCommand()) + ControlBoard.prepTrap.whileTrue(superstructure.prepTrapCommand()) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) - ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) - ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand()) - ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand()) - ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand()) - ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand()) - ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) - ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) - ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) + + ControlBoard.targetAmp.whileTrue( + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + 270.0.degrees + ) + ) + + ControlBoard.climbAlignFar.whileTrue( + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + 0.0.degrees + else 180.0.degrees + ) + ) + + ControlBoard.climbAlignLeft.whileTrue( + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + 120.degrees + else -60.degrees + ) + ) + + ControlBoard.climbAlignRight.whileTrue( + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + -120.0.degrees + else 60.0.degrees, + ) + ) /* TUNING COMMANDS @@ -187,7 +255,7 @@ object RobotContainer { fun mapTestControls() {} - fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain) + fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain, superstructure) fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index 606e4021..e1d5522e 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -1,7 +1,16 @@ package com.team4099.robot2023.auto +import com.team4099.robot2023.auto.mode.FourNoteAutoPath +import com.team4099.robot2023.auto.mode.FourNoteLeftCenterLine +import com.team4099.robot2023.auto.mode.FourNoteMiddleCenterLine +import com.team4099.robot2023.auto.mode.FourNoteRightCenterLine +import com.team4099.robot2023.auto.mode.PreloadAndLeaveCenterSubwooferAutoPath +import com.team4099.robot2023.auto.mode.PreloadAndLeaveLeftSubwooferAutoPath +import com.team4099.robot2023.auto.mode.PreloadAndLeaveRightSubwooferAutoPath import com.team4099.robot2023.auto.mode.TestAutoPath import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import com.team4099.robot2023.util.AllianceFlipUtil import edu.wpi.first.networktables.GenericEntry import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard @@ -28,21 +37,43 @@ object AutonomousSelector { // orientationChooser.addOption("Right", 270.degrees) // autoTab.add("Starting Orientation", orientationChooser) - autonomousModeChooser.addOption("Test", AutonomousMode.TEST_AUTO_PATH) + autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH) + + autonomousModeChooser.addOption( + "Four Note Right Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH + ) + autonomousModeChooser.addOption( + "Four Note Middle Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_MIDDLE_AUTO_PATH + ) + autonomousModeChooser.addOption( + "Four Note LEFT Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_LEFT_AUTO_PATH + ) + autonomousModeChooser.addOption( + "Preload + Leave from Left Side of Subwoofer", + AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER + ) + autonomousModeChooser.addOption( + "Preload + Leave from Right Side of Subwoofer", + AutonomousMode.PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER + ) + autonomousModeChooser.addOption( + "Preload + Leave from Center Side of Subwoofer", + AutonomousMode.PRELOAD_AND_LEAVE_CENTER_SUBWOOFER + ) // autonomousModeChooser.addOption("Characterize Elevator", // AutonomousMode.ELEVATOR_CHARACTERIZE) autoTab.add("Mode", autonomousModeChooser.sendableChooser).withSize(4, 2).withPosition(2, 0) waitBeforeCommandSlider = autoTab - .add("Wait Time before Running Auto", 0) + .add("Wait Time Before Shooting", 0) .withSize(3, 2) .withPosition(0, 2) .withWidget(BuiltInWidgets.kTextView) .entry secondaryWaitInAuto = autoTab - .add("Secondary Wait Time During Auto Path", 0) + .add("Secondary Wait Time Between Shooting and Driving", 0) .withSize(3, 2) .withPosition(3, 2) .withWidget(BuiltInWidgets.kTextView) @@ -55,19 +86,103 @@ object AutonomousSelector { val secondaryWaitTime: Time get() = secondaryWaitInAuto.getDouble(0.0).seconds - fun getCommand(drivetrain: Drivetrain): Command { - val mode = AutonomousMode.TEST_AUTO_PATH - // val mode = autonomousModeChooser.get() - // println("${waitTime().inSeconds} wait command") + fun getCommand(drivetrain: Drivetrain, superstructure: Superstructure): Command { + val mode = autonomousModeChooser.get() + when (mode) { AutonomousMode.TEST_AUTO_PATH -> - return WaitCommand(waitTime.inSeconds).andThen(TestAutoPath(drivetrain)) + return WaitCommand(waitTime.inSeconds) + .andThen({ + drivetrain.tempZeroGyroYaw(TestAutoPath.startingPose.rotation) + drivetrain.resetFieldFrameEstimator( + AllianceFlipUtil.apply(TestAutoPath.startingPose) + ) + }) + .andThen(TestAutoPath(drivetrain)) + AutonomousMode.FOUR_NOTE_AUTO_PATH -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteAutoPath(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteRightCenterLine.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteRightCenterLine(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_MIDDLE_AUTO_PATH -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteMiddleCenterLine.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteMiddleCenterLine(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_LEFT_AUTO_PATH -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteLeftCenterLine.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteMiddleCenterLine(drivetrain, superstructure)) + AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(PreloadAndLeaveLeftSubwooferAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen( + PreloadAndLeaveLeftSubwooferAutoPath( + drivetrain, superstructure, secondaryWaitTime + ) + ) + AutonomousMode.PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(PreloadAndLeaveRightSubwooferAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen( + PreloadAndLeaveRightSubwooferAutoPath( + drivetrain, superstructure, secondaryWaitTime + ) + ) + AutonomousMode.PRELOAD_AND_LEAVE_CENTER_SUBWOOFER -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(PreloadAndLeaveCenterSubwooferAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen( + PreloadAndLeaveCenterSubwooferAutoPath( + drivetrain, superstructure, secondaryWaitTime + ) + ) else -> println("ERROR: unexpected auto mode: $mode") } return InstantCommand() } private enum class AutonomousMode { - TEST_AUTO_PATH + TEST_AUTO_PATH, + FOUR_NOTE_AUTO_PATH, + FOUR_NOTE_RIGHT_AUTO_PATH, + FOUR_NOTE_MIDDLE_AUTO_PATH, + FOUR_NOTE_LEFT_AUTO_PATH, + PRELOAD_AND_LEAVE_LEFT_SUBWOOFER, + PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER, + PRELOAD_AND_LEAVE_CENTER_SUBWOOFER } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt index d1e4b419..e0dd96b9 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt @@ -1,7 +1,6 @@ package com.team4099.robot2023.auto.mode import com.team4099.robot2023.auto.PathStore -import com.team4099.robot2023.commands.drivetrain.FollowPathPlannerPathCommand import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.wpilibj2.command.SequentialCommandGroup @@ -13,7 +12,6 @@ class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() { addCommands( ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)), - FollowPathPlannerPathCommand(drivetrain, PathStore.examplePath) ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt new file mode 100644 index 00000000..8f9c4581 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt @@ -0,0 +1,80 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FiveNoteCenterlineAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.51.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Middle wing note + FieldWaypoint( + Translation2d(2.51.meters, 6.98.meters).translation2d, + null, + 235.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d(3.meters, 6.98.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Right in front of the leftmost wing note + FieldWaypoint( + Translation2d(7.79.meters, 7.42.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost center line note + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(7.79.meters, 5.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Second leftmost wing note + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + ) + }, + resetPose = true + ) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt new file mode 100644 index 00000000..639f7f6d --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -0,0 +1,147 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.9.meters - 0.75.meters, 6.9.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.9.meters, 6.9.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(1.435.meters, 5.535.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + }, + keepTrapping = false + ), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.435.meters, 5.535.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters - 0.75.meters, 4.14.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.41.meters + 0.225.meters, 4.14.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(1.42.meters, 5.535.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + }, + keepTrapping = false + ), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.42.meters, 5.535.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + 0.25.meters, + 5.55.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + 0.25.meters, + 5.45.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(1.36.meters, 5.535.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + }, + keepTrapping = false + ), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand() + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.42.meters, 5.535.meters), 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteLeftCenterLine.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteLeftCenterLine.kt new file mode 100644 index 00000000..22b93f65 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteLeftCenterLine.kt @@ -0,0 +1,121 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteLeftCenterLine(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(startingPose.x, startingPose.y, startingPose.rotation)), + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.39.meters, 4.15.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(0.76.meters, 6.79.meters).translation2d, + null, + 60.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(0.78.meters, 4.33.meters).translation2d, + null, + -120.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.72.meters, 0.84.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(0.78.meters, 4.33.meters).translation2d, + null, + -120.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(0.78.meters, 4.33.meters).translation2d, + null, + 60.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.88.meters, 2.0.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(7.7.meters, 2.38.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(4.88.meters, 2.0.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(0.78.meters, 4.33.meters).translation2d, + null, + 60.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand() + ) + } + companion object { + val startingPose = Pose2d(Translation2d(0.78.meters, 4.33.meters), 60.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteMiddleCenterLine.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteMiddleCenterLine.kt new file mode 100644 index 00000000..d5723f9a --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteMiddleCenterLine.kt @@ -0,0 +1,144 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteMiddleCenterLine(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(startingPose.x, startingPose.y, startingPose.rotation)), + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.0.meters, 5.525.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.4.meters, 5.5.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.0.meters, 5.475.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(1.48.meters + 3.inches, 5.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.87.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(7.72.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(4.87.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(1.48.meters + 3.inches, 5.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.87.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(7.72.meters, 2.47.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(4.87.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(1.48.meters + 3.inches, 5.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand() + ) + } + companion object { + val startingPose = Pose2d(Translation2d(1.46.meters, 5.5.meters), 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteRightCenterLine.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteRightCenterLine.kt new file mode 100644 index 00000000..fc6f1dfa --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteRightCenterLine.kt @@ -0,0 +1,119 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteRightCenterLine(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.scoreCommand(), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.25.meters, 6.99.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(0.76.meters, 6.79.meters).translation2d, + null, + -120.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(0.76.meters, 6.79.meters).translation2d, + null, + -120.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.0.meters, 7.4.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(0.76.meters, 6.79.meters).translation2d, + null, + -120.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(0.76.meters, 6.79.meters).translation2d, + null, + -120.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.71.meters, 6.41.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(7.5.meters, 5.81.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(5.71.meters, 6.41.meters).translation2d, + 180.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(0.76.meters, 6.79.meters).translation2d, + null, + -120.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.scoreCommand() + ) + } + companion object { + val startingPose = Pose2d(Translation2d(0.76.meters, 6.79.meters), -120.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt new file mode 100644 index 00000000..5c0f702d --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt @@ -0,0 +1,56 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class PreloadAndLeaveCenterSubwooferAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure, + secondaryWaitTime: Time +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.scoreCommand(), + WaitCommand(secondaryWaitTime.inSeconds), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.87.meters, 6.27.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.16.meters, 6.69.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + ) + } + + companion object { + val startingPose = Pose2d(1.41.meters, 5.55.meters, 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveLeftSubwooferAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveLeftSubwooferAutoPath.kt new file mode 100644 index 00000000..d395b717 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveLeftSubwooferAutoPath.kt @@ -0,0 +1,61 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class PreloadAndLeaveLeftSubwooferAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure, + secondaryWaitTime: Time +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.scoreCommand(), + WaitCommand(secondaryWaitTime.inSeconds), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(1.90.meters, 6.76.meters).translation2d, + null, + -180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.87.meters, 6.27.meters).translation2d, + null, + -180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.16.meters, 6.69.meters).translation2d, + null, + -180.degrees.inRotation2ds + ) + ) + } + ) + ) + } + + companion object { + val startingPose = Pose2d(0.75.meters, 6.70.meters, (-120).degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveRightSubwooferAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveRightSubwooferAutoPath.kt new file mode 100644 index 00000000..f52545b2 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveRightSubwooferAutoPath.kt @@ -0,0 +1,56 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class PreloadAndLeaveRightSubwooferAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure, + secondaryWaitTime: Time +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.scoreCommand(), + WaitCommand(secondaryWaitTime.inSeconds), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(3.29.meters, 1.58.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.98.meters, 1.58.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + ) + } + + companion object { + val startingPose = Pose2d(0.7.meters, 4.4.meters, 120.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndResetAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndResetAutoPath.kt new file mode 100644 index 00000000..4a622c94 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndResetAutoPath.kt @@ -0,0 +1,32 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees + +class PreloadAndResetAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure, + secondaryWaitTime: Time +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.scoreCommand(), + WaitCommand(secondaryWaitTime.inSeconds), + ResetPoseCommand(drivetrain, startingPose) + ) + } + + companion object { + val startingPose = Pose2d(1.41.meters, 5.55.meters, 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt new file mode 100644 index 00000000..2da3ff8c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt @@ -0,0 +1,80 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class SixNoteCenterlineAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.51.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters, 4.14.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Rightmost wing note + FieldWaypoint( + Translation2d(2.41.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Middle wing note + FieldWaypoint( + Translation2d(2.41.meters, 7.03.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d(7.79.meters, 7.42.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost center line note + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(7.79.meters, 5.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Second leftmost wing note + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + }, + resetPose = true + ) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt new file mode 100644 index 00000000..24bcb535 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt @@ -0,0 +1,95 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class SixNoteCenterlineWithPickupAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.51.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters, 4.14.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Rightmost wing note + FieldWaypoint( + Translation2d(2.41.meters, 5.49.meters).translation2d, + null, + 235.degrees.inRotation2ds + ), // Middle wing note + FieldWaypoint( + Translation2d(2.41.meters, 7.03.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d((2.41.meters + 7.79.meters) / 2, 7.21.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d(7.79.meters, 7.21.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost center line note + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(7.79.meters, 5.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Second leftmost wing note + FieldWaypoint( + Translation2d(4.83.meters, 4.07.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.83.meters, 4.07.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.77.meters, 4.03.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + }, + resetPose = true + ) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 541c97d1..479d917d 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -1,11 +1,12 @@ package com.team4099.robot2023.auto.mode -import com.team4099.lib.trajectory.OdometryWaypoint +import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Translation2d -import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds @@ -14,29 +15,38 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { addRequirements(drivetrain) addCommands( - DrivePathCommand.createPathInOdometryFrame( + DrivePathCommand.createPathInFieldFrame( drivetrain, { listOf( - OdometryWaypoint( - Translation2d(5.0.meters, 2.0.meters).translation2d, + FieldWaypoint( + startingPose.translation.translation2d, null, - 0.0.degrees.inRotation2ds + startingPose.rotation.inRotation2ds ), - OdometryWaypoint( - Translation2d(7.0.meters, 2.0.meters).translation2d, + FieldWaypoint( + Translation2d(16.0.feet, 10.0.feet).translation2d, null, - 90.0.degrees.inRotation2ds + 210.0.degrees.inRotation2ds ), - OdometryWaypoint( - Translation2d(7.0.meters, 3.0.meters).translation2d, + FieldWaypoint( + Translation2d(13.0.feet, 11.0.feet).translation2d, null, - 0.0.degrees.inRotation2ds + 180.degrees.inRotation2ds ), + FieldWaypoint( + Translation2d(10.0.feet, 10.0.feet).translation2d, + null, + 180.degrees.inRotation2ds + ) ) }, resetPose = true ) ) } + + companion object { + val startingPose = Pose2d(10.feet, 10.feet, 180.0.degrees) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/CharacterizeWristCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/CharacterizeWristCommand.kt new file mode 100644 index 00000000..cd6328c4 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/CharacterizeWristCommand.kt @@ -0,0 +1,34 @@ +package com.team4099.robot2023.commands + +import com.team4099.lib.hal.Clock +import com.team4099.robot2023.subsystems.wrist.Wrist +import edu.wpi.first.wpilibj2.command.Command +import org.team4099.lib.units.base.inMilliseconds +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +class CharacterizeWristCommand(val wrist: Wrist, val positive: Boolean) : Command() { + private var currentVoltage = 0.volts + private var lastCallTime = Clock.fpgaTime + + init { + addRequirements(wrist) + } + + override fun execute() { + if ((Clock.fpgaTime - lastCallTime).inMilliseconds > 500) { + currentVoltage += if (positive) 0.01.volts else (-0.01).volts + wrist.setWristVoltage(currentVoltage) + lastCallTime = Clock.fpgaTime + } + } + + override fun isFinished(): Boolean { + return wrist.inputs.wristVelocity.absoluteValue > 0.5.degrees.perSecond + } + + override fun end(interrupted: Boolean) { + wrist.setWristVoltage(0.0.volts) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt new file mode 100644 index 00000000..64a2d1b0 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt @@ -0,0 +1,105 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +import com.team4099.lib.logging.LoggedTunableNumber +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.math.MathUtil +import edu.wpi.first.math.filter.SlewRateLimiter +import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.base.inInches +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.angle +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.radians +import kotlin.math.abs +import kotlin.math.hypot + +class WheelRadiusCharacterizationCommand( + val drivetrain: Drivetrain, + val omegaDirection: Direction +) : Command() { + val omegaLimiter = SlewRateLimiter(1.0) + var lastGyroYawRads: Angle = 0.0.radians + var accumGyroYawRads: Angle = 0.0.radians + lateinit var startWheelPositions: List + var currentEffectiveWheelRadius = 0.0.meters + + val characterizationSpeed = + LoggedTunableNumber("Drivetrain/wheelRadiusCharacterizationRadPerSec", 0.1) + val driveRadius: Double = + hypot( + (DrivetrainConstants.DRIVETRAIN_LENGTH / 2).inMeters, + (DrivetrainConstants.DRIVETRAIN_WIDTH / 2).inMeters + ) + val gyroYawSupplier = { drivetrain.odomTRobot.rotation } + + init { + addRequirements(drivetrain) + } + + override fun initialize() { + lastGyroYawRads = gyroYawSupplier() + accumGyroYawRads = 0.0.radians + startWheelPositions = drivetrain.swerveModules.map { it.modulePosition.angle.angle } + omegaLimiter.reset(0.0) + } + + override fun execute() { + // Run drive at velocity + drivetrain.currentRequest = + Request.DrivetrainRequest.ClosedLoop( + ChassisSpeeds( + 0.0, + 0.0, + omegaLimiter.calculate(omegaDirection.value * characterizationSpeed.get()) + ) + ) + + // Get yaw and wheel positions + accumGyroYawRads += + MathUtil.angleModulus((gyroYawSupplier() - lastGyroYawRads).inRadians).radians + lastGyroYawRads = gyroYawSupplier() + var averageWheelPosition = 0.0 + val wheelPositions = drivetrain.swerveModules.map { it.modulePosition.angle.angle } + for (i in 0 until 4) { + averageWheelPosition += abs((wheelPositions[i] - startWheelPositions[i]).inRadians) + } + + averageWheelPosition /= 4.0 + currentEffectiveWheelRadius = + (accumGyroYawRads.inRadians * driveRadius / averageWheelPosition).meters + Logger.recordOutput("Drivetrain/RadiusCharacterization/DrivePosition", averageWheelPosition) + Logger.recordOutput( + "Drivetrain/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads.inRadians + ) + Logger.recordOutput( + "Drivetrain/RadiusCharacterization/CurrentWheelRadiusInches", + currentEffectiveWheelRadius.inInches + ) + } + + override fun end(interrupted: Boolean) { + if (accumGyroYawRads <= (Math.PI * 2.0).radians) { + println("Not enough data for characterization") + } else { + println("Effective Wheel Radius: ${currentEffectiveWheelRadius.inInches} inches") + } + } + + companion object { + enum class Direction(val value: Int) { + CLOCKWISE(-1), + COUNTER_CLOCKWISE(1) + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 22b8e298..42efb8a7 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -11,7 +11,6 @@ import com.team4099.lib.trajectory.OdometryWaypoint import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.FrameType import com.team4099.robot2023.util.Velocity2d @@ -21,6 +20,7 @@ import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGeneratio import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController @@ -122,26 +122,25 @@ private constructor( val poskP = LoggedTunableValue( - "Pathfollow/poskP", + "Pathfollow/posKP", DrivetrainConstants.PID.AUTO_POS_KP, Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) ) val poskI = LoggedTunableValue( - "Pathfollow/poskI", + "Pathfollow/posKI", DrivetrainConstants.PID.AUTO_POS_KI, Pair({ it.inMetersPerSecondPerMeterSeconds }, { it.meters.perSecond.perMeterSeconds }) ) val poskD = LoggedTunableValue( - "Pathfollow/poskD", + "Pathfollow/posKD", DrivetrainConstants.PID.AUTO_POS_KD, Pair( { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } ) ) - private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)) private var lastSampledPose = Pose2d() private lateinit var pathTransform: Transform2d @@ -184,6 +183,12 @@ private constructor( init { addRequirements(drivetrain) + if (RobotBase.isSimulation()) { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + } + xPID = PIDController(poskP.get(), poskI.get(), poskD.get()) yPID = PIDController(poskP.get(), poskI.get(), poskD.get()) thetaPID = @@ -209,7 +214,11 @@ private constructor( xPID.wpiPidController, yPID.wpiPidController, thetaPID.wpiPidController ) - swerveDriveController.setTolerance(Pose2d(0.5.inches, 0.5.inches, 2.5.degrees).pose2d) + if (keepTrapping) { + swerveDriveController.setTolerance(Pose2d(3.inches, 3.inches, 2.5.degrees).pose2d) + } else { + swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 10.degrees).pose2d) + } } override fun initialize() { @@ -217,7 +226,7 @@ private constructor( pathTransform = Transform2d( Translation2d(waypoints.get()[0].translation), - waypoints.get()[0].driveRotation?.radians?.radians ?: drivePoseSupplier().rotation + waypoints.get()[0].holonomicRotation?.radians?.radians ?: drivePoseSupplier().rotation ) // trajectory generation! @@ -252,38 +261,71 @@ private constructor( trajectoryGenerator.holonomicRotationSequence.sample(trajCurTime.inSeconds) val targetHolonomicPose = - Pose2d( - desiredState.poseMeters.x.meters, - desiredState.poseMeters.y.meters, - desiredRotation.position.radians.radians + AllianceFlipUtil.apply( + Pose2d( + desiredState.poseMeters.x.meters, + desiredState.poseMeters.y.meters, + desiredRotation.position.radians.radians + ) ) - val robotPoseInSelectedFrame: Pose2d = drivePoseSupplier() + var robotPoseInSelectedFrame: Pose2d = drivePoseSupplier() if (pathFrame == stateFrame) { lastSampledPose = targetHolonomicPose + when (stateFrame) { + FrameType.FIELD -> { + Logger.recordOutput( + "Pathfollow/fieldTRobotTargetVisualized", + targetHolonomicPose.toDoubleArray().toDoubleArray() + ) + + Logger.recordOutput( + "Pathfollow/fieldTRobot", robotPoseInSelectedFrame.toDoubleArray().toDoubleArray() + ) + } + FrameType.ODOMETRY -> { + Logger.recordOutput( + "Pathfollow/odomTRobotTargetVisualized", + targetHolonomicPose.toDoubleArray().toDoubleArray() + ) + + Logger.recordOutput( + "Pathfollow/odomTRobot", robotPoseInSelectedFrame.toDoubleArray().toDoubleArray() + ) + } + } + // pathTransform.inverse().asPose2d().transformBy(targetHolonomicPose.asTransform2d()) } else { when (pathFrame) { - FrameType.ODOMETRY -> + FrameType.ODOMETRY -> { + // TODO (saraansh) we disallow this, not possible to get to. remove or find use case lastSampledPose = odoTField.inverse().asPose2d().transformBy(targetHolonomicPose.asTransform2d()) - FrameType.FIELD -> + } + FrameType.FIELD -> { + // robotPose is currently odomTrobot we want fieldTRobot. we obtain that via fieldTodo x + // odoTRobot + robotPoseInSelectedFrame = + odoTField.inverse().asPose2d().transformBy(robotPoseInSelectedFrame.asTransform2d()) lastSampledPose = odoTField.asPose2d().transformBy(targetHolonomicPose.asTransform2d()) + + Logger.recordOutput( + "Pathfollow/fieldTRobotTargetVisualized", + targetHolonomicPose.toDoubleArray().toDoubleArray() + ) + + Logger.recordOutput( + "Pathfollow/fieldTRobot", robotPoseInSelectedFrame.toDoubleArray().toDoubleArray() + ) + } } } // flip lastSampledPose = AllianceFlipUtil.apply(lastSampledPose) Logger.recordOutput( - "Pathfollow/frameSpecificTargetPose", lastSampledPose.toDoubleArray().toDoubleArray() - ) - val pathFrameTRobotPose = robotPoseInSelectedFrame - Logger.recordOutput( - "Pathfollow/fieldTRobotVisualized", pathFrameTRobotPose.toDoubleArray().toDoubleArray() - ) - Logger.recordOutput( - "Pathfollow/fieldTRobotTargetVisualized", - targetHolonomicPose.toDoubleArray().toDoubleArray() + "Pathfollow/targetPoseInStateFrame", lastSampledPose.toDoubleArray().toDoubleArray() ) if (flipForAlliances) { @@ -299,7 +341,9 @@ private constructor( desiredState.curvatureRadPerMeter.radians.sin var nextDriveState = - swerveDriveController.calculate(pathFrameTRobotPose.pose2d, desiredState, desiredRotation) + swerveDriveController.calculate( + robotPoseInSelectedFrame.pose2d, desiredState, desiredRotation + ) if (leaveOutYAdjustment) { nextDriveState = @@ -367,7 +411,7 @@ private constructor( xPID.integralGain = poskI.get() yPID.integralGain = poskI.get() } - if (poskD.hasChanged()) { + if (poskD.hasChanged() && poskD.hasChanged()) { xPID.derivativeGain = poskD.get() yPID.derivativeGain = poskD.get() } @@ -376,8 +420,10 @@ private constructor( override fun isFinished(): Boolean { trajCurTime = Clock.fpgaTime - trajStartTime return endPathOnceAtReference && - (swerveDriveController.atReference()) && - trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds + ( + (swerveDriveController.atReference()) && + trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds + ) } override fun end(interrupted: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt deleted file mode 100644 index eaca7e80..00000000 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ /dev/null @@ -1,332 +0,0 @@ -package com.team4099.robot2023.commands.drivetrain - -import com.pathplanner.lib.path.PathPlannerPath -import com.pathplanner.lib.path.PathPlannerTrajectory -import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.lib.logging.toDoubleArray -import com.team4099.lib.math.asPose2d -import com.team4099.lib.math.asTransform2d -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.FrameType -import com.team4099.robot2023.util.inverse -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.kinematics.ChassisSpeeds -import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger -import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Transform2d -import org.team4099.lib.geometry.Translation2d -import org.team4099.lib.hal.Clock -import org.team4099.lib.pplib.PathPlannerHolonomicDriveController -import org.team4099.lib.pplib.PathPlannerRotationPID -import org.team4099.lib.pplib.PathPlannerTranslationPID -import org.team4099.lib.units.base.inMeters -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.base.inches -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds -import org.team4099.lib.units.derived.inMetersPerSecondPerMeter -import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds -import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond -import org.team4099.lib.units.derived.inRadians -import org.team4099.lib.units.derived.inRotation2ds -import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds -import org.team4099.lib.units.derived.perMeter -import org.team4099.lib.units.derived.perMeterSeconds -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.perSecond - -class FollowPathPlannerPathCommand( - val drivetrain: Drivetrain, - val path: PathPlannerPath, - var stateFrame: FrameType = FrameType.ODOMETRY, - var pathFrame: FrameType = FrameType.FIELD, -) : Command() { - private val translationToleranceAtEnd = 1.inches - private val thetaToleranceAtEnd = 2.5.degrees - - private var swerveDriveController: PathPlannerHolonomicDriveController - - private var trajCurTime = 0.0.seconds - private var trajStartTime = 0.0.seconds - - private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)) - private var lastSampledPose = Pose2d() - - private var drivePoseSupplier: () -> Pose2d - private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees) - - private val atReference: Boolean - get() = - ( - (lastSampledPose.translation - drivePoseSupplier().translation).magnitude.meters <= - translationToleranceAtEnd && - (lastSampledPose.rotation - drivePoseSupplier().rotation).absoluteValue <= - thetaToleranceAtEnd - ) - - val thetakP = - LoggedTunableValue( - "Pathfollow/thetakP", - DrivetrainConstants.PID.AUTO_THETA_PID_KP, - Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) - ) - val thetakI = - LoggedTunableValue( - "Pathfollow/thetakI", - DrivetrainConstants.PID.AUTO_THETA_PID_KI, - Pair( - { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } - ) - ) - val thetakD = - LoggedTunableValue( - "Pathfollow/thetakD", - DrivetrainConstants.PID.AUTO_THETA_PID_KD, - Pair( - { it.inDegreesPerSecondPerDegreePerSecond }, - { it.degrees.perSecond.perDegreePerSecond } - ) - ) - - val thetaMaxVel = - LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL) - val thetaMaxAccel = - LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) - - val poskP = - LoggedTunableValue( - "Pathfollow/poskP", - DrivetrainConstants.PID.AUTO_POS_KP, - Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) - ) - val poskI = - LoggedTunableValue( - "Pathfollow/poskI", - DrivetrainConstants.PID.AUTO_POS_KI, - Pair({ it.inMetersPerSecondPerMeterSeconds }, { it.meters.perSecond.perMeterSeconds }) - ) - val poskD = - LoggedTunableValue( - "Pathfollow/poskD", - DrivetrainConstants.PID.AUTO_POS_KD, - Pair( - { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } - ) - ) - - var translationPID: PathPlannerTranslationPID - var rotationPID: PathPlannerRotationPID - // private val thetaPID: PIDController> - // private val posPID: PIDController> - // private val pathConstraints = PathPlannerHolonomicDriveController.Companion.PathConstraints( - // maxVelocity = DrivetrainConstants.MAX_AUTO_VEL, - // maxAcceleration = DrivetrainConstants.MAX_AUTO_ACCEL, - // maxAngularVelocity = thetaMaxVel.get(), - // maxAngularAcceleration = thetaMaxAccel.get() - // ) - - init { - addRequirements(drivetrain) - - when (stateFrame) { - FrameType.ODOMETRY -> drivePoseSupplier = { drivetrain.odomTRobot } - FrameType.FIELD -> { - drivePoseSupplier = { drivetrain.fieldTRobot } - // if we're already in field frame we do not want to shift by `odoTField` again - pathFrame = FrameType.FIELD - } - } - - translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get()) - rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get()) - - swerveDriveController = - PathPlannerHolonomicDriveController( - translationPID, - rotationPID, - DrivetrainConstants.MAX_AUTO_VEL, - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, - DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - .magnitude - .meters, - ) - } - - private lateinit var currentSpeeds: ChassisSpeeds - private lateinit var poseRotation: Rotation2d - private lateinit var generatedTrajectory: PathPlannerTrajectory - - private lateinit var pathTransform: Transform2d - - override fun initialize() { - trajStartTime = Clock.fpgaTime - odoTField = drivetrain.odomTField - - currentSpeeds = drivetrain.targetedChassisSpeeds - poseRotation = drivePoseSupplier().rotation.inRotation2ds - generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) - pathTransform = Pose2d(path.previewStartingHolonomicPose).asTransform2d() - } - - override fun execute() { - if (thetakP.hasChanged() || - thetakI.hasChanged() || - thetakD.hasChanged() || - poskP.hasChanged() || - poskI.hasChanged() || - poskD.hasChanged() - ) { - translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get()) - rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get()) - - swerveDriveController = - PathPlannerHolonomicDriveController( - translationPID, - rotationPID, - DrivetrainConstants.MAX_AUTO_VEL, - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, - DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - .magnitude - .meters, - ) - } - - val currentSpeeds = drivetrain.targetedChassisSpeeds - val poseRotation = drivetrain.odomTRobot.rotation.inRotation2ds - val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) - - // Sampling the trajectory for a state that we're trying to target - val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds) - - // Retrieves the last sampled pose, so we can keep our `atReference` variable updated - Logger.recordOutput( - "Odometry/targetPose", - doubleArrayOf( - lastSampledPose.x.inMeters, - lastSampledPose.y.inMeters, - lastSampledPose.rotation.inRadians - ) - ) - val robotPoseInSelectedFrame: Pose2d = drivePoseSupplier() - if (pathFrame == stateFrame) { - // odoTField x fieldTRobot - lastSampledPose = - (Pose2d(stateFromTrajectory.targetHolonomicPose) - pathTransform.asPose2d()).asPose2d() - } else { - when (pathFrame) { - FrameType.ODOMETRY -> - lastSampledPose = - odoTField - .asPose2d() - .transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d()) - FrameType.FIELD -> - lastSampledPose = - odoTField - .inverse() - .asPose2d() - .transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d()) - } - } - - Logger.recordOutput("Pathfollow/thetaSetpointDegrees", lastSampledPose.rotation.inDegrees) - Logger.recordOutput("Pathfollow/currentThetaDegrees", drivePoseSupplier().rotation.inDegrees) - - // var targetedChassisSpeeds = - // swerveDriveController.calculateRobotRelativeSpeeds( - // (robotPoseInSelectedFrame + pathTransform), stateFromTrajectory - // ) - - val pathFrameTRobotPose = (robotPoseInSelectedFrame + pathTransform) - - val targettedSpeeds = - swerveDriveController.calculateRobotRelativeSpeeds(pathFrameTRobotPose, stateFromTrajectory) - // // Calculate feedforward velocities (field-relative). - // val xFF = (stateFromTrajectory.velocityMps * - // stateFromTrajectory.heading.cos).meters.perSecond - // val yFF = (stateFromTrajectory.velocityMps * - // stateFromTrajectory.heading.sin).meters.perSecond - // - // // Calculate feedback velocities (based on position error). - // val xFeedback = - // posPID.calculate(pathFrameTRobotPose.x, stateFromTrajectory.positionMeters.x.meters) - // val yFeedback = - // posPID.calculate(pathFrameTRobotPose.y, stateFromTrajectory.positionMeters.y.meters) - // val thetaFeedback = - // thetaPID.calculate( - // pathFrameTRobotPose.rotation, - // stateFromTrajectory.targetHolonomicRotation.radians.radians - // ) - // - // // Return next output. - // val targetedChassisSpeeds = - // org.team4099.lib.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( - // xFF + xFeedback, yFF + yFeedback, thetaFeedback, pathFrameTRobotPose.rotation - // ) - // - // Logger.recordOutput("Pathfollow/yFF+F", (yFF + yFeedback).inMetersPerSecond) - // Logger.recordOutput("Pathfollow/xFF+F", (xFF + xFeedback).inMetersPerSecond) - // Logger.recordOutput("Pathfollow/thetaFeedback", (thetaFeedback).inDegreesPerSecond) - // Logger.recordOutput( - // "Pathfollow/thetaFeedbackNorm", targetedChassisSpeeds.omega.inDegreesPerSecond - // ) - - Logger.recordOutput( - "Pathfollow/fieldTRobotVisualized", - (robotPoseInSelectedFrame + pathTransform).toDoubleArray().toDoubleArray() - ) - Logger.recordOutput( - "Pathfollow/fieldTRobotTargetVisualized", - Pose2d(stateFromTrajectory.targetHolonomicPose).toDoubleArray().toDoubleArray() - ) - - // Set closed loop request - pathFollowRequest.chassisSpeeds = targettedSpeeds.chassisSpeedsWPILIB - drivetrain.currentRequest = pathFollowRequest - - // Update trajectory time - trajCurTime = Clock.fpgaTime - trajStartTime - } - - // override fun isFinished(): Boolean { - // trajCurTime = Clock.fpgaTime - trajStartTime - // return endPathOnceAtReference && - // (!keepTrapping || swerveDriveController.atReference()) && - // trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds - // } - override fun isFinished(): Boolean { - return atReference && trajCurTime.inSeconds > generatedTrajectory.totalTimeSeconds - } - - override fun end(interrupted: Boolean) { - if (interrupted) { - // Stop where we are if interrupted - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) - } else { - // Execute one last time to end up in the final state of the trajectory - // Since we weren't interrupted, we know curTime > endTime - execute() - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) - } - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index 96c58b6f..d305748e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain +import com.team4099.lib.logging.toDoubleArray import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Pose2dWPILIB class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() { init { @@ -14,13 +14,15 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() override fun initialize() { drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose)) + drivetrain.zeroGyroYaw(AllianceFlipUtil.apply(pose).rotation) Logger.recordOutput( - "Drivetrain/lastResetPose", Pose2dWPILIB.struct, AllianceFlipUtil.apply(pose).pose2d + "Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).toDoubleArray().toDoubleArray() ) Logger.recordOutput("ActiveCommands/ResetPoseCommand", true) } override fun isFinished(): Boolean { + Logger.recordOutput("ActiveCommands/ResetPoseCommand", false) return true } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt new file mode 100644 index 00000000..fb723c9a --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt @@ -0,0 +1,140 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.driver.DriverProfile +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.math.PI + +class TargetAngleCommand( + val driver: DriverProfile, + val driveX: () -> Double, + val driveY: () -> Double, + val turn: () -> Double, + val slowMode: () -> Boolean, + val drivetrain: Drivetrain, + val targetAngle: Angle +) : Command() { + private var thetaPID: PIDController> + val thetakP = + LoggedTunableValue( + "Pathfollow/thetaAmpkP", + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetaAmpkI", + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + Pair( + { it.inDegreesPerSecondPerDegreePerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + init { + addRequirements(drivetrain) + + thetaPID = + PIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + ) + + if (!(RobotBase.isSimulation())) { + + thetakP.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD) + + thetaPID = + PIDController( + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP, + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI, + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD + ) + } else { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + + thetaPID = + PIDController( + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP, + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI, + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD + ) + } + + thetaPID.enableContinuousInput(-PI.radians, PI.radians) + } + + override fun initialize() { + thetaPID.reset() // maybe do first for x? + /* + if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { + thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) + } + + */ + } + + override fun execute() { + + drivetrain.defaultCommand.end(true) + Logger.recordOutput("ActiveCommands/TargetAngleCommand", true) + Logger.recordOutput( + "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees + ) + + val thetaFeedback = thetaPID.calculate(drivetrain.odomTRobot.rotation, targetAngle) + Logger.recordOutput("Testing/error", thetaPID.error.inDegrees) + Logger.recordOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) + + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = true + ) + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + Logger.recordOutput("ActiveCommands/TargetAngleCommand", false) + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + driver.rotationSpeedClampedSupplier(turn, slowMode), + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = true + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 4ed0d7ce..6ff97742 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -33,4 +33,8 @@ class TeleopDriveCommand( override fun isFinished(): Boolean { return false } + + override fun end(interrupted: Boolean) { + Logger.recordOutput("ActiveCommands/TeleopDriveCommand", false) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 3a0019ff..57485833 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -21,10 +21,10 @@ object ControlBoard { } val strafe: Double - get() = driver.leftXAxis + get() = -driver.leftXAxis val forward: Double - get() = driver.leftYAxis + get() = -driver.leftYAxis val turn: Double get() = driver.rightXAxis @@ -34,34 +34,37 @@ object ControlBoard { val resetGyro = Trigger { driver.startButton && driver.selectButton } - val shooterUp = Trigger { driver.bButton } - val shooterDown = Trigger { driver.xButton } - val wristTestUp = Trigger { driver.yButton } - val wristTestDown = Trigger { driver.aButton } - val feederTest = Trigger { driver.rightShoulderButton } - - val elevatorUp = Trigger { driver.rightTriggerAxis > 0.5 } - val elevatorDown = Trigger { driver.leftTriggerAxis > 0.5 } - - val runGroundIntake = Trigger { driver.leftShoulderButton } - val ejectGamePiece = Trigger { driver.rightShoulderButton } - val prepAmpScore = Trigger { driver.xButton } - val ampScore = Trigger { driver.yButton } - - val scoreSpeakerLow = Trigger { operator.aButton } - val scoreSpeakerMid = Trigger { operator.bButton } - val scoreSpeakerHigh = Trigger { operator.xButton } - val requestIdle = Trigger { operator.yButton } - - val climbExtend = Trigger { driver.dPadUp } - val climbRetract = Trigger { driver.dPadDown } - - // testing Trigger - val testIntake = Trigger { driver.aButton } - val testFeederIntake = Trigger { driver.bButton } - val testFeederShoot = Trigger { driver.xButton } - val testFlywheel = Trigger { driver.yButton } - val testWrist = Trigger { operator.aButton } - val testElevator = Trigger { operator.bButton } - val setTuningMode = Trigger { driver.rightShoulderButton } + // sim triggers + val score = Trigger { driver.leftTriggerAxis > 0.5 } + val intake = Trigger { driver.rightShoulderButton } + val forceIdle = Trigger { driver.dPadDown || operator.startButton && operator.selectButton } + + // sim trigger + // val score = Trigger {driver.bButton} + // val intake = Trigger { driver.xButton} + + // real triggers + // val score = Trigger {driver.leftTriggerAxis > 0.5} + // val intake = Trigger { driver.rightShoulderButton} + + val targetAmp = Trigger { driver.aButton } + val prepAmp = Trigger { operator.aButton } + val prepLow = Trigger { operator.xButton } + val prepHighProtected = Trigger { operator.bButton } + val prepHigh = Trigger { operator.yButton } + + val extendClimb = Trigger { operator.dPadUp } + val retractClimb = Trigger { operator.dPadDown } + + val prepTrap = Trigger { operator.rightShoulderButton } + val ejectGamePiece = Trigger { driver.rightTriggerAxis > 0.5 } + + val testWrist = Trigger { driver.aButton } + + val characterizeWrist = Trigger { driver.rightShoulderButton } + + val climbAlignFar = Trigger { driver.yButton } + val climbAlignLeft = Trigger { driver.xButton } + val climbAlignRight = Trigger { driver.bButton } + // week0 controls } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index e5c2f98d..e6d2737e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -1,7 +1,9 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.milli +import org.team4099.lib.units.perSecond typealias GamePiece = Constants.Universal.GamePiece @@ -11,6 +13,7 @@ typealias NodeTier = Constants.Universal.NodeTier object Constants { object Universal { + val gravity = -9.8.meters.perSecond.perSecond val SIM_MODE = Tuning.SimType.SIM const val REAL_FIELD = false @@ -51,7 +54,7 @@ object Constants { object Tuning { - const val TUNING_MODE = true + const val TUNING_MODE = false const val DEBUGING_MODE = true const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 51f6e70c..72f6ae64 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -50,8 +50,8 @@ object DrivetrainConstants { .perSecond // 648 // cruise velocity and accel for steering motor - val STEERING_VEL_MAX = 270.degrees.perSecond - val STEERING_ACCEL_MAX = 600.degrees.perSecond.perSecond + val STEERING_VEL_MAX = 151.degrees.perSecond + val STEERING_ACCEL_MAX = 302.degrees.perSecond.perSecond const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value @@ -59,7 +59,7 @@ object DrivetrainConstants { val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond val MAX_AUTO_VEL = 3.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 3.meters.perSecond.perSecond // 3 + val MAX_AUTO_ACCEL = 2.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3 @@ -73,11 +73,11 @@ object DrivetrainConstants { val ALLOWED_STEERING_ANGLE_ERROR = 1.degrees val STEERING_SUPPLY_CURRENT_LIMIT = 20.0.amps - val DRIVE_SUPPLY_CURRENT_LIMIT = 35.0.amps + val DRIVE_SUPPLY_CURRENT_LIMIT = 60.0.amps val DRIVE_THRESHOLD_CURRENT_LIMIT = 60.0.amps val DRIVE_TRIGGER_THRESHOLD_TIME = 0.1.seconds - val DRIVE_STATOR_CURRENT_LIMIT = 65.0.amps + val DRIVE_STATOR_CURRENT_LIMIT = 60.0.amps val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds @@ -101,7 +101,7 @@ object DrivetrainConstants { val AUTO_POS_KP: ProportionalGain> get() { if (RobotBase.isReal()) { - return 8.0.meters.perSecond / 1.0.meters + return 0.23.meters.perSecond / 1.0.meters // todo:4 } else { return 7.0.meters.perSecond / 1.0.meters } @@ -118,7 +118,8 @@ object DrivetrainConstants { val AUTO_POS_KD: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + return (0.14.meters.perSecond / (1.0.meters.perSecond)) + .metersPerSecondPerMetersPerSecond // todo: 0.25 } else { return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond } @@ -126,21 +127,20 @@ object DrivetrainConstants { val AUTO_THETA_ALLOWED_ERROR = 3.degrees - val AUTO_THETA_PID_KP = 15.degrees.perSecond / 1.degrees + val AUTO_THETA_PID_KP = 0.1.degrees.perSecond / 1.degrees // 0.1 val AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val AUTO_THETA_PID_KD = - (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.02.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val SIM_AUTO_THETA_PID_KP = 15.degrees.perSecond / 1.degrees + val TELEOP_ALIGN_PID_KP = 4.degrees.perSecond / 1.degrees + val TELEOP_ALIGN_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) + val TELEOP_ALIGN_PID_KD = + (0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + + val SIM_AUTO_THETA_PID_KP = 5.degrees.perSecond / 1.degrees val SIM_AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val SIM_AUTO_THETA_PID_KD = - (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - - val AUTO_LEVEL_KP = 1.meters.perSecond / 1.0.degrees // tune this - val AUTO_LEVEL_KI = 0.0.meters.perSecond / (1.0.degrees * 1.seconds) // tune this - val AUTO_LEVEL_KD = 0.0.meters.perSecond / (1.0.degrees.perSecond) // tune this - val AUTO_LEVEL_MAX_VEL_SETPOINT = 2.degrees.perSecond - val AUTO_LEVEL_MAX_ACCEL_SETPOINT = 2.degrees.perSecond.perSecond + (1.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val MAX_AUTO_ANGULAR_VEL = 270.0.degrees.perSecond val MAX_AUTO_ANGULAR_ACCEL = 600.0.degrees.perSecond.perSecond @@ -158,7 +158,7 @@ object DrivetrainConstants { val DRIVE_KFF = 12.0.volts / 4.1675.meters.perSecond val DRIVE_KS = 0.5.volts - val DRIVE_KV = 0.0.volts / 1.0.meters.perSecond + val DRIVE_KV = 0.11.volts / 1.0.meters.perSecond val DRIVE_KA = 0.0.volts / 1.0.meters.perSecond.perSecond // val DRIVE_KS = 0.23677.volts diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index acfbd8f8..ae6dc21f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -16,7 +16,7 @@ import org.team4099.lib.units.perSecond object ElevatorConstants { // TODO: Change values later based on CAD - val REAL_KP = 0.0.volts / 1.inches + val REAL_KP = 0.725.volts / 1.inches val REAL_KI = 0.0.volts / (1.inches * 1.seconds) val REAL_KD = 0.0.volts / (1.inches.perSecond) @@ -29,11 +29,11 @@ object ElevatorConstants { val LEADER_INVERTED = false val FOLLOWER_INVERTED = true - val LEADER_KP: ProportionalGain = 0.325.volts / 1.inches + val LEADER_KP: ProportionalGain = 0.725.volts / 1.inches val LEADER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) val LEADER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) - val FOLLOWER_KP: ProportionalGain = 0.325.volts / 1.inches + val FOLLOWER_KP: ProportionalGain = 0.725.volts / 1.inches val FOLLOWER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) val FOLLOWER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) @@ -42,8 +42,8 @@ object ElevatorConstants { val SIM_KD = 0.0.volts / (1.inches.perSecond) val ELEVATOR_KS = 0.0.volts - val ELEVATOR_KG = 0.3.volts - val ELEVATOR_KV = 0.5.volts / 1.inches.perSecond + val ELEVATOR_KG = 0.28.volts + val ELEVATOR_KV = 0.48.volts / 1.inches.perSecond val ELEVATOR_KA = 0.075.volts / 1.inches.perSecond.perSecond val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts @@ -56,15 +56,15 @@ object ElevatorConstants { val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches val ELEVATOR_SAFE_THRESHOLD = 5.0.inches - val ELEVATOR_TOLERANCE = 0.2.inches + val ELEVATOR_TOLERANCE = 0.75.inches val MAX_VELOCITY = 1.meters.perSecond val MAX_ACCELERATION = 2.0.meters.perSecond.perSecond - val SHOOT_SPEAKER_LOW_POSITION = 0.0.inches - val SHOOT_SPEAKER_MID_POSITION = 9.0.inches - val SHOOT_SPEAKER_HIGH_POSITION = 14.0.inches - val SHOOT_AMP_POSITION = 0.0.inches + val SHOOT_SPEAKER_LOW_POSITION = 0.25.inches + val SHOOT_SPEAKER_MID_POSITION = 16.0.inches + val SHOOT_SPEAKER_HIGH_POSITION = 16.0.inches + val SHOOT_AMP_POSITION = 17.0.inches // 16 val SOURCE_NOTE_OFFSET = 0.0.inches val ELEVATOR_THETA_POS = 0.0.degrees val HOMING_STATOR_CURRENT = 3.0.amps diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index d495d2a8..0b128980 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -16,21 +16,22 @@ object FeederConstants { val VOLTAGE_COMPENSATION = 12.0.volts val FEEDER_CURRENT_LIMIT = 40.0.amps - val BEAM_BREAK_WAIT_TIME = 0.2.seconds + val BEAM_BREAK_WAIT_TIME = 0.3.seconds val FEEDER_GEAR_RATIO = 24.0 / 12.0 val FEEDER_INERTIA = 0.0017672509.kilo.grams * 1.0.meters.squared var NOTE_VELOCITY_THRESHOLD = 60.degrees.perSecond - var WAIT_BEFORE_DETECT_VELOCITY_DROP = 1.seconds + var WAIT_BEFORE_DETECT_VELOCITY_DROP = 0.5.seconds val IDLE_VOLTAGE = 0.volts - val INTAKE_NOTE_VOLTAGE = 1.5.volts + val INTAKE_NOTE_VOLTAGE = 1.volts + val AUTO_INTAKE_NOTE_VOLTAGE = 1.volts - val SHOOT_NOTE_VOLTAGE = 1.volts + val SHOOT_NOTE_VOLTAGE = 2.volts val OUTTAKE_NOTE_VOLTAGE = (-6).volts - val beamBreakFilterTime = 0.04.seconds + val beamBreakFilterTime = 0.1.seconds } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 9519e3e9..b9937744 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -1,7 +1,109 @@ package com.team4099.robot2023.config.constants -import org.team4099.lib.units.base.feet +import edu.wpi.first.apriltag.AprilTagFields +import org.team4099.lib.apriltag.AprilTag +import org.team4099.lib.apriltag.AprilTagFieldLayout +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +/** + * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets + * of corners start in the lower left moving clockwise. **All units in Meters**



+ * + * All translations and poses are stored with the origin at the rightmost point on the BLUE ALLIANCE + * wall.



Length refers to the *x* direction (as described by wpilib)

+ * Width refers to the *y* direction (as described by wpilib) + */ object FieldConstants { - val fieldLength = 54.feet + var fieldLength = 651.223.inches + var fieldWidth = 323.277.inches + + val aprilTags: List = listOf() + val homeAprilTags: List = listOf() + + val wpilibAprilTags = + if (Constants.Universal.REAL_FIELD) aprilTags.map { it.apriltagWpilib } + else homeAprilTags.map { it.apriltagWpilib } + + val wpilibFieldLayout = + edu.wpi.first.apriltag.AprilTagFieldLayout( + wpilibAprilTags, fieldLength.inMeters, fieldWidth.inMeters + ) + + var wingX = 229.201.inches + var podiumX = 126.75.inches + var startingLineX = 74.111.inches + + val fieldAprilTags: List = + listOf( + AprilTag(4, Pose3d()), + AprilTag(3, Pose3d(Translation3d(0.meters, 0.5.meters, 0.meters), Rotation3d())) + ) + + val tags = AprilTagFields.k2024Crescendo + + var ampCenter = Translation2d(72.455.inches, 322.996.inches) + + var aprilTagWidth = 6.50.inches + + var noteThickness = 2.inches + + /** Staging locations for each note */ + object StagingLocations { + var centerlineX = fieldLength / 2.0 + + // need to update + var centerlineFirstY = 29.638.inches + var centerlineSeparationY = 66.0.inches + var spikeX = 114.0.inches + + // need + var spikeFirstY = 161.638.inches + var spikeSeparationY = 57.0.inches + + var centerlineTranslations: Array = arrayOfNulls(5) + var spikeTranslations: Array = arrayOfNulls(3) + + init { + for (i in centerlineTranslations.indices) { + centerlineTranslations[i] = + Translation2d(centerlineX, centerlineFirstY + (centerlineSeparationY * i)) + } + } + + init { + for (i in spikeTranslations.indices) { + spikeTranslations[i] = Translation2d(spikeX, spikeFirstY + (spikeSeparationY * i)) + } + } + } + + /** Each corner of the speaker * */ + object Speaker { + // corners (blue alliance origin) + var topRightSpeaker = Translation3d(18.055.inches, 238.815.inches, 83.091.inches) + + var topLeftSpeaker = Translation3d(18.055.inches, 197.765.inches, 83.091.inches) + + var bottomRightSpeaker: Translation3d = Translation3d(0.0.inches, 238.815.inches, 78.324.inches) + var bottomLeftSpeaker: Translation3d = Translation3d(0.0.inches, 197.765.inches, 78.324.inches) + + /** Center of the speaker opening (blue alliance) */ + var centerSpeakerOpening: Translation3d = (bottomLeftSpeaker + topRightSpeaker) / 2.0 + var speakerTargetPose = Pose2d(centerSpeakerOpening.x, centerSpeakerOpening.y, 0.0.degrees) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 54d5e3ab..71258530 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -3,6 +3,7 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.grams +import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.DerivativeGain @@ -20,7 +21,11 @@ import org.team4099.lib.units.perSecond object FlywheelConstants { val LEFT_GEAR_RATIO = 1.0 - val RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS = 24.0 / 48.0 + val FLYWHEEL_RADIUS = 2.inches + + val FLYWHEEL_SPEED_TRANSFER_PERCENTAGE = 0.47 + + val RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS = 2.0 val VOLTAGE_COMPENSATION = 12.volts @@ -51,18 +56,20 @@ object FlywheelConstants { 0.0.volts / (1.0.rotations.perMinute.perSecond) val REAL_FLYWHEEL_KS = 0.0.volts - val REAL_FLYWHEEL_KV = 0.022.volts / 1.radians.perSecond + val REAL_FLYWHEEL_KV = 0.011.volts / 1.radians.perSecond val REAL_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond val SIM_FLYWHEEL_KS = 0.0.volts - val SIM_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond + val SIM_FLYWHEEL_KV = 0.011.volts / 1.radians.perSecond val SIM_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond } val IDLE_VELOCITY = 0.0.rotations.perMinute - val SPEAKER_VELOCITY = 10_000.rotations.perMinute + val SPEAKER_VELOCITY = 3000.rotations.perMinute val AMP_VELOCITY = 5_000.rotations.perMinute - val AMP_SCORE_TIME = 1.seconds - val SPEAKER_SCORE_TIME = 1.seconds + val TRAP_VELOCITY = 5_000.rotations.perMinute + + val AMP_SCORE_TIME = 0.5.seconds + val SPEAKER_SCORE_TIME = 0.4.seconds val EJECT_VELOCITY = 3_000.rotations.perMinute } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 9f3baa5e..bcc28efa 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -1,6 +1,10 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.geometry.Translation2d import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.volts object IntakeConstants { @@ -8,6 +12,10 @@ object IntakeConstants { val CENTER_WHEEL_INERTIA = 0.002459315 val VOLTAGE_COMPENSATION = 12.0.volts + val SIM_INTAKE_DISTANCE = 8.inches + + val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees) + // TODO: Change gear ratio according to robot val ROLLER_CURRENT_LIMIT = 50.0.amps const val ROLLER_MOTOR_INVERTED = true @@ -18,8 +26,8 @@ object IntakeConstants { val IDLE_ROLLER_VOLTAGE = 1.0.volts val IDLE_CENTER_WHEEL_VOLTAGE = 0.0.volts - val INTAKE_ROLLER_VOLTAGE = -9.volts + val INTAKE_ROLLER_VOLTAGE = -12.volts val INTAKE_CENTER_WHEEL_VOLTAGE = -9.volts - val OUTTAKE_ROLLER_VOLTAGE = (-10).volts - val OUTTAKE_CENTER_WHEEL_VOLTAGE = (-10).volts + val OUTTAKE_ROLLER_VOLTAGE = (10).volts + val OUTTAKE_CENTER_WHEEL_VOLTAGE = (10).volts } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index bfce7c8c..d95146fa 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -56,7 +56,7 @@ object VisionConstants { ) // camera facing leftward ) - val CAMERA_NAMES = listOf("northstar_1", "northstar_2", "northstar_3") + val CAMERA_NAMES = listOf("parakeet_1", "parakeet_2", "parakeet_3") object Limelight { val LIMELIGHT_NAME = "limelight-zapdos" diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 97d4c4f0..59524e6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -22,6 +22,11 @@ object WristConstants { // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps + val WRIST_AXIS_TO_NOTE_HOLD_POSITION = 14.5.inches + val WRIST_AXIS_TO_NOTE_LAUNCH_POSITION = 10.inches + + val NOTE_ANGLE_SIM_OFFSET = -24.degrees + val VOLTAGE_COMPENSATION = 12.0.volts val ABSOLUTE_ENCODER_OFFSET = 0.degrees val WRIST_LENGTH = 18.6.inches @@ -29,12 +34,15 @@ object WristConstants { val WRIST_ENCODER_GEAR_RATIO = 0.0 - val WRIST_GEAR_RATIO = 12.0 / 90.0 * 24.0 / 90.0 * 32.0 / 90.0 + val WRIST_GEAR_RATIO = 1.0 / 5.0 * 1.0 / 4.0 * 1.0 / 3.0 * 42.0 / 46.0 * 33.0 / 90.0 val WRIST_VOLTAGE_COMPENSATION = 12.0.volts - val WRIST_STATOR_CURRENT_LIMIT = 40.0.amps + val WRIST_STATOR_CURRENT_LIMIT = 10.0.amps + val WRIST_SUPPLY_CURRENT_LIMIT = 10.amps + val WRIST_THRESHOLD_CURRENT_LIMIT = 1.0.amps + val WRIST_TRIGGER_THRESHOLD_TIME = 10.0.seconds - val WRIST_MAX_ROTATION = 90.degrees - val WRIST_MIN_ROTATION = (-37.0).degrees + val WRIST_MAX_ROTATION = 20.degrees + val WRIST_MIN_ROTATION = -36.25.degrees val WRIST_ZERO_SIM_OFFSET = 27.5.degrees @@ -43,18 +51,18 @@ object WristConstants { val HARDSTOP_OFFSET = 47.degrees object PID { - val REAL_KP: ProportionalGain = 0.07.volts / 1.0.degrees + val REAL_KP: ProportionalGain = 0.5.volts / 1.0.degrees val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) - val REAL_KD: DerivativeGain = 0.0175.volts / (1.0.rotations / 1.0.seconds) + val REAL_KD: DerivativeGain = 0.0.volts / (1.0.rotations / 1.0.seconds) val SIM_KP: ProportionalGain = 1.volts / 1.0.degrees val SIM_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val SIM_KD: DerivativeGain = 0.0175.volts / (1.0.degrees / 1.0.seconds) - val REAL_WRIST_KG = 0.3.volts - val REAL_WRIST_KV = 1.6.volts / 1.0.radians.perSecond - val REAL_WRIST_KA = 0.175.volts / 1.0.radians.perSecond.perSecond - val REAL_WRIST_KS = 0.15.volts + val REAL_WRIST_KG = 0.0.volts + val REAL_WRIST_KV = 0.0.volts / 1.0.radians.perSecond + val REAL_WRIST_KA = 0.0.volts / 1.0.radians.perSecond.perSecond + val REAL_WRIST_KS = 0.0.volts val SIM_WRIST_KG = 1.3.volts val SIM_WRIST_KV = 1.6.volts / 1.0.radians.perSecond @@ -65,11 +73,12 @@ object WristConstants { val WRIST_TOLERANCE = 0.1.degrees val IDLE_ANGLE = (-35.0).degrees - val AMP_SCORE_ANGLE = 0.0.degrees - val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = 0.0.degrees - val SUBWOOFER_SPEAKER_SHOT_ANGLE_MID = 30.0.degrees - val SUBWOOFER_SPEAKER_SHOT_ANGLE_HIGH = 50.0.degrees - val CLIMB_ANGLE = 20.0.degrees + val AMP_SCORE_ANGLE = -16.0.degrees + val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = -36.0.degrees + val SUBWOOFER_SPEAKER_SHOT_ANGLE_MID = -7.5.degrees + val SUBWOOFER_SPEAKER_SHOT_ANGLE_HIGH = -2.degrees + val CLIMB_ANGLE = 10.0.degrees + val TRAP_ANGLE = 35.0.degrees val INTAKE_ANGLE = (-35.0).degrees val IDLE_ANGLE_HAS_GAMEPEICE = -35.0.degrees } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index f08c8c25..84273c0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -16,6 +16,7 @@ import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.FieldFrameEstimator import com.team4099.robot2023.util.Velocity2d import com.team4099.robot2023.util.inverse +import com.team4099.robot2023.util.rotateBy import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.kinematics.SwerveDriveOdometry @@ -78,6 +79,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) private set + var isInAutonomous = false + private set + var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) private var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) @@ -89,7 +93,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB private var omegaVelocity = 0.0.radians.perSecond - var lastGyroYaw = 0.0.radians + var lastGyroYaw = { gyroInputs.gyroYaw } var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED private set @@ -122,6 +126,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB targetedChassisSpeeds = value.chassisSpeeds targetedChassisAccels = value.chassisAccels } + is DrivetrainRequest.ZeroSensors -> { + isInAutonomous = value.isInAutonomous + } else -> {} } field = value @@ -282,6 +289,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) ) + Logger.recordOutput( + "" + "FieldRelativePose/robotPose", fieldTRobot.toDoubleArray().toDoubleArray() + ) Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) Logger.recordOutput("Drivetrain/setPointStates", *setPointStates.toTypedArray()) @@ -301,11 +311,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB .pose3d ) - Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) - - Logger.recordOutput( - "FieldFrameEstimator/odomTField", odomTField.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.toDoubleArray()) Logger.recordOutput( "Odometry/targetPose", @@ -330,7 +336,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB nextState = DrivetrainState.ZEROING_SENSORS } DrivetrainState.ZEROING_SENSORS -> { - zeroSensors() + zeroSensors(isInAutonomous) // Transitions currentRequest = DrivetrainRequest.Idle() nextState = fromRequestToState(currentRequest) @@ -418,6 +424,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB val allianceFlippedDriveVector = Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) + Logger.recordOutput( + "Drivetrain/driveVectorFirst", allianceFlippedDriveVector.first.inMetersPerSecond + ) + Logger.recordOutput( + "Drivetrain/driveVectorSecond", allianceFlippedDriveVector.second.inMetersPerSecond + ) + val swerveModuleStates: Array var desiredChassisSpeeds: ChassisSpeeds @@ -542,11 +555,14 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } /** Zeros all the sensors on the drivetrain. */ - fun zeroSensors() { + fun zeroSensors(isInAutonomous: Boolean) { zeroGyroPitch() zeroGyroRoll() zeroSteering() - zeroDrive() + + if (!isInAutonomous) { + zeroDrive() + } } /** Resets the field frame estimator given some current pose of the robot. */ @@ -556,6 +572,14 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) } + fun tempZeroGyroYaw(toAngle: Angle = 0.0.degrees) { + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + lastModulePositions, + Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d + ) + } + /** * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. * diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index 7225ced3..07bd876a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt @@ -61,32 +61,6 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared ) - init { - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - } - var turnRelativePosition = 0.0.radians var turnAbsolutePosition = (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to @@ -116,6 +90,30 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { init { steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + + MotorChecker.add( + "Drivetrain", + "Drive", + MotorCollection( + mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + + MotorChecker.add( + "Drivetrain", + "Steering", + MotorCollection( + mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) } var driveAppliedVolts = 0.0.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index a588dd40..b810c02b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -8,10 +8,11 @@ import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionDutyCycle -import com.ctre.phoenix6.controls.VelocityDutyCycle +import com.ctre.phoenix6.controls.VelocityVoltage import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 @@ -42,6 +43,8 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond +import org.team4099.lib.units.derived.perMeterPerSecond import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond @@ -57,9 +60,7 @@ class SwerveModuleIOTalon( ) : SwerveModuleIO { private val steeringSensor = ctreAngularMechanismSensor( - steeringFalcon, - DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_COMPENSATION_VOLTAGE + steeringFalcon, 1.0, DrivetrainConstants.STEERING_COMPENSATION_VOLTAGE ) private val driveSensor = @@ -83,88 +84,13 @@ class SwerveModuleIOTalon( } } - init { - driveFalcon.configurator.apply(TalonFXConfiguration()) - steeringFalcon.configurator.apply(TalonFXConfiguration()) - - driveFalcon.clearStickyFaults() - steeringFalcon.clearStickyFaults() - - steeringConfiguration.Slot0.kP = - steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) - steeringConfiguration.Slot0.kI = - steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) - steeringConfiguration.Slot0.kD = - steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.Slot0.kV = - steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) - steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = - steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) - steeringConfiguration.MotionMagic.MotionMagicAcceleration = - steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - - /* - steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true - steeringConfiguration.Feedback.SensorToMechanismRatio = - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO - steeringConfiguration.Feedback.SensorToMechanismRatio = 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO - */ - - steeringConfiguration.MotorOutput.NeutralMode = - NeutralModeValue.Brake // change back to coast maybe? - steeringFalcon.inverted = true - steeringFalcon.configurator.apply(steeringConfiguration) - - driveConfiguration.Slot0.kP = - driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) - driveConfiguration.Slot0.kI = - driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) - driveConfiguration.Slot0.kD = - driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.12679 / 15 - // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - driveConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive - driveFalcon.configurator.apply(driveConfiguration) - - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) + private val driveKV = + LoggedTunableValue( + "Drivetrain/kV", + DrivetrainConstants.PID.DRIVE_KV, + Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) ) - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(Falcon500(steeringFalcon, "$label Steering Motor")), - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT - 10.amps, - 110.celsius - ) - ) - } val driveStatorCurrentSignal: StatusSignal val driveSupplyCurrentSignal: StatusSignal val steeringStatorCurrentSignal: StatusSignal @@ -197,10 +123,10 @@ class SwerveModuleIOTalon( steeringConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - // steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true + steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - // steeringConfiguration.Feedback.SensorToMechanismRatio = 1 / - // DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO + steeringConfiguration.Feedback.SensorToMechanismRatio = + 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO steeringConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake // change back to coast maybe? @@ -214,7 +140,7 @@ class SwerveModuleIOTalon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 + driveConfiguration.Slot0.kV = driveKV.get().inVoltsPerMetersPerSecond // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes @@ -225,7 +151,7 @@ class SwerveModuleIOTalon( driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true driveConfiguration.CurrentLimits.StatorCurrentLimit = DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune + driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast driveFalcon.configurator.apply(driveConfiguration) @@ -233,7 +159,7 @@ class SwerveModuleIOTalon( driveStatorCurrentSignal = driveFalcon.statorCurrent driveSupplyCurrentSignal = driveFalcon.supplyCurrent steeringStatorCurrentSignal = steeringFalcon.statorCurrent - steeringSupplyCurrentSignal = steeringFalcon.statorCurrent + steeringSupplyCurrentSignal = steeringFalcon.supplyCurrent driveTempSignal = driveFalcon.deviceTemp steeringTempSignal = steeringFalcon.deviceTemp steeringPosition = steeringFalcon.position @@ -374,11 +300,11 @@ class SwerveModuleIOTalon( ) { val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign driveFalcon.setControl( - VelocityDutyCycle( + VelocityVoltage( driveSensor.velocityToRawUnits(speed), driveSensor.accelerationToRawUnits(acceleration), DrivetrainConstants.FOC_ENABLED, - feedforward.inVolts / DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE.inVolts, + feedforward.inVolts, 0, false, false, @@ -443,7 +369,7 @@ class SwerveModuleIOTalon( PIDConfig.kP = driveSensor.proportionalVelocityGainToRawUnits(kP) PIDConfig.kI = driveSensor.integralVelocityGainToRawUnits(kI) PIDConfig.kD = driveSensor.derivativeVelocityGainToRawUnits(kD) - PIDConfig.kV = 0.05425 + PIDConfig.kV = driveKV.get().inVoltsPerMetersPerSecond driveFalcon.configurator.apply(PIDConfig) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 0505459c..603cf51d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -19,6 +19,16 @@ import org.team4099.lib.units.derived.volts class Feeder(val io: FeederIO) : SubsystemBase() { val inputs = FeederIO.FeederIOInputs() + var rumbleTrigger = false + + var rumbleTime = 0.5.seconds + + val rumbleStartTime = Clock.fpgaTime + + var lastHeldGamePiece = false + + var lastDropTime = Clock.fpgaTime + object TunableFeederStates { val idleVoltage = LoggedTunableValue( @@ -36,6 +46,12 @@ class Feeder(val io: FeederIO) : SubsystemBase() { FeederConstants.OUTTAKE_NOTE_VOLTAGE, Pair({ it.inVolts }, { it.volts }) ) + val autoIntakeVoltage = + LoggedTunableValue( + "Feeder/autoIntakeVoltage", + FeederConstants.AUTO_INTAKE_NOTE_VOLTAGE, + Pair({ it.inVolts }, { it.volts }) + ) val shootVoltage = LoggedTunableValue( "Feeder/shootVoltage", @@ -106,6 +122,17 @@ class Feeder(val io: FeederIO) : SubsystemBase() { override fun periodic() { io.updateInputs(inputs) + if (lastHeldGamePiece != hasNote && !rumbleTrigger) { + rumbleTrigger = true + lastDropTime = Clock.fpgaTime + } + + if (Clock.fpgaTime - lastDropTime > rumbleTime) { + rumbleTrigger = false + } + + lastHeldGamePiece = hasNote + hasNote = debounceFilter.calculate(inputs.beamBroken) Logger.recordOutput("Feeder/hasNote", hasNote) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index d47c7473..800d8d95 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -51,6 +51,12 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { FlywheelConstants.AMP_VELOCITY, Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) ) + val trapVelocity = + LoggedTunableValue( + "Flywheel/trapVelocity", + FlywheelConstants.TRAP_VELOCITY, + Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) + ) val ampScoreTime = LoggedTunableValue( "Flywheel/ampScoreTime", @@ -123,8 +129,8 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { private var lastRightFlywheelVoltage = 0.0.volts var flywheelTargetVoltage = 0.volts - var flywheelRightTargetVelocity: AngularVelocity = 0.0.rotations.perMinute - var flywheelLeftTargetVelocity: AngularVelocity = 0.0.rotations.perMinute + var flywheelRightTargetVelocity: AngularVelocity = -1337.0.rotations.perMinute + var flywheelLeftTargetVelocity: AngularVelocity = -1337.0.rotations.perMinute val isAtTargetedVelocity: Boolean get() = @@ -143,9 +149,9 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { flywheelTargetVoltage = value.flywheelVoltage } is Request.FlywheelRequest.TargetingVelocity -> { - flywheelRightTargetVelocity = value.flywheelVelocity + flywheelRightTargetVelocity = value.flywheelVelocity * 2 // left needs to be half of the right one - flywheelLeftTargetVelocity = value.flywheelVelocity / 2 + flywheelLeftTargetVelocity = value.flywheelVelocity } else -> {} } @@ -191,7 +197,14 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { } override fun periodic() { io.updateInputs(inputs) + Logger.processInputs("Flywheel", inputs) + Logger.recordOutput( + "Flywheel/targetDifference", + (inputs.leftFlywheelVelocity - flywheelLeftTargetVelocity) + .absoluteValue + .inRotationsPerMinute + ) Logger.recordOutput("Flywheel/currentState", currentState.name) Logger.recordOutput("Flywheel/requestedState", currentRequest.javaClass.simpleName) @@ -199,9 +212,13 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { Logger.recordOutput("Flywheel/isAtTargetedVelocity", isAtTargetedVelocity) if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("Flywheel/FlywheelTargetVoltage", flywheelTargetVoltage.inVolts) + Logger.recordOutput("Flywheel/FlywheelRightTargetVoltage", flywheelTargetVoltage.inVolts) + Logger.recordOutput("Flywheel/FlywheelLeftTargetVoltage", flywheelTargetVoltage.inVolts) + Logger.recordOutput( + "Flywheel/FlywheelRightTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute + ) Logger.recordOutput( - "Flywheel/FlywheelTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute + "Flywheel/FlywheelLeftTargetVelocity", flywheelLeftTargetVelocity.inRotationsPerMinute ) Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastRightFlywheelVoltage.inVolts) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt index 68027739..a3b2fce2 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt @@ -27,14 +27,14 @@ object FlywheelIOSim : FlywheelIO { private val flywheelRightSim: FlywheelSim = FlywheelSim( DCMotor.getKrakenX60Foc(2), - FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS, + 1 / FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS, FlywheelConstants.INERTIA.inKilogramsMeterSquared ) private val flywheelLeftSim: FlywheelSim = FlywheelSim( DCMotor.getKrakenX60Foc(2), - FlywheelConstants.LEFT_GEAR_RATIO, + 1 / FlywheelConstants.LEFT_GEAR_RATIO, FlywheelConstants.INERTIA.inKilogramsMeterSquared ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt new file mode 100644 index 00000000..0511c685 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -0,0 +1,241 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. +package com.team4099.robot2023.subsystems.led + +import edu.wpi.first.math.MathUtil +import edu.wpi.first.wpilibj.AddressableLED +import edu.wpi.first.wpilibj.AddressableLEDBuffer +import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.DriverStation.Alliance +import edu.wpi.first.wpilibj.Notifier +import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj.util.Color +import edu.wpi.first.wpilibj2.command.Subsystem +import kotlin.math.floor +import kotlin.math.pow +import kotlin.math.sin + +class Leds : Subsystem { + // Robot state tracking + var loopCycleCount: Int = 0 + var hasNote = false + var isIdle = false + var alliance = DriverStation.getAlliance() + var lowBatteryAlert = false + var minLoopCycleCount = 10 + var length = 40 + private val breathDuration = 1.0 + private val waveExponent = 0.4 + private val waveFastCycleLength = 25.0 + private val waveFastDuration = 0.25 + private val waveSlowCycleLength = 25.0 + private val waveSlowDuration = 3.0 + private val waveAllianceCycleLength = 15.0 + private val waveAllianceDuration = 2.0 + + // LED IO + private val leds: AddressableLED + private val buffer: AddressableLEDBuffer + private val loadingNotifier: Notifier + + init { + println("[Init] Creating LEDs") + leds = AddressableLED(9) + buffer = AddressableLEDBuffer(length) + leds.setLength(length) + leds.setData(buffer) + leds.start() + loadingNotifier = + Notifier { + synchronized(this) { + breath( + Section.STATIC, + Color.kWhite, + Color.kBlack, + 0.25, + System.currentTimeMillis() / 1000.0 + ) + leds.setData(buffer) + } + } + loadingNotifier.startPeriodic(0.02) + } + + @Synchronized + override fun periodic() { + // Update alliance color + if (DriverStation.isFMSAttached()) { + alliance = DriverStation.getAlliance() + } + + // Exit during initial cycles + loopCycleCount += 1 + if (loopCycleCount < minLoopCycleCount) { + return + } + + // Stop loading notifier if running + loadingNotifier.stop() + + // Select LED mode + + if (DriverStation.isDisabled()) { + if (lowBatteryAlert) { + // Low battery + solid(Section.FULL, Color.kOrangeRed) + } + // Default pattern + when (alliance.get()) { + Alliance.Red -> + wave( + Section.FULL, + Color.kRed, + Color.kBlack, + waveAllianceCycleLength, + waveAllianceDuration + ) + Alliance.Blue -> + wave( + Section.FULL, + Color.kBlue, + Color.kBlack, + waveAllianceCycleLength, + waveAllianceDuration + ) + else -> + wave(Section.FULL, Color.kGold, Color.kDarkBlue, waveSlowCycleLength, waveSlowDuration) + } + } else if (DriverStation.isAutonomous()) { + wave(Section.FULL, Color.kGold, Color.kBlack, waveFastCycleLength, waveFastDuration) + } else { + if (isIdle) { + if (hasNote) { + solid(Section.FULL, Color.kGreen) + } else { + solid(Section.FULL, Color.kGold) + } + } else { + solid(Section.FULL, Color.kBlue) + } + } + + // Update LEDs + leds.setData(buffer) + } + + private fun solid(section: Section, color: Color?) { + if (color != null) { + for (i in section.start() until section.end()) { + buffer.setLED(i, color) + } + } + } + + private fun solid(percent: Double, color: Color) { + var i = 0 + while (i < MathUtil.clamp(length * percent, 0.0, length.toDouble())) { + buffer.setLED(i, color) + i++ + } + } + + private fun strobe(section: Section, color: Color?, duration: Double) { + val on = ((Timer.getFPGATimestamp() % duration) / duration) > 0.5 + solid(section, if (on) color else Color.kBlack) + } + + private fun breath( + section: Section, + c1: Color, + c2: Color, + duration: Double, + timestamp: Double = Timer.getFPGATimestamp(), + ) { + val x = ((timestamp % breathDuration) / breathDuration) * 2.0 * Math.PI + val ratio = (sin(x) + 1.0) / 2.0 + val red = (c1.red * (1 - ratio)) + (c2.red * ratio) + val green = (c1.green * (1 - ratio)) + (c2.green * ratio) + val blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio) + solid(section, Color(red, green, blue)) + } + + private fun rainbow(section: Section, cycleLength: Double, duration: Double) { + var x = (1 - ((Timer.getFPGATimestamp() / duration) % 1.0)) * 180.0 + val xDiffPerLed = 180.0 / cycleLength + for (i in 0 until section.end()) { + x += xDiffPerLed + x %= 180.0 + if (i >= section.start()) { + buffer.setHSV(i, x.toInt(), 255, 255) + } + } + } + + private fun wave(section: Section, c1: Color, c2: Color, cycleLength: Double, duration: Double) { + var x = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI + val xDiffPerLed = (2.0 * Math.PI) / cycleLength + for (i in 0 until section.end()) { + x += xDiffPerLed + if (i >= section.start()) { + var ratio: Double = (sin(x).pow(waveExponent) + 1.0) / 2.0 + if (java.lang.Double.isNaN(ratio)) { + ratio = (-sin(x + Math.PI).pow(waveExponent) + 1.0) / 2.0 + } + if (java.lang.Double.isNaN(ratio)) { + ratio = 0.5 + } + val red = (c1.red * (1 - ratio)) + (c2.red * ratio) + val green = (c1.green * (1 - ratio)) + (c2.green * ratio) + val blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio) + buffer.setLED(i, Color(red, green, blue)) + } + } + } + + private fun stripes(section: Section, colors: List, length: Int, duration: Double) { + val offset = (Timer.getFPGATimestamp() % duration / duration * length * colors.size).toInt() + for (i in section.start() until section.end()) { + var colorIndex = (floor((i - offset).toDouble() / length) + colors.size).toInt() % colors.size + colorIndex = colors.size - 1 - colorIndex + buffer.setLED(i, colors[colorIndex]) + } + } + + private enum class Section { + STATIC, + FULL; + + fun start(): Int { + return when (this) { + STATIC -> 0 + FULL -> 0 + else -> 0 + } + } + + fun end(): Int { + return when (this) { + STATIC -> staticLength + FULL -> length + } + } + } + + companion object { + var instance: Leds? = null + get() { + if (field == null) { + field = Leds() + } + return field + } + private set + + private const val length = 43 + private const val staticLength = 14 + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index cdfd1c29..e2c777ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -21,9 +21,21 @@ sealed interface Request { class ScoreAmp() : SuperstructureRequest + class PrepScoreSpeakerLow() : SuperstructureRequest + + class PrepScoreSpeakerMid() : SuperstructureRequest + + class PrepScoreSpeakerHigh() : SuperstructureRequest + + class ScoreSpeaker() : SuperstructureRequest class ScoreSpeakerLow() : SuperstructureRequest class ScoreSpeakerMid() : SuperstructureRequest class ScoreSpeakerHigh() : SuperstructureRequest + + class PrepTrap() : SuperstructureRequest + + class ScoreTrap() : SuperstructureRequest + class ClimbExtend() : SuperstructureRequest class ClimbRetract() : SuperstructureRequest @@ -44,7 +56,7 @@ sealed interface Request { edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) ) : DrivetrainRequest - class ZeroSensors : DrivetrainRequest + class ZeroSensors(val isInAutonomous: Boolean = false) : DrivetrainRequest class Idle : DrivetrainRequest class LockWheels : DrivetrainRequest diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 3e7ef8af..1b9d59a9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1,12 +1,18 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.hal.Clock +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.WristConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.elevator.Elevator import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.flywheel.Flywheel import com.team4099.robot2023.subsystems.intake.Intake +import com.team4099.robot2023.subsystems.led.Leds import com.team4099.robot2023.subsystems.wrist.Wrist +import com.team4099.robot2023.util.NoteSimulation +import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -27,9 +33,12 @@ class Superstructure( private val feeder: Feeder, private val elevator: Elevator, private val wrist: Wrist, - private val flywheel: Flywheel + private val flywheel: Flywheel, + private val drivetrain: Drivetrain ) : SubsystemBase() { + var leds = Leds() + var currentRequest: Request.SuperstructureRequest = Request.SuperstructureRequest.Idle() var currentState: SuperstructureStates = SuperstructureStates.UNINITIALIZED @@ -42,6 +51,10 @@ class Superstructure( var lastTransitionTime = Clock.fpgaTime + var noteHoldingID = -1 + + var notes = mutableListOf() + fun toDoubleArray(somePose: Pose3d): DoubleArray { return doubleArrayOf( somePose.x.inMeters, @@ -54,7 +67,50 @@ class Superstructure( ) } + init { + notes.add(NoteSimulation(0, Pose3d())) + notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT + + FieldConstants.StagingLocations.spikeTranslations.forEach { + notes.add( + NoteSimulation( + notes.size, + Pose3d( + it?.x ?: 0.0.inches, + it?.y ?: 0.0.inches, + FieldConstants.noteThickness / 2.0, + Rotation3d() + ) + ) + ) + } + + FieldConstants.StagingLocations.centerlineTranslations.forEach { + notes.add( + NoteSimulation( + notes.size, + Pose3d( + it?.x ?: 0.0.inches, + it?.y ?: 0.0.inches, + FieldConstants.noteThickness / 2.0, + Rotation3d() + ) + ) + ) + } + + notes.forEach { it.poseSupplier = { drivetrain.fieldTRobot } } + notes.forEach { it.wristAngleSupplier = { wrist.inputs.wristPosition } } + notes.forEach { it.elevatorHeightSupplier = { elevator.inputs.elevatorPosition } } + notes.forEach { it.flywheelAngularVelocitySupplier = { flywheel.inputs.rightFlywheelVelocity } } + } + override fun periodic() { + leds.hasNote = feeder.hasNote + leds.isIdle = currentState == SuperstructureStates.IDLE + + notes.forEach { it.periodic() } + notes.forEach { Logger.recordOutput("NoteSimulation/${it.id}", toDoubleArray(it.currentPose)) } Logger.recordOutput( "SimulatedMechanisms/0", @@ -147,14 +203,18 @@ class Superstructure( } } SuperstructureStates.HOME -> { + /* wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) + */ + elevator.currentRequest = Request.ElevatorRequest.Home() if (elevator.isHomed) { nextState = SuperstructureStates.IDLE } + noteHoldingID = 0 } SuperstructureStates.IDLE -> { intake.currentRequest = @@ -162,13 +222,28 @@ class Superstructure( Intake.TunableIntakeStates.idleRollerVoltage.get(), Intake.TunableIntakeStates.idleCenterWheelVoltage.get() ) + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.idleVoltage.get()) - flywheel.currentRequest = - Request.FlywheelRequest.TargetingVelocity( - Flywheel.TunableFlywheelStates.idleVelocity.get() - ) - if (feeder.hasNote) { + + if (DriverStation.isAutonomous()) { + flywheel.currentRequest = + Request.FlywheelRequest.TargetingVelocity( + Flywheel.TunableFlywheelStates.speakerVelocity.get() + ) + } else { + flywheel.currentRequest = + Request.FlywheelRequest.TargetingVelocity( + Flywheel.TunableFlywheelStates.idleVelocity.get() + ) + } + + if (DriverStation.isAutonomous()) { + wrist.currentRequest = + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.subwooferSpeakerShotAngleLow.get() + ) + } else if (feeder.hasNote) { wrist.currentRequest = Request.WristRequest.TargetingPosition( Wrist.TunableWristStates.idleAngleHasGamepiece.get() @@ -177,10 +252,12 @@ class Superstructure( wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) } + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition( Elevator.TunableElevatorHeights.minPosition.get() ) + when (currentRequest) { is Request.SuperstructureRequest.Home -> { nextState = SuperstructureStates.HOME_PREP @@ -197,15 +274,21 @@ class Superstructure( is Request.SuperstructureRequest.ScoreAmp -> { nextState = SuperstructureStates.SCORE_AMP } - is Request.SuperstructureRequest.ScoreSpeakerLow -> { + is Request.SuperstructureRequest.ScoreSpeaker -> { + nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } + is Request.SuperstructureRequest.PrepScoreSpeakerLow -> { nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP } - is Request.SuperstructureRequest.ScoreSpeakerMid -> { + is Request.SuperstructureRequest.PrepScoreSpeakerMid -> { nextState = SuperstructureStates.SCORE_SPEAKER_MID_PREP } - is Request.SuperstructureRequest.ScoreSpeakerHigh -> { + is Request.SuperstructureRequest.PrepScoreSpeakerHigh -> { nextState = SuperstructureStates.SCORE_SPEAKER_HIGH_PREP } + is Request.SuperstructureRequest.PrepTrap -> { + nextState = SuperstructureStates.SCORE_TRAP_PREP + } is Request.SuperstructureRequest.ClimbExtend -> { nextState = SuperstructureStates.CLIMB_EXTEND } @@ -218,8 +301,7 @@ class Superstructure( } } SuperstructureStates.GROUND_INTAKE_PREP -> { - wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.intakeAngle.get()) + wrist.currentRequest = Request.WristRequest.OpenLoop(-2.volts) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.GROUND_INTAKE } @@ -236,8 +318,26 @@ class Superstructure( Intake.TunableIntakeStates.intakeRollerVoltage.get(), Intake.TunableIntakeStates.intakeCenterWheelVoltage.get() ) + if (noteHoldingID == -1) { + for (note in notes) { + if (note.canIntake()) { + noteHoldingID = note.id + note.currentState = NoteSimulation.NoteStates.IN_ROBOT + break + } + } + } + + if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) { + currentRequest = Request.SuperstructureRequest.Idle() + nextState = SuperstructureStates.IDLE + } + feeder.currentRequest = - Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.intakeVoltage.get()) + Request.FeederRequest.OpenLoopIntake( + if (DriverStation.isAutonomous()) Feeder.TunableFeederStates.autoIntakeVoltage.get() + else Feeder.TunableFeederStates.intakeVoltage.get() + ) if (feeder.hasNote) { currentRequest = Request.SuperstructureRequest.Idle() nextState = SuperstructureStates.IDLE @@ -246,6 +346,12 @@ class Superstructure( is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE } + is Request.SuperstructureRequest.PrepScoreSpeakerLow -> { + nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } + is Request.SuperstructureRequest.ScoreSpeaker -> { + nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } } } SuperstructureStates.SCORE_AMP_PREP -> { @@ -255,13 +361,9 @@ class Superstructure( ) wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.ampScoreAngle.get()) - flywheel.currentRequest = - Request.FlywheelRequest.TargetingVelocity( - Flywheel.TunableFlywheelStates.ampVelocity.get() - ) + if (elevator.isAtTargetedPosition && wrist.isAtTargetedPosition && - flywheel.isAtTargetedVelocity && currentRequest is Request.SuperstructureRequest.ScoreAmp ) { nextState = SuperstructureStates.SCORE_AMP @@ -275,12 +377,19 @@ class Superstructure( } } SuperstructureStates.SCORE_AMP -> { + + if (noteHoldingID != -1) { + notes[noteHoldingID].currentState = NoteSimulation.NoteStates.AMP_SCORE + noteHoldingID = -1 + } + + flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(-10.volts) feeder.currentRequest = - Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get()) + Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.outtakeVoltage.get()) if (!feeder.hasNote && Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get() ) { - nextState = SuperstructureStates.IDLE + currentRequest = Request.SuperstructureRequest.Idle() } when (currentRequest) { @@ -304,9 +413,10 @@ class Superstructure( ) if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && - elevator.isAtTargetedPosition + elevator.isAtTargetedPosition && + currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { - nextState = SuperstructureStates.SCORE_SPEAKER_LOW + nextState = SuperstructureStates.SCORE_SPEAKER } when (currentRequest) { @@ -315,14 +425,20 @@ class Superstructure( } } } - SuperstructureStates.SCORE_SPEAKER_LOW -> { + SuperstructureStates.SCORE_SPEAKER -> { + if (noteHoldingID != -1) { + notes[noteHoldingID].currentState = NoteSimulation.NoteStates.IN_FLIGHT + noteHoldingID = -1 + } feeder.currentRequest = Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get()) - if (!feeder.hasNote && + + if ((!feeder.hasNote) && Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.speakerScoreTime.get() ) { - nextState = SuperstructureStates.IDLE + + currentRequest = Request.SuperstructureRequest.Idle() } when (currentRequest) { @@ -346,24 +462,10 @@ class Superstructure( ) if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && - elevator.isAtTargetedPosition - ) { - nextState = SuperstructureStates.SCORE_SPEAKER_MID - } - - when (currentRequest) { - is Request.SuperstructureRequest.Idle -> { - nextState = SuperstructureStates.IDLE - } - } - } - SuperstructureStates.SCORE_SPEAKER_MID -> { - feeder.currentRequest = - Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get()) - if (!feeder.hasNote && - Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get() + elevator.isAtTargetedPosition && + currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { - nextState = SuperstructureStates.IDLE + nextState = SuperstructureStates.SCORE_SPEAKER } when (currentRequest) { @@ -387,9 +489,10 @@ class Superstructure( ) if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && - elevator.isAtTargetedPosition + elevator.isAtTargetedPosition && + currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { - nextState = SuperstructureStates.SCORE_SPEAKER_HIGH + nextState = SuperstructureStates.SCORE_SPEAKER } when (currentRequest) { @@ -398,12 +501,26 @@ class Superstructure( } } } - SuperstructureStates.SCORE_SPEAKER_HIGH -> { - feeder.currentRequest = - Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get()) - if (!feeder.hasNote && - Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get() + SuperstructureStates.SCORE_TRAP_PREP -> { + wrist.currentRequest = + Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.trapAngle.get()) + if (wrist.isAtTargetedPosition && + currentRequest is Request.SuperstructureRequest.ScoreTrap ) { + nextState = SuperstructureStates.SCORE_TRAP + } + + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } + } + SuperstructureStates.SCORE_TRAP -> { + feeder.currentRequest = + Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.outtakeVoltage.get()) + + if (!feeder.hasNote) { nextState = SuperstructureStates.IDLE } @@ -430,7 +547,7 @@ class Superstructure( } } SuperstructureStates.CLIMB_RETRACT -> { - elevator.currentRequest = Request.ElevatorRequest.OpenLoop(-4.volts) + elevator.currentRequest = Request.ElevatorRequest.OpenLoop(-6.volts) when (currentRequest) { is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE @@ -441,14 +558,13 @@ class Superstructure( } } SuperstructureStates.EJECT_GAME_PIECE -> { + intake.currentRequest = + Request.IntakeRequest.OpenLoop( + Intake.TunableIntakeStates.outtakeRolllerVoltage.get(), + Intake.TunableIntakeStates.outtakeCenterWheelVoltage.get() + ) feeder.currentRequest = - Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get()) - if (!feeder.hasNote && - Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get() - ) { - currentRequest = Request.SuperstructureRequest.Idle() - nextState = SuperstructureStates.IDLE - } + Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.outtakeVoltage.get()) when (currentRequest) { is Request.SuperstructureRequest.Idle -> { @@ -460,11 +576,8 @@ class Superstructure( SuperstructureStates.EJECT_GAME_PIECE_PREP -> { wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) - flywheel.currentRequest = - Request.FlywheelRequest.TargetingVelocity( - Flywheel.TunableFlywheelStates.ejectVelocity.get() - ) - if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity) { + + if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.EJECT_GAME_PIECE } @@ -516,10 +629,8 @@ class Superstructure( } fun groundIntakeCommand(): Command { - val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() }.until { - isAtRequestedState && currentState == SuperstructureStates.GROUND_INTAKE - } + val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() } + returnCommand.name = "GroundIntakeCommand" return returnCommand } @@ -542,39 +653,64 @@ class Superstructure( return returnCommand } - fun scoreAmpCommand(): Command { + fun prepSpeakerLowCommand(): Command { + val returnCommand = + runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreSpeakerLow() }.until { + isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } + returnCommand.name = "PrepSpeakerLowCommand" + return returnCommand + } + + fun scoreCommand(): Command { + val returnCommand = + runOnce { + if (currentState == SuperstructureStates.SCORE_AMP_PREP) { + currentRequest = Request.SuperstructureRequest.ScoreAmp() + } else if (currentState == SuperstructureStates.SCORE_TRAP_PREP) { + currentRequest = Request.SuperstructureRequest.ScoreTrap() + } else { + currentRequest = Request.SuperstructureRequest.ScoreSpeaker() + } + } + .until { isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER } + returnCommand.name = "ScoreSpeakerCommand" + return returnCommand + } + + fun prepSpeakerMidCommand(): Command { val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.ScoreAmp() }.until { - isAtRequestedState && currentState == SuperstructureStates.SCORE_AMP + runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreSpeakerMid() }.until { + isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_MID_PREP } - returnCommand.name = "ScoreAmpCommand" + returnCommand.name = "ScoreSpeakerCommand" return returnCommand } - fun scoreSpeakerLowCommand(): Command { + fun prepSpeakerHighCommand(): Command { val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.ScoreSpeakerLow() }.until { - isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_LOW + runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreSpeakerHigh() }.until { + isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_HIGH_PREP } - returnCommand.name = "ScoreSpeakerLowCommand" + returnCommand.name = "ScoreSpeakerCommand" return returnCommand } - fun scoreSpeakerMidCommand(): Command { + fun prepTrapCommand(): Command { val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.ScoreSpeakerMid() }.until { - isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_MID + runOnce { currentRequest = Request.SuperstructureRequest.PrepTrap() }.until { + isAtRequestedState && currentState == SuperstructureStates.SCORE_TRAP_PREP } - returnCommand.name = "ScoreSpeakerMidCommand" + returnCommand.name = "PrepTrapCommand" return returnCommand } - fun scoreSpeakerHighCommand(): Command { + fun prepScoreTrapCommand(): Command { val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.ScoreSpeakerHigh() }.until { - isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_HIGH + runOnce { currentRequest = Request.SuperstructureRequest.PrepTrap() }.until { + isAtRequestedState && currentState == SuperstructureStates.SCORE_TRAP } - returnCommand.name = "ScoreSpeakerHighCommand" + returnCommand.name = "ScoreTrapCommand" return returnCommand } @@ -684,15 +820,15 @@ class Superstructure( SCORE_AMP_PREP, SCORE_AMP, SCORE_SPEAKER_LOW_PREP, - SCORE_SPEAKER_LOW, SCORE_SPEAKER_MID_PREP, - SCORE_SPEAKER_MID, SCORE_SPEAKER_HIGH_PREP, - SCORE_SPEAKER_HIGH, + SCORE_SPEAKER, + SCORE_TRAP_PREP, + SCORE_TRAP, CLIMB_EXTEND, CLIMB_RETRACT, EJECT_GAME_PIECE, - EJECT_GAME_PIECE_PREP + EJECT_GAME_PIECE_PREP, } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index c3ee809f..3503eb8b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -3,6 +3,7 @@ package com.team4099.robot2023.subsystems.vision import com.team4099.lib.hal.Clock import com.team4099.lib.logging.TunableNumber import com.team4099.lib.vision.TimestampedVisionUpdate +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.vision.camera.CameraIO import edu.wpi.first.math.VecBuilder @@ -13,15 +14,12 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Pose3dWPILIB -import org.team4099.lib.geometry.Quaternion -import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.radians import java.util.function.Consumer import java.util.function.Supplier import kotlin.math.pow @@ -86,164 +84,59 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { for (instance in io.indices) { - for (frameIndex in inputs[instance].timestamps.indices) { - lastFrameTimes[instance] = Clock.fpgaTime - val timestamp = inputs[instance].timestamps[frameIndex] - val values = inputs[instance].frames[frameIndex] + lastFrameTimes[instance] = Clock.fpgaTime + val timestamp = inputs[instance].timestamp + val values = inputs[instance].frame - var cameraPose: Pose3d? = null - var robotPose: Pose2d? = null + var cameraPose: Pose3d? = inputs[instance].frame + var robotPose: Pose2d? = cameraPose?.transformBy(cameraPoses[instance])?.toPose2d() - when (values[0]) { - 1.0 -> { - cameraPose = - Pose3d( - values[2].meters, - values[3].meters, - values[4].meters, - Rotation3d(Quaternion(values[5].radians, values[6], values[7], values[8])) - ) - - // - // Logger.recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform", - // cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d) - - robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d() - println( - "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}" - ) - } - 2.0 -> { - val error0 = values[1] - val error1 = values[9] - - var use0 = false - var use1 = false - - if (error0 < error1 * ambiguityThreshold) { - use0 = true - } else if (error1 < error0 * ambiguityThreshold) { - use1 = true - } - - if (use0) { - cameraPose = - Pose3d( - values[2].meters, - values[3].meters, - values[4].meters, - Rotation3d(Quaternion(values[5].radians, values[6], values[7], values[8])) - ) - - robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d() - } else if (use1) { - cameraPose = - Pose3d( - values[10].meters, - values[11].meters, - values[12].meters, - Rotation3d( - Quaternion(values[13].radians, values[14], values[15], values[16]) - ) - ) - - robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d() - } else { - - val cameraPose0 = - Pose3d( - values[2].meters, - values[3].meters, - values[4].meters, - Rotation3d(Quaternion(values[5].radians, values[6], values[7], values[8])) - ) - - val robotPose0 = cameraPose0.transformBy(cameraPoses[instance].inverse()).toPose2d() - - val cameraPose1 = - Pose3d( - values[10].meters, - values[11].meters, - values[12].meters, - Rotation3d( - Quaternion(values[13].radians, values[14], values[15], values[16]) - ) - ) - - val robotPose1 = cameraPose1.transformBy(cameraPoses[instance].inverse()).toPose2d() - - if (robotPose0.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue < - robotPose1.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue - ) { - cameraPose = cameraPose0 - robotPose = robotPose0 - } else { - cameraPose = cameraPose1 - robotPose = robotPose1 - } - - // - // Logger.recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform", - // cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d) - } - } - } - - if (cameraPose == null || robotPose == null) { - continue - } + if (cameraPose == null || robotPose == null) { + continue + } - if ((robotPose.rotation - fieldTCurrentRobotEstimate.rotation).absoluteValue > 7.degrees && - DriverStation.isEnabled() - ) { - continue - } + if ((robotPose.rotation - fieldFramePoseSupplier.get().rotation).absoluteValue > 7.degrees && + DriverStation.isEnabled() + ) { + continue + } - // Find all detected tag poses - val tagPoses = mutableListOf() - for (i in (if (values[0] == 1.0) 9 else 17) until values.size) { - val tagId: Int = values[i].toInt() - lastTagDetectionTimes[tagId] = Clock.fpgaTime - // TODO: Convert detected tag to the actual pose for 2024 - } + // Find all detected tag poses + val tagPoses = inputs[instance].usedTargets.map { FieldConstants.fieldAprilTags[it].pose } - // Calculate average distance to tag - var totalDistance = 0.0.meters - for (tagPose in tagPoses) { - totalDistance += tagPose.translation.getDistance(cameraPose.translation) - } - val averageDistance = totalDistance / tagPoses.size + // Calculate average distance to tag + var totalDistance = 0.0.meters + for (tagPose in tagPoses) { + totalDistance += tagPose.translation.getDistance(cameraPose.translation) + } + val averageDistance = totalDistance / tagPoses.size - // Add to vision updates - val xyStdDev = xyStdDevCoefficient.get() * averageDistance.inMeters.pow(2) / tagPoses.size - val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size + // Add to vision updates + val xyStdDev = xyStdDevCoefficient.get() * averageDistance.inMeters.pow(2) / tagPoses.size + val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size - visionUpdates.add( - TimestampedVisionUpdate( - timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) - ) + visionUpdates.add( + TimestampedVisionUpdate( + timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) ) - robotPoses.add(robotPose) + ) + robotPoses.add(robotPose) - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS", - (Clock.fpgaTime - timestamp).inMilliseconds - ) + Logger.recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS", + (Clock.fpgaTime - timestamp).inMilliseconds + ) - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", - Pose2dWPILIB.struct, - robotPose.pose2d - ) + Logger.recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", robotPose.pose2d + ) - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", - Pose3dWPILIB.struct, - *tagPoses.map { it.pose3d }.toTypedArray() - ) - } + Logger.recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", + *tagPoses.map { it.pose3d }.toTypedArray() + ) - if (inputs[instance].timestamps.isEmpty()) { + if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol Logger.recordOutput( "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", Pose2dWPILIB.struct, @@ -258,23 +151,24 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { *arrayOf() ) } - } - val allTagPoses = mutableListOf() - // for (detectionEntry in lastTagDetectionTimes.entries) { - // if (Clock.fpgaTime - detectionEntry.value < targetLogTime) { - // FieldConstants.getTagPose(detectionEntry.key)?.let { allTagPoses.add(it) } - // } - // } + val allTagPoses = mutableListOf() + // for (detectionEntry in lastTagDetectionTimes.entries) { + // if (Clock.fpgaTime - detectionEntry.value < targetLogTime) { + // FieldConstants.getTagPose(detectionEntry.key)?.let { allTagPoses.add(it) } + // } + // } - Logger.recordOutput( - "Vision/allTagPoses", Pose3dWPILIB.struct, *allTagPoses.map { it.pose3d }.toTypedArray() - ) + Logger.recordOutput( + "Vision/allTagPoses", Pose3dWPILIB.struct, *allTagPoses.map { it.pose3d }.toTypedArray() + ) - visionConsumer.accept(visionUpdates) + visionConsumer.accept(visionUpdates) - Logger.recordOutput( - "LoggedRobot/Subsystems/VisionLoopTimeMS", (Clock.realTimestamp - startTime).inMilliseconds - ) + Logger.recordOutput( + "LoggedRobot/Subsystems/VisionLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) + } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt index 969e3876..557cb15c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt @@ -2,39 +2,55 @@ package com.team4099.robot2023.subsystems.vision.camera import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.base.Time +import org.photonvision.common.dataflow.structures.Packet +import org.photonvision.targeting.PhotonTrackedTarget +import org.photonvision.targeting.PhotonTrackedTarget.APacketSerde +import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Pose3dWPILIB import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.seconds interface CameraIO { class CameraInputs : LoggableInputs { - var timestamps = listOf