diff --git a/.cursor/rules/java.mdc b/.cursor/rules/java.mdc index 055f4fb..2588b88 100644 --- a/.cursor/rules/java.mdc +++ b/.cursor/rules/java.mdc @@ -2,3 +2,10 @@ globs: src/main/java/** alwaysApply: false --- + +Use this rule when writing or refactoring Java robot code under `src/main/java/**`. + +## Expected notation + +- **Public static methods**: Must start with an uppercase letter. + - Example: `public static void Register(...)` (not `public static void register(...)`). diff --git a/.cursor/rules/lighting.mdc b/.cursor/rules/lighting.mdc new file mode 100644 index 0000000..d32b79c --- /dev/null +++ b/.cursor/rules/lighting.mdc @@ -0,0 +1,70 @@ +--- +description: Rules for extending and using the robot lighting framework (LightsSubsystem, lighting effects, and polling-based multi-command orchestration) +globs: + - src/main/java/frc/robot/subsystem/LightsSubsystem.java + - src/main/java/frc/robot/util/lighting/** + - src/main/java/frc/robot/command/lighting/** + - src/main/java/frc/robot/command/util/PollingCommand.java + - src/main/java/frc/robot/constant/LEDConstants.java + - src/main/java/frc/robot/RobotContainer.java +alwaysApply: false +--- + +Use this rule when modifying robot lights code. + +## Core ownership model + +- `LightsSubsystem` is the only class that should write LED hardware frames. +- `LightsEngine` owns effect composition, priorities, blend logic, diffing, and compression. +- `PollingCommand` is the default command of `LightsSubsystem` and runs multiple lighting commands per loop. + +Do not add direct CANdle writes in lighting commands. + +## Multi-command model (important) + +The lights subsystem supports multiple independent lighting commands via polling: +- Register commands with `LightsSubsystem.addLightsCommand(...)`. +- Keep the default command as the internal `PollingCommand`. +- Do not overwrite default command in `RobotContainer` for lights orchestration. + +If adding a new lighting behavior, make it a command in `src/main/java/frc/robot/command/lighting`. + +## Read-only command expectations + +Lighting commands should be read-only signal producers: +- Read robot state (sensors, subsystem status, operator inputs). +- Create effects in `initialize()`. +- Update effect parameters in `execute()` (for example `setInput` / `setProgress`). +- Cleanup with `removeEffect(...)` in `end(...)` when appropriate. + +They should not: +- Mutate unrelated robot subsystem state. +- Perform scheduling side effects. +- Send raw hardware controls directly. + +## API conventions + +- Prefer named zones through `lights.rangeOf(LightZone.X)` over raw indices when possible. +- Keep `EffectHandle` references in command state; this is the stable id for updates/removal. +- Prefer typed updates through `setInput(handle, value)` when an effect exposes a typed input. +- For base layers: lower priority + `BlendMode.OVERWRITE`. +- For overlays: higher priority + `BlendMode.ADD`. +- Clamp-like behavior is already handled in effect/color classes; do not duplicate ad hoc clamp logic unless needed. + +## Render semantics to preserve + +- Effects are composited in ascending priority. +- `OVERWRITE` replaces touched pixels only. +- `ADD` channel-adds and clamps each channel to 255. +- If frame diff is empty, no writes are emitted. +- If segment count exceeds `LEDConstants.maxSolidWritesPerCycle`, adaptive compression must remain active. + +## Safe extension checklist + +When introducing a new effect type: +1. Add effect class under `src/main/java/frc/robot/util/lighting/effects`. +2. Extend `LightsSubsystem` with a typed `addX(...)` creation method. +3. Choose an input type `T` for `LightEffect` (`Void` if no runtime input), and wire `updateInput` if needed. +4. Keep behavior deterministic with respect to `(ledIndex, nowSeconds)` inputs. +5. Add/extend tests in `src/test/java/frc/robot/util/lighting`. +6. Preserve existing priority/blend/segment invariants. diff --git a/.gitignore b/.gitignore index e98c7db..504fa00 100644 --- a/.gitignore +++ b/.gitignore @@ -208,4 +208,6 @@ src/main/deploy/config *.db .coverage +*.lock +*.bin checkpoints/ diff --git a/.gradle-local/permwrapper/dists/gradle-8.5-bin/5t9huq95ubn472n8rpzujfbqh/gradle-8.5-bin.zip.lck b/.gradle-local/permwrapper/dists/gradle-8.5-bin/5t9huq95ubn472n8rpzujfbqh/gradle-8.5-bin.zip.lck new file mode 100644 index 0000000..e69de29 diff --git a/.gradle-local/permwrapper/dists/gradle-8.5-bin/5t9huq95ubn472n8rpzujfbqh/gradle-8.5-bin.zip.part b/.gradle-local/permwrapper/dists/gradle-8.5-bin/5t9huq95ubn472n8rpzujfbqh/gradle-8.5-bin.zip.part new file mode 100644 index 0000000..e69de29 diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..fe4a6b7 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,37 @@ +# AGENTS + +## Verified workflows + +- Workspace bootstrap: `git submodule update --init --recursive` then `npm install` +- Full robot build: `./gradlew build` + - Also runs dynamic vendor dependency build (`scripts/clone_and_build_repos.py --config-file-path config.ini`) and protobuf generation. +- Java build wrapper: `make build` +- Java simulation: `./gradlew simulateJava` +- Robot deploy: `./gradlew deploy -PteamNumber=` +- Robot deploy wrapper: `make deploy` (uses `TEAM_NUMBER`, default `4765`) +- Combined deploy flow: `./gradlew deployAll` + - Runs robot deploy plus backend deploy task. +- Gradle backend deploy task: `./gradlew deployBackend` + - Invokes `make deploy-backend` and validates deployed Pi count against `EXPECTED_NUM_OF_PIS`. +- Python backend deploy directly: `make deploy-backend` +- Python test flow: `make initialize` (creates `.venv`, installs `requirements.txt`, then runs tests) and `make test` +- Config generation from TypeScript: `npm run config -- --dir src/config` +- Regenerate Thrift TS bindings: `npm run generate-thrift` +- Generate backend code artifacts: `make generate` (Python protobuf + Python thrift + Java proto task) +- Java protobuf generation via Gradle: `./gradlew generate` (depends on `generateProto`) + +## Command notes + +- `make build` and `make deploy` enforce Java 17 via `/usr/libexec/java_home -v 17`. +- `make deploy` defaults `TEAM_NUMBER=4765` unless overridden. +- `./gradlew deployBackend` expects deployment on `EXPECTED_NUM_OF_PIS` (currently `2`) and fails if mismatch. +- Dynamic vendor dependency builds are intentionally forced every Gradle compile/build cycle (`buildDynamicDeps.outputs.upToDateWhen { false }`). + +## TODO + +- Confirm whether automation should keep `TEAM_NUMBER=4765` and `EXPECTED_NUM_OF_PIS=2` as defaults or document per-robot override policy. +- README references `applyBackend` and `make prep-project`; verify whether docs should be updated to `deployBackend`/`make initialize`. + +## Agent docs + +- Lighting API and extension guide: `docs/LightingApiForAgents.md` diff --git a/Makefile b/Makefile index 4fe96b1..a7b5ec6 100644 --- a/Makefile +++ b/Makefile @@ -14,6 +14,16 @@ THRIFT_GEN_DIR = $(GEN_DIR)/thrift THRIFT_TS_SCHEMA_GEN_DIR = $(THRIFT_GEN_DIR)/ts_schema PROTO_PY_GEN_DIR = $(PROTO_GEN_DIR)/python +TEAM_NUMBER=4765 + +build: + export JAVA_HOME=$(/usr/libexec/java_home -v 17) + ./gradlew build + +deploy: + export JAVA_HOME=$(/usr/libexec/java_home -v 17) + ./gradlew deploy -PteamNumber=$(TEAM_NUMBER) + initialize: python3 -m venv .venv .venv/bin/pip install -r requirements.txt @@ -37,7 +47,10 @@ thrift-to-py: -out $(THRIFT_GEN_DIR) \ $(THRIFT_ROOT_FILE); -generate: generate-proto-python thrift-to-py +proto-to-java: + ./gradlew generateProto + +generate: generate-proto-python thrift-to-py proto-to-java deploy-backend: PYTHONPATH="$(PWD)/src" $(VENV_PYTHON) -m backend.deploy \ No newline at end of file diff --git a/ThriftTsConfig b/ThriftTsConfig index 7c6829a..fc29cf2 160000 --- a/ThriftTsConfig +++ b/ThriftTsConfig @@ -1 +1 @@ -Subproject commit 7c6829a28e6e6b31d04fcd941779fddb42fb827d +Subproject commit fc29cf2bacb3399bda72830e76911d6d2adab6ea diff --git a/build.gradle b/build.gradle index 38faed0..f80a37a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,233 +1,240 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2026.2.1" - id "com.google.protobuf" version "0.9.3" - id "groovy" -} - -java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 - toolchain { - languageVersion = JavaLanguageVersion.of(17) - } -} - -ext { - alreadyAdded = [] -} - -repositories { - mavenCentral() - maven { - url 'https://jitpack.io' - } -} - -def ROBOT_MAIN_CLASS = "frc.robot.Main" - -def EXPECTED_NUM_OF_PIS = 3; -def dynamicLibDir = layout.projectDirectory.dir("lib/build") - -// Define deploy target and artifacts -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) - - artifacts { - frcJava(getArtifactTypeClass('FRCJavaArtifact')) {} - - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree("src/main/deploy") - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -wpi.java.debugJni = false - -def includeDesktopSupport = false - -def buildDynamicDepsTask = tasks.register("buildDynamicDeps") { - group = "build setup" - description = "Clones/builds dynamic deps and drops jars into lib/build" - - inputs.file("config.ini") - outputs.dir(dynamicLibDir) - outputs.upToDateWhen { false } // run every build cycle so deps are always refreshed - - doLast { - def stdout = new ByteArrayOutputStream() - def stderr = new ByteArrayOutputStream() - - try { - exec { - def javaHome = - System.getenv("JAVA_HOME") - ?: (project.findProperty("org.gradle.java.home") as String) - ?: System.getProperty("java.home") - - environment "JAVA_HOME", javaHome - environment "PATH", "${javaHome}/bin:${System.getenv("PATH")}" - - commandLine "python3", "scripts/clone_and_build_repos.py", - "--config-file-path", "config.ini" - - standardOutput = stdout - errorOutput = stderr - } - } catch (Exception e) { - // print error output, then rethrow - def err = stderr.toString().trim() - if (err) println err - println "Exception during exec: $e" - throw e - } - - println stdout.toString().trim() - def err = stderr.toString().trim() - if (err) println err - } -} - -// Create a provider that forces lazy evaluation of the directory -// This ensures VSCode picks up new JARs when it refreshes the classpath -def dynamicJarsProvider = buildDynamicDepsTask.map { - dynamicLibDir -} - -// Define project dependencies -dependencies { - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() - - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() - - compileOnly 'org.projectlombok:lombok:1.18.30' - annotationProcessor 'org.projectlombok:lombok:1.18.30' - - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - - implementation "com.google.protobuf:protobuf-java:3.22.2" - // Use provider with fileTree to force lazy evaluation so VSCode picks up new JARs - implementation(files(dynamicJarsProvider.map { dir -> - fileTree(dir: dir, include: "*.jar") - }).builtBy(buildDynamicDepsTask)) -} - -test { - useJUnitPlatform() - systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' -} - -// Simulation settings -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() - -// Create a fat jar (including all dependencies) and set the manifest for WPILib -jar { - from sourceSets.main.output - from({ configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }) - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.EXCLUDE -} - -// Set the jar task for deployment -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) - - -protobuf { - protoc { - artifact = "com.google.protobuf:protoc:3.22.2" - } - generateProtoTasks { - all().each { task -> - task.builtins { - java {} - } - } - } - - generatedFilesBaseDir = "$buildDir/generated/source" -} - -sourceSets { - main { - java { - srcDirs += "$buildDir/generated/source/proto/main/java" - } - proto { - srcDirs = ['src/proto'] - } - } -} - -tasks.register("deployBackend") { - group = "build" - description = "Deploy backend" - doLast { - def stdout = new ByteArrayOutputStream() - def stderr = new ByteArrayOutputStream() - def buildFailed = false - - try { - project.exec { - commandLine "make", "deploy-backend" - standardOutput = stdout - errorOutput = stderr - ignoreExitValue = false - } - } catch (Exception e) { - buildFailed = true - println "Failed to apply backend: ${e.message}" - println "Error Output:\n${stderr.toString().trim()}" - } - - println stdout - println stderr - - def expectedNumOfPisStr = EXPECTED_NUM_OF_PIS.toString() - def deployedPisRegex = /Deployed on (\d+) Pis/ - - def outputCombined = stdout.toString() + "\n" + stderr.toString() - def matcher = (outputCombined =~ deployedPisRegex) - def deployedNumOfPis = matcher.find() ? matcher.group(1) : "unknown" - - if (!outputCombined.contains("Deployed on " + expectedNumOfPisStr + " Pis")) { - println "" - println "=============================================================" - println "Failed to apply backend because it was not deployed on the expected number of Pis. Please set the expected number of Pis in the build.gradle file and try again (check 'EXPECTED_NUM_OF_PIS' variable)." - println "Expected number of Pis: " + expectedNumOfPisStr - println "Deployed number of Pis: " + deployedNumOfPis - println "=============================================================" - println "" - - throw new Exception("Failed to apply backend because it was not deployed on the expected number of Pis (expected " + expectedNumOfPisStr + " Pis, deployed " + deployedNumOfPis + " Pis)") - } - } -} - -tasks.jar.dependsOn(tasks.generateProto) -tasks.compileJava.dependsOn(tasks.generateProto) -tasks.compileJava.dependsOn(tasks.buildDynamicDeps) \ No newline at end of file +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "com.google.protobuf" version "0.9.3" + id "groovy" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 + toolchain { + languageVersion = JavaLanguageVersion.of(17) + } +} + +ext { + alreadyAdded = [] +} + +repositories { + mavenCentral() + maven { + url 'https://jitpack.io' + } +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +def EXPECTED_NUM_OF_PIS = 3; +def dynamicLibDir = layout.projectDirectory.dir("lib/build") + +// Define deploy target and artifacts +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + frcJava(getArtifactTypeClass('FRCJavaArtifact')) {} + + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree("src/main/deploy") + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +wpi.java.debugJni = false + +def includeDesktopSupport = false + +def buildDynamicDepsTask = tasks.register("buildDynamicDeps") { + group = "build setup" + description = "Clones/builds dynamic deps and drops jars into lib/build" + + inputs.file("config.ini") + outputs.dir(dynamicLibDir) + outputs.upToDateWhen { false } // run every build cycle so deps are always refreshed + + doLast { + def stdout = new ByteArrayOutputStream() + def stderr = new ByteArrayOutputStream() + + try { + exec { + def javaHome = + System.getenv("JAVA_HOME") + ?: (project.findProperty("org.gradle.java.home") as String) + ?: System.getProperty("java.home") + + environment "JAVA_HOME", javaHome + environment "PATH", "${javaHome}/bin:${System.getenv("PATH")}" + + commandLine "python3", "scripts/clone_and_build_repos.py", + "--config-file-path", "config.ini" + + standardOutput = stdout + errorOutput = stderr + } + } catch (Exception e) { + // print error output, then rethrow + def err = stderr.toString().trim() + if (err) println err + println "Exception during exec: $e" + throw e + } + + println stdout.toString().trim() + def err = stderr.toString().trim() + if (err) println err + } +} + +// Create a provider that forces lazy evaluation of the directory +// This ensures VSCode picks up new JARs when it refreshes the classpath +def dynamicJarsProvider = buildDynamicDepsTask.map { + dynamicLibDir +} + +// Define project dependencies +dependencies { + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + compileOnly 'org.projectlombok:lombok:1.18.30' + annotationProcessor 'org.projectlombok:lombok:1.18.30' + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + implementation "com.google.protobuf:protobuf-java:3.22.2" + // Use provider with fileTree to force lazy evaluation so VSCode picks up new JARs + implementation(files(dynamicJarsProvider.map { dir -> + fileTree(dir: dir, include: "*.jar") + }).builtBy(buildDynamicDepsTask)) +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation settings +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Create a fat jar (including all dependencies) and set the manifest for WPILib +jar { + from sourceSets.main.output + from({ configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }) + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.EXCLUDE +} + +// Set the jar task for deployment +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + + +protobuf { + protoc { + artifact = "com.google.protobuf:protoc:3.22.2" + } + generateProtoTasks { + all().each { task -> + task.builtins { + java {} + } + } + } + + generatedFilesBaseDir = "$buildDir/generated/source" +} + +sourceSets { + main { + java { + srcDirs += "$buildDir/generated/source/proto/main/java" + } + proto { + srcDirs = ['src/proto'] + } + } +} + +tasks.register("deployBackend") { + group = "build" + description = "Deploy backend" + doLast { + def stdout = new ByteArrayOutputStream() + def stderr = new ByteArrayOutputStream() + def buildFailed = false + + try { + project.exec { + commandLine "make", "deploy-backend" + standardOutput = stdout + errorOutput = stderr + ignoreExitValue = false + } + } catch (Exception e) { + buildFailed = true + println "Failed to apply backend: ${e.message}" + println "Error Output:\n${stderr.toString().trim()}" + } + + println stdout + println stderr + + def expectedNumOfPisStr = EXPECTED_NUM_OF_PIS.toString() + def deployedPisRegex = /Deployed on (\d+) Pis/ + + def outputCombined = stdout.toString() + "\n" + stderr.toString() + def matcher = (outputCombined =~ deployedPisRegex) + def deployedNumOfPis = matcher.find() ? matcher.group(1) : "unknown" + + if (!outputCombined.contains("Deployed on " + expectedNumOfPisStr + " Pis")) { + println "" + println "=============================================================" + println "Failed to apply backend because it was not deployed on the expected number of Pis. Please set the expected number of Pis in the build.gradle file and try again (check 'EXPECTED_NUM_OF_PIS' variable)." + println "Expected number of Pis: " + expectedNumOfPisStr + println "Deployed number of Pis: " + deployedNumOfPis + println "=============================================================" + println "" + + throw new Exception("Failed to apply backend because it was not deployed on the expected number of Pis (expected " + expectedNumOfPisStr + " Pis, deployed " + deployedNumOfPis + " Pis)") + } + } +} + +tasks.jar.dependsOn(tasks.generateProto) +tasks.compileJava.dependsOn(tasks.generateProto) +tasks.compileJava.dependsOn(tasks.buildDynamicDeps) + +tasks.register("deployAll") +tasks.deployAll.dependsOn(tasks.deploy) +tasks.deployAll.dependsOn(tasks.deployBackend) + +tasks.register("generate") +tasks.generate.dependsOn(tasks.generateProto) \ No newline at end of file diff --git a/docs/ButtonMappingVisualization.md b/docs/ButtonMappingVisualization.md new file mode 100644 index 0000000..145aefa --- /dev/null +++ b/docs/ButtonMappingVisualization.md @@ -0,0 +1,113 @@ +# Robot Button Mapping + +Source of truth: [RobotContainer.java](/Users/godbrigero/Documents/2026Rebuilt/src/main/java/frc/robot/RobotContainer.java) + +This visualization reflects the current bindings in `RobotContainer`. It includes both direct button bindings and the analog inputs used by default commands. + +## Visual Map + +```mermaid +flowchart TB + subgraph LS["Left Flight Stick (USB 2)"] + LS_ROT["Twist / rotation axis"] --> DRIVE_ROT["Swerve rotation command"] + LS_B5["B5 press"] --> GPS_TOGGLE["Toggle swerve GPS assist"] + end + + subgraph RS["Right Flight Stick (USB 3)"] + RS_Y["Joystick Y axis"] --> DRIVE_X["Swerve X translation"] + RS_X["Joystick X axis"] --> DRIVE_Y["Swerve Y translation"] + RS_B5["B5 press"] --> RESET_DRIVER["Reset driver-relative swerve heading"] + RS_B17["B17 hold"] --> EXTAKE["Override intake to extake"] + RS_SLIDER["Right slider"] --> MANUAL_SHOOT["Manual shooter speed setpoint"] + end + + subgraph OP["Operator Panel (USB 1)"] + OP_BLACK["Black button hold"] --> RESET_GLOBAL["Continuously align unified gyro to global pose"] + OP_GREEN["Green button press"] --> GPS_AIM["Toggle turret + shooter GPS assist"] + OP_WHEEL["Wheel axis"] --> MANUAL_AIM["Manual turret aim when GPS assist is off"] + OP_TOGGLE_MID["Toggle wheel middle"] --> INTAKE_TOP["Set intake idle wrist position to TOP"] + OP_TOGGLE_MIDDOWN["Toggle wheel mid-down"] --> INTAKE_MID["Set intake idle wrist position to MIDDLE"] + OP_METAL["Metal switch down"] --> SHOOT_ENABLE["Enable shooting command"] + OP_METAL --> INTAKE_ENABLE["Lower intake wrist and run intake"] + OP_METAL -. switch up .-> BASE_SPEED["Shooter idles at base speed"] + end + + GPS_TOGGLE --> DRIVE_MODE["Field-relative swerve with optional lane assist"] + DRIVE_X --> DRIVE_MODE + DRIVE_Y --> DRIVE_MODE + DRIVE_ROT --> DRIVE_MODE + + GPS_AIM --> TURRET_MODE["Turret default: auto aim or manual wheel aim"] + GPS_AIM --> SHOOT_MODE["Shooter while metal switch is down: auto target or manual speed"] + MANUAL_AIM --> TURRET_MODE + MANUAL_SHOOT --> SHOOT_MODE + SHOOT_ENABLE --> SHOOT_MODE + INTAKE_ENABLE --> INTAKE_MODE["Intake default command"] + EXTAKE --> INTAKE_MODE +``` + +## Binding Table + +| Control | Type | Behavior | +| --- | --- | --- | +| Left flight stick twist | Analog | Commands swerve rotation | +| Left flight stick `B5` | Press | Toggles swerve GPS assist / lane-assist features | +| Right flight stick X | Analog | Commands swerve Y translation | +| Right flight stick Y | Analog | Commands swerve X translation | +| Right flight stick `B5` | Press | Resets driver-relative swerve heading | +| Right flight stick `B17` | Hold | Forces intake to extake instead of intake | +| Right flight stick slider | Analog | Sets manual shooter velocity when shooter GPS assist is off | +| Operator panel wheel | Analog | Manual turret target when turret GPS assist is off | +| Operator panel green button | Press | Toggles turret GPS assist and shooter GPS assist; cancels current turret/shooter command so defaults restart in the new mode | +| Operator panel black button | Hold | Repeatedly resets unified gyro rotation to the current global pose heading | +| Operator panel toggle wheel middle | Press | Sets the intake idle wrist position to `TOP` | +| Operator panel toggle wheel mid-down | Press | Sets the intake idle wrist position to `MIDDLE` | +| Operator panel metal switch down | Hold state | Enables shooter command and also drives intake behavior | +| Operator panel metal switch up | Hold state | Shooter runs base speed instead of the active shooting command | + +## Command-Level Behavior + +### Drivetrain + +| Inputs | Result | +| --- | --- | +| Left stick twist + right stick X/Y | Default teleop swerve driving | +| Left stick `B5` | Enables/disables GPS-assisted lane adjustment inside swerve teleop | +| Right stick `B5` | Re-zeros the driver-relative reference heading | +| Operator panel black button | Resets full gyro heading from global position while held | + +### Turret + +| Condition | Active behavior | +| --- | --- | +| Turret GPS assist enabled | `ContinuousAimCommand` aims at `AimPoint.getTarget()` | +| Turret GPS assist disabled | `ManualAimCommand` maps the operator wheel to turret position | +| Operator panel green button | Toggles between those two modes | + +### Shooter + +| Condition | Active behavior | +| --- | --- | +| Metal switch down + shooter GPS assist enabled | `ContinuousShooter` uses `AimPoint.getTarget()` | +| Metal switch down + shooter GPS assist disabled | `ContinuousManualShooter` uses the right-stick slider for shooter speed | +| Metal switch up | Shooter falls back to base speed | +| Operator panel green button | Toggles shooter GPS assist mode | + +### Intake + +| Condition | Active behavior | +| --- | --- | +| Metal switch down | Intake wrist moves to bottom and intake motor runs | +| Metal switch up | Intake motor stops and intake wrist moves to the currently selected idle raise location | +| Right stick `B17` while intake is active | Intake motor reverses to extake | +| Operator panel toggle wheel middle | Changes the idle raise location to `TOP` | +| Operator panel toggle wheel mid-down | Changes the idle raise location to `MIDDLE` | + +## Notes + +| Item | Detail | +| --- | --- | +| Shared switch | The operator panel metal switch affects both shooter and intake behavior | +| Intake idle state | Intake defaults to `MIDDLE` when not active until changed by a toggle wheel binding | +| Climber | No climber bindings are currently implemented in `RobotContainer` | +| Autonomous named commands | `ContinuousAimCommand`, `IntakeCommand`, and `ContinuousShooterCommand` are also registered for PathPlanner autos, but they are not direct driver controls | diff --git a/docs/LightingApiForAgents.md b/docs/LightingApiForAgents.md new file mode 100644 index 0000000..840ffb3 --- /dev/null +++ b/docs/LightingApiForAgents.md @@ -0,0 +1,124 @@ +# Lighting API Guide (For AI Agents) + +This document explains how to use and extend the robot lighting system safely. + +## Architecture + +- `LightsSubsystem` is the single owner of LED hardware writes. +- `LightsEngine` builds each frame by compositing active effects. +- `CandleLightsTransport` sends contiguous color segments to CANdle. +- `PollingCommand` is the default command on `LightsSubsystem` and runs multiple light commands each cycle. + +Key files: +- `src/main/java/frc/robot/subsystem/LightsSubsystem.java` +- `src/main/java/frc/robot/util/lighting/LightsEngine.java` +- `src/main/java/frc/robot/command/util/PollingCommand.java` + +## Command Model (Important) + +Lighting behavior commands are intended to be **read-only signal producers**: +- They should read robot state and update effect parameters (for example progress). +- They should not directly write CANdle hardware. +- They should not own scheduling of `LightsSubsystem` directly. + +Register them through: +- `LightsSubsystem.GetInstance().addLightsCommand(commandA, commandB, ...)` + +Do **not** replace the lights default command in `RobotContainer`; the subsystem installs its poller internally. + +## API Surface + +Core types: +- `LedRange(startInclusive, endInclusive)` and `LedRange.single(index)` +- `LedColor(red, green, blue, white)` (clamped to 0..255) +- `EffectHandle` (opaque effect id + input type marker) +- `BlendMode.OVERWRITE` and `BlendMode.ADD` +- `LightZone` enum (`ONBOARD`, `FULL_STRIP`, `EXTERNAL_STRIP`, `LEFT_HALF`, `RIGHT_HALF`) + +Primary `LightsSubsystem` methods: +- `addSolid(...)` +- `addProgressBar(...)` +- `addBlink(...)` +- `addBreathe(...)` +- `addChase(...)` +- `addLarsonScanner(...)` (Knight Rider / Cylon) +- `addConvergingArrows(...)` (aiming reticle; drive exactness via `setInput(handle, 0..1)`) +- `addRainbow(...)` +- `setProgress(handle, value01)` +- `setInput(handle, typedValue)` (generic typed input channel) +- `setEnabled(handle, enabled)` +- `setPriority(handle, priority)` +- `removeEffect(handle)` +- `clearEffects()` + +`LightsApi` supports typed handles: +- Effects that accept scalar progress typically return `EffectHandle`. +- Effects without runtime input typically return `EffectHandle`. +- Use `setInput(handle, value)` for generic typed updates. +- `setProgress(...)` remains available as a convenience path. + +Use `LightsApi` when you only need progress bars. Use `LightsSubsystem` when you need richer effects. + +## Render Semantics + +- Priorities are processed ascending (`low -> high`). +- `OVERWRITE` replaces previous color for touched pixels. +- `ADD` adds channel values and clamps each channel to 255. +- If no pixels changed from prior frame, no hardware writes are sent. +- If segment count exceeds `LEDConstants.maxSolidWritesPerCycle`, adaptive compression is applied. + +## How To Add A New Lighting Command + +1. Create a command under `src/main/java/frc/robot/command/lighting`. +2. In `initialize()`, create effect(s) and store `EffectHandle`s. +3. In `execute()`, only update existing handles based on robot state. +4. In `end(...)`, remove effects you no longer want active (recommended). +5. Register command with `addLightsCommand(...)` in `RobotContainer`. + +Minimal pattern: + +```java +public class ExampleLightCommand extends Command { + private final LightsSubsystem lights; + private EffectHandle handle; + + public ExampleLightCommand(LightsSubsystem lights) { + this.lights = lights; + } + + @Override + public void initialize() { + handle = lights.addProgressBar(new LedRange(8, 67), + new LedColor(0, 255, 0), new LedColor(10, 10, 10), + 10, BlendMode.OVERWRITE); + } + + @Override + public void execute() { + lights.setInput(handle, 0.5); + } + + @Override + public void end(boolean interrupted) { + if (handle != null) { + lights.removeEffect(handle); + } + } +} +``` + +## Conventions For Agents + +- Prefer named zones (`lights.rangeOf(LightZone.X)`) before raw indices. +- Keep LED commands deterministic and side-effect light. +- Avoid heavy allocations in `execute()`. +- Treat `EffectHandle` as the only durable reference to a created effect. +- For overlays, use higher priority plus `BlendMode.ADD`. +- For baseline backgrounds, use lower priority plus `BlendMode.OVERWRITE`. + +## Troubleshooting + +- LEDs not changing: verify the command is registered via `addLightsCommand(...)`. +- Effect not visible: check priority and blend mode interactions. +- Unexpected dimming/artifacts: adaptive compression may be active (`Lights/AdaptiveCompressionActive` log key). +- Out-of-range exception: confirm index/range against `LEDConstants` (`0..399`). diff --git a/scripts/turret/linear_regression_creator.py b/scripts/turret/linear_regression_creator.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/turret/write_data.html b/scripts/turret/write_data.html new file mode 100644 index 0000000..8f48632 --- /dev/null +++ b/scripts/turret/write_data.html @@ -0,0 +1,1377 @@ + + + + + + Turret Data Recorder (v, t, d) + + + +
+
+
+
+

Turret Data Recorder

+
+ Record samples for regression training: velocity (m/s), time + (s), distance (d). + autosave: … +
+
+
+ entries: 0 + avg v: – + avg t: – + avg d: – +
+
+
+
+ +
+
+
+
+ Add entry + Tip: press Enter to add +
+
+
+
+ +
+ + + + +
+
+ +
+ + +
+ +
+ + + +
+ + + +
+ + +
+ +
+ - **Autosaves** to this browser (local storage). Export anytime to a JSON file.
+ - Distance units are whatever you use consistently (meters recommended). +
+
+
+
+ +
+
+ Entries +
+ + + + +
+
+
+
+ + + + + + + + + + + + +
#v (m/s)t (s)dnoteactions
+
+
+ Export format: + {"tests":[{"name":"Test 1","entries":[{"v":0,"t":0,"d":0}]}]} +
+
+
+ +
+
+ Charts + Point graphs: velocity vs time, velocity vs distance +
+
+
+
+
v (m/s) vs t (s)
+ +
+
+
v (m/s) vs d
+ +
+
+
+ - Each dot is one entry. +
+
+
+ +
+
+ Bulk add (paste) + One entry per line: v,t,d or v t d +
+
+ +
+ + +
+
+ - Lines starting with # are ignored. + - You can add notes after a |, e.g. + 12.3,0.42,7.8 | angle=30. +
+
+
+
+
+ + + + diff --git a/src/backend/deploy.py b/src/backend/deploy.py index d9f53b1..68203f4 100644 --- a/src/backend/deploy.py +++ b/src/backend/deploy.py @@ -23,7 +23,6 @@ def pi_name_to_process_types(pi_names: list[str]) -> dict[str, list[ProcessType] return ( ProcessPlan[ProcessType]() .add(ProcessType.POS_EXTRAPOLATOR) - .pin(ProcessType.APRIL_SERVER, "nathan-hale") .pin(ProcessType.APRIL_SERVER, "tynan") .pin(ProcessType.APRIL_SERVER, "agatha-king") .assign(pi_names) @@ -52,30 +51,35 @@ def get_modules() -> list[_Module]: extra_run_args=[], equivalent_run_definition=ProcessType.APRIL_SERVER.get_name(), ), - ModuleTypes.PythonModule( - local_root_folder_path="python/fan_color", - local_main_file_path="main.py", - extra_run_args=[], - equivalent_run_definition=ProcessType.FAN_COLOR.get_name(), - ), + ] + + +if __name__ == "__main__": + DeploymentOptions.with_automatic_discovery(get_modules(), pi_name_to_process_types) + + +""" ModuleTypes.CPPLibraryModule( name="cuda-tags-lib", project_root_folder_path="cpp/CudaTags", build_for_platforms=[], # SystemType.JETPACK_L4T_R36_2 compilation_config=CPPBuildConfig.with_cmake( - clean_build_dir=False, cmake_args=[ "-DCUDATAGS_BUILD_PYTHON=ON", + "-DCMAKE_BUILD_TYPE=Release", + "-DPYBIND11_FINDPYTHON=ON", + "-DPYBIND11_PYTHON_VERSION=3.12", + "-DPython_EXECUTABLE=/usr/bin/python3.12", + "-DPython3_EXECUTABLE=/usr/bin/python3.12", + "-DPYTHON_EXECUTABLE=/usr/bin/python3.12", + "-DPython3_FIND_STRATEGY=LOCATION", ], compiler_args=[], libs=[ CPPLibrary(name="python3"), - CPPLibrary(name="python3-dev"), CPPLibrary(name="python-is-python3"), CPPLibrary(name="python3-numpy"), CPPLibrary(name="python3-pip"), - CPPLibrary(name="python3-distutils"), - CPPLibrary(name="pybind11-dev"), CPPLibrary(name="libopencv-dev"), CPPLibrary(name="openjdk-11-jdk"), CPPLibrary(name="default-jdk"), @@ -86,11 +90,26 @@ def get_modules() -> list[_Module]: CPPLibrary(name="build-essential"), CPPLibrary(name="libssl-dev"), ], - extra_docker_commands=[], + extra_docker_commands=[ + "apt-get update", + "apt-get install -y software-properties-common", + "add-apt-repository -y ppa:deadsnakes/ppa", + "apt-get update", + "apt-get install -y python3.12 python3.12-dev python3.12-venv", + "curl -sS https://bootstrap.pypa.io/get-pip.py | python3.12", + "python3.12 -m pip install --no-cache-dir numpy", + "git clone --depth 1 --branch v2.12.0 https://github.com/pybind/pybind11.git /tmp/pybind11", + "cmake -S /tmp/pybind11 -B /tmp/pybind11/build -DPYBIND11_TEST=OFF -DCMAKE_BUILD_TYPE=Release", + "cmake --build /tmp/pybind11/build -j$(nproc)", + "cmake --install /tmp/pybind11/build", + ], ), ), - ] - -if __name__ == "__main__": - DeploymentOptions.with_automatic_discovery(get_modules(), pi_name_to_process_types) + ModuleTypes.PythonModule( + local_root_folder_path="python/fan_color", + local_main_file_path="main.py", + extra_run_args=[], + equivalent_run_definition=ProcessType.FAN_COLOR.get_name(), + ), + """ diff --git a/src/backend/deployment/compilation/cpp/Dockerfile b/src/backend/deployment/compilation/cpp/Dockerfile index f7e61f8..b6f5a89 100644 --- a/src/backend/deployment/compilation/cpp/Dockerfile +++ b/src/backend/deployment/compilation/cpp/Dockerfile @@ -11,8 +11,8 @@ RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone & ARG CPPLIBRARIES ARG EXTRA_DOCKER_COMMANDS="echo No EXTRA_DOCKER_COMMANDS provided" -RUN /bin/bash -c "${CPPLIBRARIES}" -RUN /bin/bash -c "${EXTRA_DOCKER_COMMANDS}" +RUN /bin/bash -c "apt-get update && ${CPPLIBRARIES}" +RUN /bin/bash -c "apt-get update && ${EXTRA_DOCKER_COMMANDS}" WORKDIR /work diff --git a/src/backend/deployment/util.py b/src/backend/deployment/util.py index a45c9a7..fcda2a4 100644 --- a/src/backend/deployment/util.py +++ b/src/backend/deployment/util.py @@ -24,6 +24,7 @@ SERVICE = _pi_api.SERVICE DISCOVERY_TIMEOUT = _pi_api.DISCOVERY_TIMEOUT BACKEND_DEPLOYMENT_PATH = "/opt/blitz/B.L.I.T.Z/backend" +DEFAULT_SSH_USER = "ubuntu" GITIGNORE_PATH = ".gitignore" VENV_PATH = ".venv/bin/python" LOCAL_BINARIES_PATH = "build/release/" @@ -103,7 +104,7 @@ def _deploy_backend_to_pi( "StrictHostKeyChecking=no", "-p", str(pi.ssh_port), - f"ubuntu@{pi.address}", + f"{DEFAULT_SSH_USER}@{pi.address}", f"mkdir -p {remote_target_dir}", ] @@ -113,7 +114,7 @@ def _deploy_backend_to_pi( f"Failed to create remote directory {remote_target_dir} on {pi.address}: {mkdir_proc.returncode}" ) - target = f"ubuntu@{pi.address}:{remote_target_dir}" + target = f"{DEFAULT_SSH_USER}@{pi.address}:{remote_target_dir}" rsync_cmd = [ "sshpass", @@ -155,7 +156,7 @@ def _deploy_binaries(pi: _RaspberryPi, local_binaries_path: str): "ssh", "-p", str(pi.ssh_port), - f"ubuntu@{pi.address}", + f"{DEFAULT_SSH_USER}@{pi.address}", f"rm -rf {remote_full_path}", ] @@ -173,7 +174,7 @@ def _deploy_binaries(pi: _RaspberryPi, local_binaries_path: str): "ssh", "-p", str(pi.ssh_port), - f"ubuntu@{pi.address}", + f"{DEFAULT_SSH_USER}@{pi.address}", f"mkdir -p {remote_full_path}", ] @@ -195,7 +196,7 @@ def _deploy_binaries(pi: _RaspberryPi, local_binaries_path: str): "-e", f"ssh -p {getattr(pi, 'port', 22)} -o StrictHostKeyChecking=no", local_binaries_path, - f"ubuntu@{pi.address}:{remote_full_path}", + f"{DEFAULT_SSH_USER}@{pi.address}:{remote_full_path}", ] rsync_proc = subprocess.run(rsync_cmd) @@ -256,7 +257,7 @@ def _deploy_on_pi( "StrictHostKeyChecking=no", "-p", str(pi.ssh_port), - f"ubuntu@{pi.address}", + f"{DEFAULT_SSH_USER}@{pi.address}", f"echo {pi.password} | sudo -S systemctl restart startup.service", ] diff --git a/src/backend/python/april/src/__tests__/test_camera.py b/src/backend/python/april/src/__tests__/test_camera.py index 06ef066..a8e29b7 100644 --- a/src/backend/python/april/src/__tests__/test_camera.py +++ b/src/backend/python/april/src/__tests__/test_camera.py @@ -1,11 +1,18 @@ +import os from numpy.typing import NDArray from backend.python.common.camera.type_camera.OV2311_camera import OV2311Camera from backend.python.common.util.system import SystemStatus, get_system_status import numpy as np +def _should_run_camera_test() -> bool: + if get_system_status() == SystemStatus.SIMULATION: + return False + return os.path.exists("/dev/video0") + + def test_camera_open(): - if get_system_status() != SystemStatus.DEVELOPMENT: + if not _should_run_camera_test(): return # hardware-only video_capture = OV2311Camera( @@ -33,7 +40,7 @@ def calculate_avg_pixel_value(frame: NDArray[np.uint8]) -> float: def test_camera_exposure_time(): - if get_system_status() != SystemStatus.DEVELOPMENT: + if not _should_run_camera_test(): return # hardware-only video_capture = OV2311Camera( diff --git a/src/backend/python/common/__tests__/test_math.py b/src/backend/python/common/__tests__/test_math.py index 5cd1bed..0dc8f95 100644 --- a/src/backend/python/common/__tests__/test_math.py +++ b/src/backend/python/common/__tests__/test_math.py @@ -4,7 +4,7 @@ get_np_from_matrix, get_np_from_vector, get_robot_in_world, - transform_matrix_to_size, + _transform_matrix_to_size, transform_matrix_to_size_square, transform_vector_to_size, ) @@ -55,14 +55,14 @@ def test_get_robot_in_world(): def test_transform_matrix_to_size_square(): matrix = np.eye(6) used_diagonals = [True, True, True, True, True, True] - transformed_matrix = transform_matrix_to_size(used_diagonals, matrix) + transformed_matrix = _transform_matrix_to_size(used_diagonals, matrix) assert np.allclose(transformed_matrix, matrix) def test_transform_matrix_to_size_nonsq(): matrix = np.eye(6) used_diagonals = [True, True, False, False, False, False] - transformed_matrix = transform_matrix_to_size(used_diagonals, matrix) + transformed_matrix = _transform_matrix_to_size(used_diagonals, matrix) assert np.allclose(transformed_matrix[0, 0], 1) assert np.allclose(transformed_matrix[1, 1], 1) assert transformed_matrix.shape[0] == 2 diff --git a/src/backend/python/common/__tests__/test_replay.py b/src/backend/python/common/__tests__/test_replay.py index 3eb2b12..6ca1068 100644 --- a/src/backend/python/common/__tests__/test_replay.py +++ b/src/backend/python/common/__tests__/test_replay.py @@ -158,12 +158,11 @@ def test_record_output_uses_current_time_each_call(tmp_path, monkeypatch): recorder.record_output("a", b"x") recorder.record_output("b", b"y") + recorder.close() rows = list(replay_recorder.ReplayDB.select().order_by(replay_recorder.ReplayDB.id)) assert [row.timestamp for row in rows] == [100.0, 101.25] - recorder.close() - def test_record_output_encodes_supported_types(tmp_path): db_path = tmp_path / "types.db" diff --git a/src/backend/python/common/debug/replay_recorder.py b/src/backend/python/common/debug/replay_recorder.py index f1439c3..0749991 100644 --- a/src/backend/python/common/debug/replay_recorder.py +++ b/src/backend/python/common/debug/replay_recorder.py @@ -4,6 +4,7 @@ from typing import Literal, Union import cv2 import numpy as np +import threading from backend.python.common.camera.image_utils import encode_image from backend.python.common.debug.logger import error, success, warning @@ -90,9 +91,26 @@ def close(self): close_database() -class Recorder(iPod): +class Recorder(iPod, threading.Thread): def __init__(self, path: str): - super().__init__(path, "w") + iPod.__init__(self, path, "w") + threading.Thread.__init__(self, daemon=True) + + self._buffer: list[dict[str, str | float | bytes]] = [] + self._buffer_lock = threading.Lock() + self._last_flush_time = time_module.time() + self._batch_size = 128 + self._flush_interval_s = 0.2 + self._stop_event = threading.Event() + self._is_closed = False + self.start() + + def run(self): + while not self._stop_event.is_set(): + self.flush() + self._stop_event.wait(self._flush_interval_s) + + self.flush() def record_output(self, key: str, data: T): if isinstance(data, np.ndarray): @@ -129,9 +147,39 @@ def _record_bytes(self, key: str, data: bytes): self.write(key, "bytes", data) def write(self, key: str, data_type: str, data: bytes, time: float | None = None): - if time is None: - time = time_module.time() - ReplayDB.create(key=key, timestamp=time, data_type=data_type, data=data) + timestamp = time if time is not None else time_module.time() + with self._buffer_lock: + self._buffer.append( + { + "key": key, + "timestamp": timestamp, + "data_type": data_type, + "data": data, + } + ) + + def flush(self): + rows: list[dict[str, str | float | bytes]] = [] + with self._buffer_lock: + if not self._buffer: + return + rows = self._buffer + self._buffer = [] + + db = ReplayDB._meta.database # type: ignore + with db.atomic(): + ReplayDB.insert_many(rows).execute() + + def close(self): + if self._is_closed: + return + + self._is_closed = True + self._stop_event.set() + if self.is_alive(): + self.join() + self.flush() + super().close() class Player(iPod): @@ -249,6 +297,10 @@ def get_next_key_replay(key: str) -> Replay | None: def record_output(key: str, data: T): global GLOBAL_INSTANCE + + if isinstance(GLOBAL_INSTANCE, Player): + return + if GLOBAL_INSTANCE is None: error("Replay recorder not initialized or in write mode") raise RuntimeError("Replay recorder not initialized or in write mode") diff --git a/src/backend/python/common/util/math.py b/src/backend/python/common/util/math.py index 6ec93fe..c2d99cf 100644 --- a/src/backend/python/common/util/math.py +++ b/src/backend/python/common/util/math.py @@ -1,8 +1,11 @@ -from typing import Optional, cast +from typing import cast import numpy as np from numpy.typing import NDArray -from backend.generated.thrift.config.common.ttypes import GenericVector, GenericMatrix +from backend.generated.thrift.config.common.ttypes import ( + GenericVector, + GenericMatrix, +) def get_translation_rotation_components( @@ -19,8 +22,8 @@ def normalize_vector(vector: NDArray[np.float64]) -> NDArray[np.float64]: def make_transformation_matrix_p_d( *, - position: NDArray[np.float64], - direction_vector: NDArray[np.float64], + position: NDArray[np.float64] = np.array([0, 0, 0]), + direction_vector: NDArray[np.float64] = np.array([1, 0, 0]), z_axis: NDArray[np.float64] = np.array([0, 0, 1]), ) -> NDArray[np.float64]: x_axis = normalize_vector(direction_vector) @@ -130,7 +133,7 @@ def get_np_from_matrix( return np.array(matrix.values) -def transform_matrix_to_size( +def _transform_matrix_to_size( used_diagonals: list[bool], matrix: NDArray[np.float64] = np.eye(6), ) -> NDArray[np.float64]: @@ -138,6 +141,14 @@ def transform_matrix_to_size( return matrix[indices, :] +def transform_matrix_to_size( + matrix: NDArray[np.float64], + used_diagonals: list[bool], +) -> NDArray[np.float64]: + indices = [i for i, used in enumerate(used_diagonals) if used] + return matrix[indices, :] + + def transform_matrix_to_size_square( used_diagonals: list[bool], matrix: NDArray[np.float64] = np.eye(6), diff --git a/src/backend/python/common/util/system.py b/src/backend/python/common/util/system.py index 0039d6b..31a4578 100644 --- a/src/backend/python/common/util/system.py +++ b/src/backend/python/common/util/system.py @@ -17,6 +17,7 @@ from backend.generated.thrift.config.ttypes import Config import importlib import importlib.util +import importlib.machinery self_name: None | str = None @@ -58,8 +59,6 @@ class BasicSystemConfig(BaseModel): class SystemStatus(Enum): PRODUCTION = "production" - DEVELOPMENT_LOCAL = "development_local" - DEVELOPMENT = "development_remote" SIMULATION = "simulation" @@ -146,31 +145,68 @@ def load_configs() -> tuple[BasicSystemConfig, Config]: def get_glibc_version() -> str: """ - Returns the system's glibc version as a string, e.g., "2.35". + Returns the system's glibc version string, e.g., "2.35-0ubuntu3.8" + Strips any extraneous parentheses or trailing characters such as ')'. + In the special case of ldd (Ubuntu GLIBC 2.35-0ubuntu3.8) 2.35, + will return just "2.35". """ + import re + try: - # Parse output from ldd --version (first line, after 'ldd (GNU libc) X.Y[.Z]') output = subprocess.check_output( ["ldd", "--version"], encoding="utf-8", errors="ignore" ) - for line in output.splitlines(): - if "GNU libc" in line or "GLIBC" in line or "GLIBC" in line: - parts = line.strip().split() - for part in parts: - if part[0].isdigit(): - return part - if line.strip() and line.strip()[0].isdigit(): - vers_part = line.strip().split()[0] - if vers_part[0].isdigit(): - return vers_part - # Fallback: try to find a digit group in first line - first_line = output.splitlines()[0] - for s in first_line.split(): - if s[0].isdigit(): - return s + lines = output.splitlines() + + # Preferred pattern: match ldd (...) + for line in lines: + # Pattern 1: ldd (Ubuntu GLIBC 2.35-0ubuntu3.8) 2.35 + # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ^^^^ + m = re.match(r"^ldd\s+\((.*?)\)\s+([0-9\.]+)", line) + if m: + # group(2) is the version after the paren + return m.group(2) + + # Pattern 2: 'GLIBC 2.35-0ubuntu3.8' + m2 = re.search( + r"(?:GLIBC|GNU libc)[^\d]*([0-9]+(?:\.[0-9]+)*(?:-[\w\.]+)?)", line + ) + if m2: + # Only keep the pure version number: + # If m2.group(1) looks like '2.35-0ubuntu3.8', try to extract the major.minor part + version = m2.group(1) + # Extract first digit dot digit pattern + core = re.match(r"^([0-9]+\.[0-9]+)", version) + if core: + return core.group(1) + return version + + # Pattern 3: fallback paren group with version inside + m3 = re.search(r"\(([^)]*\d[^)]*)\)", line) + if m3: + inner = m3.group(1) + for piece in inner.split(): + # Find the first piece that looks like a version + core = re.match(r"^([0-9]+\.[0-9]+)", piece) + if core: + return core.group(1) + if any(ch.isdigit() for ch in piece): + return piece.rstrip(")") + # If nothing else, just return the whole group + return inner.rstrip(")") + + # Final fallback: scan all words in first line for digit-dot-digit pattern + if lines: + for word in lines[0].split(): + core = re.match(r"^([0-9]+\.[0-9]+)", word) + if core: + return core.group(1) + if any(ch.isdigit() for ch in word): + return word.rstrip(")") + except Exception: pass - # As a fallback, try to load from libc.so version string + # Try libc.so.6 version as last resort try: import ctypes @@ -198,37 +234,107 @@ def setup_shared_library_python_extension( *, module_name: str, py_lib_searchpath: str, - module_basename: str | None = None, + module_basename: str, ) -> ModuleType: binary_path = get_local_binary_path() + print(f"[Loader] binary_path: {binary_path}") - module_basename = module_basename if module_basename else module_name + search_path = os.path.join(binary_path, str(py_lib_searchpath)) + dir_path = search_path + explicit_extension_file: str | None = None - module_parent = str( - os.path.dirname(os.path.join(binary_path, str(py_lib_searchpath))) - ) - if module_parent not in sys.path: - sys.path.insert(0, module_parent) + # `py_lib_searchpath` is typically a directory like "cpp/cuda-tags-lib/". + # Keep that directory, do not strip to parent. + if os.path.isfile(search_path): + explicit_extension_file = search_path + dir_path = os.path.dirname(search_path) + + print(f"[Loader] module_parent: {dir_path}") + + if dir_path not in sys.path: + sys.path.insert(0, dir_path) + print(f"[Loader] Added '{dir_path}' to sys.path") + + # Ensure native dependency lookup includes the module folder. + current_ld_library_path = os.environ.get("LD_LIBRARY_PATH", "") + ld_entries = [p for p in current_ld_library_path.split(":") if p] + if dir_path not in ld_entries: + os.environ["LD_LIBRARY_PATH"] = ":".join([dir_path, *ld_entries]) + print(f"[Loader] Prepended '{dir_path}' to LD_LIBRARY_PATH") + + print(f"[Loader] module_search_path: {search_path}") - module_path = os.path.join(str(py_lib_searchpath), module_basename) extension_file: str | None = None - dir_path = os.path.dirname(module_path) - base_stem = os.path.basename(module_path) - if os.path.isdir(dir_path): + + print(f"[Loader] dir_path: {dir_path}, base_stem: {module_basename}") + + if explicit_extension_file is not None: + extension_file = explicit_extension_file + print(f"[Loader] Using explicit extension file: {extension_file}") + elif os.path.isdir(dir_path): + candidates: list[str] = [] + valid_suffixes = tuple(importlib.machinery.EXTENSION_SUFFIXES) for fname in os.listdir(dir_path): + print(f"[Loader] Candidate extension file: {fname}") if ( - fname.startswith(base_stem) + fname.startswith(module_basename) and (fname.endswith(".so") or fname.endswith(".pyd")) and os.path.isfile(os.path.join(dir_path, fname)) ): - extension_file = os.path.join(dir_path, fname) - break - + # Only accept extension suffixes compatible with the current interpreter ABI. + if fname.endswith(valid_suffixes): + candidates.append(os.path.join(dir_path, fname)) + else: + print( + f"[Loader] Skipping incompatible extension suffix for current Python: {fname}" + ) + + def suffix_rank(path: str) -> int: + # Prefer the most specific suffix for the running Python (e.g. cpython-312...). + name = os.path.basename(path) + for idx, suffix in enumerate(importlib.machinery.EXTENSION_SUFFIXES): + if name.endswith(suffix): + return idx + return len(importlib.machinery.EXTENSION_SUFFIXES) + + if candidates: + candidates.sort(key=suffix_rank) + extension_file = candidates[0] + print(f"[Loader] Selected extension_file: {extension_file}") + else: + print(f"[Loader] WARNING: Directory '{dir_path}' does not exist") + + print(f"[Loader] extension_file to import: {extension_file}") + if extension_file is None: + py_ver = f"{sys.version_info.major}.{sys.version_info.minor}" + raise ImportError( + f"Could not find compatible extension module '{module_basename}' in '{dir_path}' " + f"for Python {py_ver}. Compatible suffixes: {importlib.machinery.EXTENSION_SUFFIXES}" + ) spec = importlib.util.spec_from_file_location(module_name, extension_file) if spec is None or spec.loader is None: raise ImportError( f"Failed to create spec for {module_name} from {extension_file}" ) module = importlib.util.module_from_spec(spec) - spec.loader.exec_module(module) + try: + spec.loader.exec_module(module) + except ImportError as e: + ldd_missing_lines: list[str] = [] + try: + ldd_output = subprocess.check_output( + ["ldd", extension_file], encoding="utf-8", errors="ignore" + ) + for line in ldd_output.splitlines(): + if "not found" in line: + ldd_missing_lines.append(line.strip()) + except Exception: + pass + + if ldd_missing_lines: + raise ImportError( + f"{e}. Missing shared library deps for {extension_file}: " + + "; ".join(ldd_missing_lines) + ) from e + raise return module diff --git a/src/backend/python/pos_extrapolator/__tests__/test_april_tag_prep.py b/src/backend/python/pos_extrapolator/__tests__/test_april_tag_prep.py index b4399f2..6600ff8 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_april_tag_prep.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_april_tag_prep.py @@ -4,7 +4,6 @@ from backend.generated.thrift.config.pos_extrapolator.ttypes import ( AprilTagConfig, - TagDisambiguationMode, TagNoiseAdjustConfig, TagNoiseAdjustMode, TagUseImuRotation, @@ -15,37 +14,27 @@ ProcessedTag, WorldTags, ) -from backend.generated.thrift.config.common.ttypes import ( - GenericMatrix, - GenericVector, - Point3, -) +from backend.generated.thrift.config.common.ttypes import GenericMatrix, GenericVector, Point3 from backend.python.pos_extrapolator.data_prep import DataPreparer, ExtrapolationContext from backend.python.pos_extrapolator.position_extrapolator import PositionExtrapolator -from backend.python.pos_extrapolator.preparers import AprilTagPreparer from backend.python.pos_extrapolator.preparers.AprilTagPreparer import ( - AprilTagPreparerConfig, AprilTagDataPreparer, AprilTagDataPreparerConfig, + AprilTagPreparerConfig, ) -def from_theta_to_rotation_state(theta: float) -> NDArray[np.float64]: - return np.array([0, 0, 0, 0, np.cos(np.radians(theta)), np.sin(np.radians(theta))]) +class _FakeFilter: + def __init__(self, angle_rad: float = 0.0): + self._angle_rad = angle_rad + + def angle(self) -> NDArray[np.float64]: + return np.array([np.cos(self._angle_rad), np.sin(self._angle_rad)]) def from_np_to_point3( pose: NDArray[np.float64], rotation: NDArray[np.float64] ) -> Point3: - """ - Convert a pose and rotation from robot coordinates to camera coordinates. - The pose is a 3D vector in robot coordinates. - The rotation is a 3x3 matrix in robot coordinates. - """ - - # pose = from_robot_coords_to_camera_coords(pose) - # rotation = from_robot_rotation_to_camera_rotation(rotation) - return Point3( position=GenericVector(values=[pose[0], pose[1], pose[2]], size=3), rotation=GenericMatrix( @@ -115,10 +104,14 @@ def construct_tag_world(use_imu_rotation: bool = False) -> AprilTagPreparerConfi ) april_tag_config = AprilTagConfig( tag_position_config=tags_in_world, - tag_disambiguation_mode=TagDisambiguationMode.NONE, camera_position_config=cameras_in_robot, tag_use_imu_rotation=tag_use_imu_rotation, - disambiguation_time_window_s=0.1, + noise_change_modes=[], + tag_noise_adjust_config=TagNoiseAdjustConfig( + weight_per_m_from_distance_from_tag=0.0, + weight_per_degree_from_angle_error_tag=0.0, + weight_per_confidence_tag=0.0, + ), ) return AprilTagPreparerConfig( @@ -138,10 +131,10 @@ def make_april_tag_preparer( def make_noise_adjusted_preparer( - modes: list[TagNoiseAdjustMode], config: TagNoiseAdjustConfig + mode: TagNoiseAdjustMode, config: TagNoiseAdjustConfig ) -> DataPreparer[AprilTagData, AprilTagDataPreparerConfig]: base_config = construct_tag_world() - base_config.april_tag_config.tag_noise_adjust_mode = modes + base_config.april_tag_config.noise_change_modes = [mode] base_config.april_tag_config.tag_noise_adjust_config = config return AprilTagDataPreparer( # pyright: ignore[reportReturnType] AprilTagDataPreparerConfig(base_config) @@ -149,11 +142,6 @@ def make_noise_adjusted_preparer( def test_april_tag_prep_one(): - """ - Tests the AprilTagDataPreparer with a single tag. - The tag is at 0, 0 in the world and the camera is expected to be in the -1, 0 position. - """ - preparer = make_april_tag_preparer() tag_one_R = from_robot_rotation_to_camera_rotation(from_theta_to_3x3_mat(0)) @@ -171,15 +159,18 @@ def test_april_tag_prep_one(): ) ) - output = preparer.prepare_input(tag_vision_one, "camera_1") + output = preparer.prepare_input( + tag_vision_one, + "camera_1", + ExtrapolationContext(filter=_FakeFilter(), has_gotten_rotation=False), + ) assert output is not None - inputs = output.get_input_list() - assert len(inputs) == 1 - assert inputs[0].data.shape == (4,) - assert np.all(np.isfinite(inputs[0].data)) - # The tag is 1m in front of the camera, tag in world at origin -> robot at (-1, 0). - assert float(inputs[0].data[0]) == pytest.approx(-1.0, abs=1e-6) - assert float(inputs[0].data[1]) == pytest.approx(0.0, abs=1e-6) + assert len(output) == 1 + vals = output[0].get_input() + assert vals.shape == (3,) + assert np.all(np.isfinite(vals)) + assert float(vals[0]) == pytest.approx(-1.0, abs=1e-6) + assert float(vals[1]) == pytest.approx(0.0, abs=1e-6) def test_april_tag_prep_two(): @@ -205,45 +196,45 @@ def test_april_tag_prep_two(): output = preparer.prepare_input( tag_vision_one, "camera_1", - ExtrapolationContext( - x=np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ), + ExtrapolationContext(filter=_FakeFilter(), has_gotten_rotation=False), ) assert output is not None - assert len(output.get_input_list()) == 1 + assert len(output) == 1 def test_weight_add_config_distance_mode(): preparer = make_noise_adjusted_preparer( - [TagNoiseAdjustMode.ADD_WEIGHT_PER_M_DISTANCE_TAG], - TagNoiseAdjustConfig(weight_per_m_from_distance_from_tag=2.0), + TagNoiseAdjustMode.ADD_WEIGHT_PER_M_DISTANCE_TAG, + TagNoiseAdjustConfig( + weight_per_m_from_distance_from_tag=2.0, + weight_per_degree_from_angle_error_tag=0.0, + weight_per_confidence_tag=0.0, + ), ) - - multiplier, add = preparer.get_weight_add_config( - x_hat=np.array([0.0, 0.0, 1.0, 0.0]), - x=None, - distance_from_tag_m=3.0, - tag_confidence=0.0, + assert preparer.april_tag_config.noise_change_modes == [ + TagNoiseAdjustMode.ADD_WEIGHT_PER_M_DISTANCE_TAG + ] + assert preparer.april_tag_config.tag_noise_adjust_config is not None + assert ( + preparer.april_tag_config.tag_noise_adjust_config.weight_per_m_from_distance_from_tag + == pytest.approx(2.0) ) - assert multiplier == 1.0 - assert add == pytest.approx(6.0) - def test_weight_add_config_confidence_mode(): preparer = make_noise_adjusted_preparer( - [TagNoiseAdjustMode.ADD_WEIGHT_PER_TAG_CONFIDENCE], - TagNoiseAdjustConfig(weight_per_confidence_tag=4.0), + TagNoiseAdjustMode.ADD_WEIGHT_PER_TAG_CONFIDENCE, + TagNoiseAdjustConfig( + weight_per_m_from_distance_from_tag=0.0, + weight_per_degree_from_angle_error_tag=0.0, + weight_per_confidence_tag=4.0, + ), ) - - multiplier, add = preparer.get_weight_add_config( - x_hat=np.array([0.0, 0.0, 1.0, 0.0]), - x=None, - distance_from_tag_m=0.0, - tag_confidence=2.0, + assert preparer.april_tag_config.noise_change_modes == [ + TagNoiseAdjustMode.ADD_WEIGHT_PER_TAG_CONFIDENCE + ] + assert preparer.april_tag_config.tag_noise_adjust_config is not None + assert ( + preparer.april_tag_config.tag_noise_adjust_config.weight_per_confidence_tag + == pytest.approx(4.0) ) - - assert multiplier == 1.0 - assert add == pytest.approx(8.0) diff --git a/src/backend/python/pos_extrapolator/__tests__/test_data_prep.py b/src/backend/python/pos_extrapolator/__tests__/test_data_prep.py index 5e9560f..d021d7c 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_data_prep.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_data_prep.py @@ -7,16 +7,22 @@ OdomConfig, OdometryPositionSource, ) -from backend.python.pos_extrapolator.data_prep import ( - DataPreparerManager, - ExtrapolationContext, -) -from backend.python.pos_extrapolator.preparers.ImuDataPreparer import ( - ImuDataPreparerConfig, -) -from backend.python.pos_extrapolator.preparers.OdomDataPreparer import ( - OdomDataPreparerConfig, -) +from backend.python.pos_extrapolator.data_prep import DataPreparerManager, ExtrapolationContext +from backend.python.pos_extrapolator.preparers.ImuDataPreparer import ImuDataPreparerConfig +from backend.python.pos_extrapolator.preparers.OdomDataPreparer import OdomDataPreparerConfig + + +class _FakeFilter: + def __init__(self, angle_rad: float = 0.0): + self.x = np.array([0.0, 0.0, 0.0, 0.0, angle_rad, 0.0], dtype=np.float64) + + def angle_matrix(self) -> np.ndarray: + c = float(np.cos(self.x[4])) + s = float(np.sin(self.x[4])) + return np.array([[c, -s], [s, c]], dtype=np.float64) + + def get_state(self) -> np.ndarray: + return self.x.copy() def sample_imu_data(): @@ -33,8 +39,6 @@ def sample_imu_data(): imu_data.velocity.x = 10 imu_data.velocity.y = 11 imu_data.velocity.z = 12 - # angularVelocityXYZ.z is used when rotation is enabled; default is 0.0 - return imu_data @@ -46,21 +50,18 @@ def sample_odometry_data(): odometry_data.position.direction.y = 0.7 odometry_data.velocity.x = 15 odometry_data.velocity.y = 16 - # Populate position_change too so the test is meaningful under ABS_CHANGE configs. odometry_data.position_change.x = 13 odometry_data.position_change.y = 14 return odometry_data def test_data_prep(): - # Avoid config-file/schema coupling: define the minimal preparer configs inline. DataPreparerManager.set_config( ImuData, ImuDataPreparerConfig( { "0": ImuConfig( - use_rotation_absolute=True, - use_rotation_velocity=True, + use_rotation=True, use_position=False, use_velocity=True, ) @@ -80,53 +81,48 @@ def test_data_prep(): imu_data = sample_imu_data() odometry_data = sample_odometry_data() - # 7D state: [x, y, vx, vy, cos, sin, omega] - context_x = np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) - context_P = np.eye(7) + context = ExtrapolationContext(filter=_FakeFilter(), has_gotten_rotation=False) - imu_input = data_preparer_manager.prepare_data( - imu_data, - "0", - ExtrapolationContext(x=context_x, P=context_P, has_gotten_rotation=False), - ) - odometry_input = data_preparer_manager.prepare_data( - odometry_data, - "odom", - ExtrapolationContext(x=context_x, P=context_P, has_gotten_rotation=False), - ) + imu_input = data_preparer_manager.prepare_data(imu_data, "0", context) + odometry_input = data_preparer_manager.prepare_data(odometry_data, "odom", context) assert imu_input is not None and odometry_input is not None + assert len(imu_input) == 1 + assert len(odometry_input) == 1 + imu_vals = imu_input[0].get_input() assert np.allclose( - imu_input.get_input_list()[0].data, + imu_vals, np.array( [ imu_data.velocity.x, imu_data.velocity.y, - imu_data.position.direction.x, - imu_data.position.direction.y, + np.arctan2(imu_data.position.direction.y, imu_data.position.direction.x), imu_data.angularVelocityXYZ.z, ] ), ) - assert imu_input.sensor_id == "0" - assert imu_input.sensor_type == KalmanFilterSensorType.IMU + assert imu_input[0].sensor_id == "0" + assert imu_input[0].sensor_type == KalmanFilterSensorType.IMU + odom_vals = odometry_input[0].get_input() assert np.allclose( - odometry_input.get_input_list()[0].data, + odom_vals, np.array( [ odometry_data.position.position.x, odometry_data.position.position.y, odometry_data.velocity.x, odometry_data.velocity.y, - odometry_data.position.direction.x, - odometry_data.position.direction.y, + np.arctan2( + odometry_data.position.direction.y, + odometry_data.position.direction.x, + ), ] ), ) - assert odometry_input.sensor_id == "odom" - assert odometry_input.sensor_type == KalmanFilterSensorType.ODOMETRY + assert odometry_input[0].sensor_id == "odom" + assert odometry_input[0].sensor_type == KalmanFilterSensorType.ODOMETRY def test_get_config(): @@ -136,8 +132,7 @@ def test_get_config(): ImuDataPreparerConfig( { "0": ImuConfig( - use_rotation_absolute=True, - use_rotation_velocity=True, + use_rotation=True, use_position=False, use_velocity=True, ) @@ -146,11 +141,7 @@ def test_get_config(): ) imu = sample_imu_data() - ctx = ExtrapolationContext( - x=np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) + ctx = ExtrapolationContext(filter=_FakeFilter(), has_gotten_rotation=False) imu_input = preparer_manager.prepare_data(imu, "0", ctx) assert imu_input is not None diff --git a/src/backend/python/pos_extrapolator/__tests__/test_ekf.py b/src/backend/python/pos_extrapolator/__tests__/test_ekf.py index 1b334d2..eca2777 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_ekf.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_ekf.py @@ -1,13 +1,14 @@ import time from numpy.typing import NDArray +import pytest from backend.generated.thrift.config.kalman_filter.ttypes import KalmanFilterSensorType from backend.generated.thrift.config.kalman_filter.ttypes import ( KalmanFilterConfig, KalmanFilterSensorConfig, ) from backend.generated.thrift.config.common.ttypes import GenericMatrix, GenericVector -from backend.python.pos_extrapolator.data_prep import KalmanFilterInput, ProcessedData +from backend.python.pos_extrapolator.data_prep import KalmanFilterInput from backend.python.pos_extrapolator.filters.extended_kalman_filter import ( ExtendedKalmanFilterStrategy, ) @@ -19,11 +20,11 @@ def _eye(n: int) -> list[list[float]]: def make_test_kalman_filter_config() -> KalmanFilterConfig: - # 7D state: [x, y, vx, vy, cos, sin, angular_velocity_rad_s] - dim_x = 7 - dim_z = 5 # [vx, vy, cos, sin, omega] + # 6D state: [x, y, vx, vy, angle_rad, angular_velocity_rad_s] + dim_x = 6 + dim_z = 4 # [vx, vy, angle, omega] - state_vector = GenericVector(values=[0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], size=dim_x) + state_vector = GenericVector(values=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], size=dim_x) P = GenericMatrix(values=_eye(dim_x), rows=dim_x, cols=dim_x) Q = GenericMatrix( values=[[0.01 if i == j else 0.0 for j in range(dim_x)] for i in range(dim_x)], @@ -39,48 +40,46 @@ def make_test_kalman_filter_config() -> KalmanFilterConfig: } return KalmanFilterConfig( - state_vector=state_vector, + initial_state_vector=state_vector, uncertainty_matrix=P, process_noise_matrix=Q, sensors=sensors, - dim_x_z=[dim_x, dim_z], ) def sample_jacobian_h(_x: NDArray[np.float64]) -> NDArray[np.float64]: - # 7D state: [x, y, vx, vy, cos, sin, angular_velocity_rad_s] - # Measurement: [vx, vy, cos, sin, omega] - H = np.zeros((5, 7)) + # 6D state: [x, y, vx, vy, angle, omega] + # Measurement: [vx, vy, angle, omega] + H = np.zeros((4, 6)) H[0, 2] = 1 # vx H[1, 3] = 1 # vy - H[2, 4] = 1 # cos - H[3, 5] = 1 # sin - H[4, 6] = 1 # omega + H[2, 4] = 1 # angle + H[3, 5] = 1 # omega return H def sample_hx(x: NDArray[np.float64]) -> NDArray[np.float64]: - return x[[2, 3, 4, 5, 6]] # vx, vy, cos, sin, omega + return x[[2, 3, 4, 5]] # vx, vy, angle, omega def ekf_dataset_imu_input(): return [ KalmanFilterInput( - input=ProcessedData(data=np.array([1.0, 1.0, 1.0, 0.0, 0.0])), + input=np.array([1.0, 1.0, 0.0, 0.0]), sensor_id="0", sensor_type=KalmanFilterSensorType.IMU, jacobian_h=sample_jacobian_h, hx=sample_hx, ), KalmanFilterInput( - input=ProcessedData(data=np.array([1.0, 1.0, 1.0, 0.0, 0.0])), + input=np.array([1.0, 1.0, 0.0, 0.0]), sensor_id="0", sensor_type=KalmanFilterSensorType.IMU, jacobian_h=sample_jacobian_h, hx=sample_hx, ), KalmanFilterInput( - input=ProcessedData(data=np.array([1.0, 1.0, 1.0, 0.0, 0.0])), + input=np.array([1.0, 1.0, 0.0, 0.0]), sensor_id="0", sensor_type=KalmanFilterSensorType.IMU, jacobian_h=sample_jacobian_h, @@ -98,12 +97,17 @@ def test_ekf(): state = [float(v) for v in ekf.get_state().flatten().tolist()] print(state) - # Check that the state is close to expected values (accounting for noise) - # Logic: Start near [0,0,0,0,1,0,0], measure vx=1,vy=1,cos=1,sin=0,omega=0 and predict 1s each step. - expected = [3, 3, 1, 1, 1, 0, 0] - assert len(state) == len(expected) - for i, (actual, exp) in enumerate(zip(state, expected)): - assert abs(actual - exp) < 0.25, f"State[{i}]: expected {exp}, got {actual}" + # Behavior-level checks for the current EKF tuning: + # - symmetric x/y motion from symmetric measurements + # - positive position and velocity from repeated +1 velocity measurements + # - bounded velocity due to Kalman blending + assert len(state) == 6 + assert state[0] == pytest.approx(state[1], abs=1e-6) + assert state[2] == pytest.approx(state[3], abs=1e-6) + assert 1.5 < state[0] < 3.0 + assert 0.4 < state[2] < 1.1 + assert state[4] == pytest.approx(0.0, abs=1e-6) + assert state[5] == pytest.approx(0.0, abs=1e-6) def test_ekf_timing(): diff --git a/src/backend/python/pos_extrapolator/__tests__/test_ekf_extra.py b/src/backend/python/pos_extrapolator/__tests__/test_ekf_extra.py index f63b65e..da104ce 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_ekf_extra.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_ekf_extra.py @@ -9,10 +9,10 @@ KalmanFilterSensorConfig, KalmanFilterSensorType, ) -from backend.python.pos_extrapolator.data_prep import KalmanFilterInput, ProcessedData +from backend.python.pos_extrapolator.data_prep import KalmanFilterInput from backend.python.pos_extrapolator.filters.extended_kalman_filter import ( ExtendedKalmanFilterStrategy, - add_to_diagonal, + _add_to_diagonal, ) @@ -21,11 +21,11 @@ def _eye(n: int) -> list[list[float]]: def make_cfg(*, include_sensors: bool = True) -> KalmanFilterConfig: - # 7D state: [x, y, vx, vy, cos, sin, omega] - dim_x = 7 - dim_z = 5 + # 6D state: [x, y, vx, vy, angle, omega] + dim_x = 6 + dim_z = 4 - state_vector = GenericVector(values=[0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], size=dim_x) + state_vector = GenericVector(values=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], size=dim_x) P = GenericMatrix(values=_eye(dim_x), rows=dim_x, cols=dim_x) Q = GenericMatrix(values=_eye(dim_x), rows=dim_x, cols=dim_x) R = GenericMatrix(values=_eye(dim_z), rows=dim_z, cols=dim_z) @@ -39,11 +39,10 @@ def make_cfg(*, include_sensors: bool = True) -> KalmanFilterConfig: } return KalmanFilterConfig( - state_vector=state_vector, + initial_state_vector=state_vector, uncertainty_matrix=P, process_noise_matrix=Q, sensors=sensors, - dim_x_z=[dim_x, dim_z], ) @@ -51,7 +50,7 @@ def make_kfi( *, sensor_type: KalmanFilterSensorType, sensor_id: str ) -> KalmanFilterInput: return KalmanFilterInput( - input=ProcessedData(data=np.array([0.0, 0.0, 1.0, 0.0, 0.0])), + input=np.array([0.0, 0.0, 0.0, 0.0]), sensor_id=sensor_id, sensor_type=sensor_type, ) @@ -61,7 +60,7 @@ def test_get_R_sensors_mapping_contains_sensor_and_id(): ekf = ExtendedKalmanFilterStrategy(make_cfg(), fake_dt=0.1) assert KalmanFilterSensorType.IMU in ekf.R_sensors assert "imu0" in ekf.R_sensors[KalmanFilterSensorType.IMU] - assert ekf.R_sensors[KalmanFilterSensorType.IMU]["imu0"].shape == (5, 5) + assert ekf.R_sensors[KalmanFilterSensorType.IMU]["imu0"].shape == (4, 4) def test_insert_data_warns_and_skips_unknown_sensor_type(): @@ -102,30 +101,48 @@ def test_prediction_step_clamps_dt_when_negative_or_too_large(monkeypatch): assert ekf.F[1, 3] == pytest.approx(0.05) -def test_update_transformation_delta_t_sets_velocity_and_rotation_entries(): +def test_set_delta_t_sets_velocity_and_rotation_entries(): ekf = ExtendedKalmanFilterStrategy(make_cfg(), fake_dt=0.1) - ekf._debug_set_state(np.array([0.0, 0.0, 0.0, 0.0, 0.6, 0.8, 0.0])) - ekf._update_transformation_delta_t_with_size(0.2) + ekf._debug_set_state(np.array([0.0, 0.0, 0.0, 0.0, 0.6, 0.2])) + ekf.set_delta_t(0.2) assert ekf.F[0, 2] == pytest.approx(0.2) assert ekf.F[1, 3] == pytest.approx(0.2) - # Rotation coupling - assert ekf.F[4, 6] == pytest.approx(-0.8 * 0.2) - assert ekf.F[5, 6] == pytest.approx(0.6 * 0.2) + assert ekf.F[4, 5] == pytest.approx(0.2) -def test_get_confidence_returns_zero_for_nan_or_inf_covariance(): +def test_get_confidence_currently_returns_constant_one(): ekf = ExtendedKalmanFilterStrategy(make_cfg(), fake_dt=0.1) - ekf.P = np.eye(7) + ekf.P = np.eye(6) ekf.P[0, 0] = np.nan - assert ekf.get_confidence() == 0.0 + assert ekf.get_confidence() == 1.0 - ekf.P = np.eye(7) + ekf.P = np.eye(6) ekf.P[2, 2] = np.inf - assert ekf.get_confidence() == 0.0 + assert ekf.get_confidence() == 1.0 -@pytest.mark.xfail(reason="add_to_diagonal is currently unimplemented (pass)") def test_add_to_diagonal_adds_value_to_diagonal_entries(): m = np.zeros((3, 3), dtype=float) - add_to_diagonal(m, 2.5) + _add_to_diagonal(m, 2.5) assert np.allclose(np.diag(m), np.array([2.5, 2.5, 2.5])) + + +def test_get_state_future_predicts_from_current_filter_time(): + ekf = ExtendedKalmanFilterStrategy(make_cfg(), fake_dt=1.0) + ekf._debug_set_state(np.array([0.0, 0.0, 2.0, 0.0, 0.0, 0.0])) + + projected = ekf.get_state(future_s=2.0) + + # get_state() projects +2s from current state (x=4), then advances filter by fake_dt (x=2) + assert projected[0] == pytest.approx(4.0) + assert ekf.x[0] == pytest.approx(2.0) + + +def test_get_state_future_rotates_direction_with_angular_velocity(): + ekf = ExtendedKalmanFilterStrategy(make_cfg(), fake_dt=0.0) + ekf._debug_set_state(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])) + + projected = ekf.get_state(future_s=np.pi / 2) + + assert projected[4] == pytest.approx(np.pi / 2, abs=1e-6) + assert projected[5] == pytest.approx(1.0, abs=1e-6) diff --git a/src/backend/python/pos_extrapolator/__tests__/test_mahalanobis.py b/src/backend/python/pos_extrapolator/__tests__/test_mahalanobis.py index 6150652..cd79942 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_mahalanobis.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_mahalanobis.py @@ -1,7 +1,7 @@ import numpy as np import pytest -from backend.python.pos_extrapolator.filters.gate.mahalanobis import ( +from backend.python.pos_extrapolator.util.mahalanobis import ( mahalanobis_distance, percent_confidence, ) diff --git a/src/backend/python/pos_extrapolator/__tests__/test_pose_extrapolator.py b/src/backend/python/pos_extrapolator/__tests__/test_pose_extrapolator.py index 26677d7..ae69682 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_pose_extrapolator.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_pose_extrapolator.py @@ -10,19 +10,16 @@ from backend.generated.thrift.config.kalman_filter.ttypes import KalmanFilterSensorType from backend.generated.thrift.config.pos_extrapolator.ttypes import ( AprilTagConfig, + DataSources, ImuConfig, OdomConfig, OdometryPositionSource, PosExtrapolator, PosExtrapolatorMessageConfig, - TagDisambiguationMode, + TagNoiseAdjustConfig, TagUseImuRotation, ) -from backend.python.pos_extrapolator.data_prep import ( - ExtrapolationContext, - KalmanFilterInput, - ProcessedData, -) +from backend.python.pos_extrapolator.data_prep import ExtrapolationContext, KalmanFilterInput from backend.python.pos_extrapolator.position_extrapolator import PositionExtrapolator @@ -53,7 +50,7 @@ def get_confidence(self) -> float: @dataclass class FakeDataPreparerManager: - next_return: KalmanFilterInput | None = None + next_return: list[KalmanFilterInput] | None = None last_data: object | None = None last_sensor_id: str | None = None last_context: ExtrapolationContext | None = None @@ -61,7 +58,7 @@ class FakeDataPreparerManager: def prepare_data( self, data: object, sensor_id: str, context: ExtrapolationContext | None = None - ) -> KalmanFilterInput | None: + ) -> list[KalmanFilterInput] | None: self.calls += 1 self.last_data = data self.last_sensor_id = sensor_id @@ -86,10 +83,14 @@ def make_min_config( april_tag_config = AprilTagConfig( tag_position_config={}, - tag_disambiguation_mode=TagDisambiguationMode.NONE, camera_position_config={}, tag_use_imu_rotation=tag_use_imu_rotation, - disambiguation_time_window_s=0.1, + noise_change_modes=[], + tag_noise_adjust_config=TagNoiseAdjustConfig( + weight_per_m_from_distance_from_tag=0.0, + weight_per_degree_from_angle_error_tag=0.0, + weight_per_confidence_tag=0.0, + ), ) odom_config = OdomConfig( @@ -99,20 +100,15 @@ def make_min_config( imu_config = { imu_sensor_id: ImuConfig( - use_rotation_absolute=imu_use_rotation, - use_rotation_velocity=imu_use_rotation, + use_rotation=imu_use_rotation, use_position=False, use_velocity=True, ) } - # Note: we intentionally leave kalman_filter_config unset (None) because these unit - # tests use a fake filter strategy and never call validate(). return PosExtrapolator( message_config=message_config, - enable_imu=True, - enable_odom=True, - enable_tags=True, + enabled_data_sources=[DataSources.APRIL_TAG, DataSources.ODOMETRY, DataSources.IMU], april_tag_config=april_tag_config, odom_config=odom_config, imu_config=imu_config, @@ -125,7 +121,7 @@ def make_kfi( sensor_type: KalmanFilterSensorType, sensor_id: str = "s0" ) -> KalmanFilterInput: return KalmanFilterInput( - input=ProcessedData(data=np.array([0.0])), + input=np.array([0.0]), sensor_id=sensor_id, sensor_type=sensor_type, ) @@ -151,9 +147,9 @@ def make_subject( ) if x is None: - x = np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) + x = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) if p_matrix is None: - p_matrix = np.eye(7) + p_matrix = np.eye(6) fake_filter = FakeFilterStrategy(x=x, P=p_matrix, confidence=confidence) fake_manager = FakeDataPreparerManager() @@ -180,10 +176,7 @@ def test_initial_has_gotten_rotation_false_when_tags_use_imu_rotation(): def test_insert_sensor_data_passes_context_state_and_flag(): - x = np.array([1.0, 2.0, 3.0, 4.0, 0.5, 0.5, 0.0]) - pe, fake_filter, mgr = make_subject( - tag_use_imu_rotation=TagUseImuRotation.ALWAYS, x=x - ) + pe, fake_filter, mgr = make_subject(tag_use_imu_rotation=TagUseImuRotation.ALWAYS) assert pe.has_gotten_rotation is False mgr.next_return = None @@ -191,7 +184,7 @@ def test_insert_sensor_data_passes_context_state_and_flag(): assert mgr.calls == 1 assert mgr.last_context is not None - assert np.allclose(mgr.last_context.x, fake_filter.x) + assert mgr.last_context.filter is fake_filter assert mgr.last_context.has_gotten_rotation is False @@ -206,7 +199,7 @@ def test_april_tag_does_not_set_has_gotten_rotation(): pe, _, mgr = make_subject(tag_use_imu_rotation=TagUseImuRotation.ALWAYS) assert pe.has_gotten_rotation is False - mgr.next_return = make_kfi(KalmanFilterSensorType.APRIL_TAG, sensor_id="cam0") + mgr.next_return = [make_kfi(KalmanFilterSensorType.APRIL_TAG, sensor_id="cam0")] pe.insert_sensor_data(data=object(), sensor_id="cam0") assert pe.has_gotten_rotation is False @@ -216,7 +209,7 @@ def test_odom_sets_has_gotten_rotation_only_when_config_use_rotation_true(): tag_use_imu_rotation=TagUseImuRotation.ALWAYS, odom_use_rotation=False, ) - mgr1.next_return = make_kfi(KalmanFilterSensorType.ODOMETRY, sensor_id="odom") + mgr1.next_return = [make_kfi(KalmanFilterSensorType.ODOMETRY, sensor_id="odom")] pe1.insert_sensor_data(data=object(), sensor_id="odom") assert pe1.has_gotten_rotation is False @@ -224,7 +217,7 @@ def test_odom_sets_has_gotten_rotation_only_when_config_use_rotation_true(): tag_use_imu_rotation=TagUseImuRotation.ALWAYS, odom_use_rotation=True, ) - mgr2.next_return = make_kfi(KalmanFilterSensorType.ODOMETRY, sensor_id="odom") + mgr2.next_return = [make_kfi(KalmanFilterSensorType.ODOMETRY, sensor_id="odom")] pe2.insert_sensor_data(data=object(), sensor_id="odom") assert pe2.has_gotten_rotation is True @@ -235,7 +228,7 @@ def test_imu_sets_has_gotten_rotation_only_when_config_use_rotation_true(): imu_use_rotation=False, imu_sensor_id="imu0", ) - mgr1.next_return = make_kfi(KalmanFilterSensorType.IMU, sensor_id="imu0") + mgr1.next_return = [make_kfi(KalmanFilterSensorType.IMU, sensor_id="imu0")] pe1.insert_sensor_data(data=object(), sensor_id="imu0") assert pe1.has_gotten_rotation is False @@ -244,7 +237,7 @@ def test_imu_sets_has_gotten_rotation_only_when_config_use_rotation_true(): imu_use_rotation=True, imu_sensor_id="imu0", ) - mgr2.next_return = make_kfi(KalmanFilterSensorType.IMU, sensor_id="imu0") + mgr2.next_return = [make_kfi(KalmanFilterSensorType.IMU, sensor_id="imu0")] pe2.insert_sensor_data(data=object(), sensor_id="imu0") assert pe2.has_gotten_rotation is True @@ -253,19 +246,21 @@ def test_unknown_sensor_type_defaults_to_rotation_gotten_true(): pe, _, mgr = make_subject(tag_use_imu_rotation=TagUseImuRotation.ALWAYS) assert pe.has_gotten_rotation is False - mgr.next_return = KalmanFilterInput( - input=ProcessedData(data=np.array([0.0])), - sensor_id="s", - sensor_type=999, # pyright: ignore[reportArgumentType] - ) + mgr.next_return = [ + KalmanFilterInput( + input=np.array([0.0]), + sensor_id="s", + sensor_type=999, # pyright: ignore[reportArgumentType] + ) + ] pe.insert_sensor_data(data=object(), sensor_id="s") assert pe.has_gotten_rotation is True def test_get_robot_position_estimate_returns_flattened_list(): - x = np.array([1.0, 2.0, 3.0, 4.0, 0.25, 0.75, 0.5]) + x = np.array([1.0, 2.0, 3.0, 4.0, 0.25, 0.5]) pe, _, _ = make_subject(x=x) - assert pe.get_robot_position_estimate() == [1.0, 2.0, 3.0, 4.0, 0.25, 0.75, 0.5] + assert pe.get_robot_position_estimate() == [1.0, 2.0, 3.0, 4.0, 0.25, 0.5] def test_get_robot_position_estimate_passes_future_s_through(): @@ -275,8 +270,8 @@ def test_get_robot_position_estimate_passes_future_s_through(): def test_get_robot_position_maps_state_indices_to_proto_fields(): - x = np.array([10.0, 20.0, 1.1, -2.2, 0.6, 0.8, 0.05]) - P = np.eye(7) * 2.0 + x = np.array([10.0, 20.0, 1.1, -2.2, 0.6435, 0.05]) + P = np.eye(6) * 2.0 pe, fake_filter, _ = make_subject( x=x, p_matrix=P, @@ -291,19 +286,19 @@ def test_get_robot_position_maps_state_indices_to_proto_fields(): assert float(proto.position_2d.position.y) == pytest.approx(20.0) assert float(proto.position_2d.velocity.x) == pytest.approx(1.1) assert float(proto.position_2d.velocity.y) == pytest.approx(-2.2) - assert float(proto.position_2d.direction.x) == pytest.approx(0.6) - assert float(proto.position_2d.direction.y) == pytest.approx(0.8) + assert float(proto.position_2d.direction.x) == pytest.approx(np.cos(0.6435)) + assert float(proto.position_2d.direction.y) == pytest.approx(np.sin(0.6435)) assert float(proto.position_2d.rotation_speed_rad_s) == pytest.approx(0.05) assert float(proto.confidence) == pytest.approx(0.123) def test_get_position_covariance_flattens_matrix(): - P = np.arange(49, dtype=float).reshape(7, 7) + P = np.arange(36, dtype=float).reshape(6, 6) pe, _, _ = make_subject(p_matrix=P) flat = pe.get_position_covariance() - assert len(flat) == 49 + assert len(flat) == 36 assert flat[0] == 0.0 - assert flat[-1] == 48.0 + assert flat[-1] == 35.0 def test_get_confidence_is_forwarded(): diff --git a/src/backend/python/pos_extrapolator/__tests__/test_preparers_extra.py b/src/backend/python/pos_extrapolator/__tests__/test_preparers_extra.py index a04ea60..cdea14a 100644 --- a/src/backend/python/pos_extrapolator/__tests__/test_preparers_extra.py +++ b/src/backend/python/pos_extrapolator/__tests__/test_preparers_extra.py @@ -4,35 +4,47 @@ from backend.generated.proto.python.sensor.apriltags_pb2 import AprilTagData, WorldTags from backend.generated.proto.python.sensor.imu_pb2 import ImuData from backend.generated.proto.python.sensor.odometry_pb2 import OdometryData -from backend.generated.thrift.config.common.ttypes import ( - GenericMatrix, - GenericVector, - Point3, -) +from backend.generated.thrift.config.common.ttypes import GenericMatrix, GenericVector, Point3 from backend.generated.thrift.config.pos_extrapolator.ttypes import ( AprilTagConfig, ImuConfig, OdomConfig, OdometryPositionSource, - TagDisambiguationMode, + TagNoiseAdjustConfig, TagUseImuRotation, ) from backend.python.common.util.math import from_theta_to_3x3_mat -from backend.python.pos_extrapolator.data_prep import ( - DataPreparerManager, - ExtrapolationContext, -) +from backend.python.pos_extrapolator.data_prep import DataPreparerManager, ExtrapolationContext from backend.python.pos_extrapolator.preparers.AprilTagPreparer import ( AprilTagDataPreparer, AprilTagDataPreparerConfig, AprilTagPreparerConfig, ) -from backend.python.pos_extrapolator.preparers.ImuDataPreparer import ( - ImuDataPreparerConfig, -) -from backend.python.pos_extrapolator.preparers.OdomDataPreparer import ( - OdomDataPreparerConfig, -) +from backend.python.pos_extrapolator.preparers.ImuDataPreparer import ImuDataPreparerConfig +from backend.python.pos_extrapolator.preparers.OdomDataPreparer import OdomDataPreparerConfig + + +class _FakeFilter: + def __init__(self, x: np.ndarray | None = None): + self.x = x if x is not None else np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + + def angle_matrix(self) -> np.ndarray: + c = float(np.cos(self.x[4])) + s = float(np.sin(self.x[4])) + return np.array([[c, -s], [s, c]]) + + def get_state(self) -> np.ndarray: + return self.x.copy() + + def angle(self) -> np.ndarray: + return np.array([np.cos(self.x[4]), np.sin(self.x[4])]) + + +def _ctx(*, x: np.ndarray | None = None, has_gotten_rotation: bool = False) -> ExtrapolationContext: + return ExtrapolationContext( + filter=_FakeFilter(x), + has_gotten_rotation=has_gotten_rotation, + ) def _point3(p: np.ndarray, R: np.ndarray) -> Point3: @@ -50,6 +62,24 @@ def _point3(p: np.ndarray, R: np.ndarray) -> Point3: ) +def _april_cfg( + tags_in_world: dict[int, Point3], + cameras_in_robot: dict[str, Point3], + use_imu_rotation: TagUseImuRotation, +) -> AprilTagConfig: + return AprilTagConfig( + tag_position_config=tags_in_world, + camera_position_config=cameras_in_robot, + tag_use_imu_rotation=use_imu_rotation, + noise_change_modes=[], + tag_noise_adjust_config=TagNoiseAdjustConfig( + weight_per_m_from_distance_from_tag=0.0, + weight_per_degree_from_angle_error_tag=0.0, + weight_per_confidence_tag=0.0, + ), + ) + + def sample_imu() -> ImuData: imu = ImuData() imu.position.position.x = 1.0 @@ -81,9 +111,9 @@ def sample_odom() -> OdometryData: (False, False, False, 0), (True, False, False, 2), (False, True, False, 2), - (False, False, True, 3), # cos,sin + omega + (False, False, True, 2), (True, True, False, 4), - (True, True, True, 7), + (True, True, True, 6), ], ) def test_imu_preparer_value_selection_and_shapes( @@ -94,8 +124,7 @@ def test_imu_preparer_value_selection_and_shapes( ImuDataPreparerConfig( { "imu0": ImuConfig( - use_rotation_absolute=use_rotation, - use_rotation_velocity=use_rotation, + use_rotation=use_rotation, use_position=use_position, use_velocity=use_velocity, ) @@ -104,19 +133,15 @@ def test_imu_preparer_value_selection_and_shapes( ) mgr = DataPreparerManager() - ctx = ExtrapolationContext( - x=np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) + ctx = _ctx() out = mgr.prepare_data(sample_imu(), "imu0", ctx) assert out is not None - assert out.get_input_list()[0].data.shape == (expected_len,) + assert len(out) == 1 + assert out[0].get_input().shape == (expected_len,) - # Ensure jacobian/hx produce shapes consistent with selected indices. - x = ctx.x - H = out.jacobian_h(x) if out.jacobian_h is not None else None - hx = out.hx(x) if out.hx is not None else None + state = ctx.filter.get_state() + H = out[0].jacobian_h(state) if out[0].jacobian_h is not None else None + hx = out[0].hx(state) if out[0].hx is not None else None assert H is not None and hx is not None assert H.shape[0] == expected_len assert hx.shape == (expected_len,) @@ -128,8 +153,7 @@ def test_imu_preparer_missing_sensor_id_raises_keyerror(): ImuDataPreparerConfig( { "imu0": ImuConfig( - use_rotation_absolute=True, - use_rotation_velocity=True, + use_rotation=True, use_position=False, use_velocity=True, ) @@ -137,13 +161,8 @@ def test_imu_preparer_missing_sensor_id_raises_keyerror(): ), ) mgr = DataPreparerManager() - ctx = ExtrapolationContext( - x=np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) with pytest.raises(KeyError): - _ = mgr.prepare_data(sample_imu(), "missing", ctx) + _ = mgr.prepare_data(sample_imu(), "missing", _ctx()) def test_odom_preparer_absolute_includes_position_and_rotates_velocity(): @@ -156,17 +175,11 @@ def test_odom_preparer_absolute_includes_position_and_rotates_velocity(): ), ) mgr = DataPreparerManager() - # 90 deg rotation: cos=0,sin=1 rotates (vx,vy) -> (-vy, vx) - ctx = ExtrapolationContext( - x=np.array([0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) + ctx = _ctx(x=np.array([0.0, 0.0, 0.0, 0.0, np.pi / 2, 0.0])) out = mgr.prepare_data(sample_odom(), "odom", ctx) assert out is not None - vals = out.get_input_list()[0].data.tolist() - # ABSOLUTE: x,y then rotated vx,vy + vals = out[0].get_input().tolist() assert vals[0] == pytest.approx(10.0) assert vals[1] == pytest.approx(20.0) assert vals[2] == pytest.approx(-6.0) @@ -183,21 +196,16 @@ def test_odom_preparer_abs_change_updates_position_by_delta(): ), ) mgr = DataPreparerManager() - ctx = ExtrapolationContext( - x=np.array([100.0, 200.0, 0.0, 0.0, 1.0, 0.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) + ctx = _ctx(x=np.array([100.0, 200.0, 0.0, 0.0, 0.0, 0.0])) out = mgr.prepare_data(sample_odom(), "odom", ctx) assert out is not None - vals = out.get_input_list()[0].data.tolist() - # calc_next_absolute_position currently just adds deltas directly. + vals = out[0].get_input().tolist() assert vals[0] == pytest.approx(101.0) assert vals[1] == pytest.approx(202.0) @pytest.mark.xfail( - reason="OdomDataPreparer.calc_next_absolute_position ignores rotation_matrix; expected rotated delta in robot frame" + reason="OdomDataPreparer.calc_next_absolute_position currently applies raw delta without rotating into world frame" ) def test_odom_preparer_abs_change_should_rotate_position_delta(): DataPreparerManager.set_config( @@ -209,18 +217,13 @@ def test_odom_preparer_abs_change_should_rotate_position_delta(): ), ) mgr = DataPreparerManager() - # 90 deg rotation: (dx,dy) in robot frame should rotate to (-dy, dx) - ctx = ExtrapolationContext( - x=np.array([100.0, 200.0, 0.0, 0.0, 0.0, 1.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) + ctx = _ctx(x=np.array([100.0, 200.0, 0.0, 0.0, np.pi / 2, 0.0])) odom = sample_odom() odom.position_change.x = 1.0 odom.position_change.y = 2.0 out = mgr.prepare_data(odom, "odom", ctx) assert out is not None - vals = out.get_input_list()[0].data.tolist() + vals = out[0].get_input().tolist() assert vals[0] == pytest.approx(98.0) assert vals[1] == pytest.approx(201.0) @@ -231,13 +234,7 @@ def test_april_tag_preparer_raises_on_raw_tags(): "cam0": _point3(np.array([0.0, 0.0, 0.0]), from_theta_to_3x3_mat(0)) } - april_tag_cfg = AprilTagConfig( - tag_position_config=tags_in_world, - tag_disambiguation_mode=TagDisambiguationMode.NONE, - camera_position_config=cameras_in_robot, - tag_use_imu_rotation=TagUseImuRotation.NEVER, - disambiguation_time_window_s=0.1, - ) + april_tag_cfg = _april_cfg(tags_in_world, cameras_in_robot, TagUseImuRotation.NEVER) preparer = AprilTagDataPreparer( AprilTagDataPreparerConfig( AprilTagPreparerConfig( @@ -250,10 +247,9 @@ def test_april_tag_preparer_raises_on_raw_tags(): ) data = AprilTagData() - # Setting raw_tags activates the "raw_tags" oneof. data.raw_tags.corners.extend([]) with pytest.raises(ValueError): - _ = preparer.prepare_input(data, "cam0") + _ = preparer.prepare_input(data, "cam0", _ctx()) def test_april_tag_preparer_skips_unknown_tag_ids_and_returns_empty_input_list(): @@ -261,13 +257,7 @@ def test_april_tag_preparer_skips_unknown_tag_ids_and_returns_empty_input_list() cameras_in_robot = { "cam0": _point3(np.array([0.0, 0.0, 0.0]), from_theta_to_3x3_mat(0)) } - april_tag_cfg = AprilTagConfig( - tag_position_config=tags_in_world, - tag_disambiguation_mode=TagDisambiguationMode.NONE, - camera_position_config=cameras_in_robot, - tag_use_imu_rotation=TagUseImuRotation.NEVER, - disambiguation_time_window_s=0.1, - ) + april_tag_cfg = _april_cfg(tags_in_world, cameras_in_robot, TagUseImuRotation.NEVER) preparer = AprilTagDataPreparer( AprilTagDataPreparerConfig( AprilTagPreparerConfig( @@ -280,40 +270,30 @@ def test_april_tag_preparer_skips_unknown_tag_ids_and_returns_empty_input_list() ) data = AprilTagData(world_tags=WorldTags(tags=[])) - # No tags -> empty input list - out = preparer.prepare_input(data, "cam0") + out = preparer.prepare_input(data, "cam0", _ctx()) assert out is not None - assert out.get_input_list() == [] + assert out == [] @pytest.mark.xfail( - reason="AprilTagDataPreparer.should_use_imu_rotation logic appears inverted for UNTIL_FIRST_NON_TAG_ROTATION" + reason="AprilTagDataPreparer.should_use_imu_rotation appears inverted for WHILE_NO_OTHER_ROTATION_DATA" ) -def test_april_tag_preparer_until_first_non_tag_rotation_should_use_imu_before_non_tag_rotation(): +def test_april_tag_preparer_while_no_other_rotation_should_use_imu_before_rotation_data(): tags_in_world = {0: _point3(np.array([0.0, 0.0, 0.0]), from_theta_to_3x3_mat(0))} cameras_in_robot = { "cam0": _point3(np.array([0.0, 0.0, 0.0]), from_theta_to_3x3_mat(0)) } - april_tag_cfg = AprilTagConfig( - tag_position_config=tags_in_world, - tag_disambiguation_mode=TagDisambiguationMode.NONE, - camera_position_config=cameras_in_robot, - tag_use_imu_rotation=TagUseImuRotation.UNTIL_FIRST_NON_TAG_ROTATION, - disambiguation_time_window_s=0.1, + april_tag_cfg = _april_cfg( + tags_in_world, cameras_in_robot, TagUseImuRotation.WHILE_NO_OTHER_ROTATION_DATA ) preparer = AprilTagDataPreparer( AprilTagDataPreparerConfig( AprilTagPreparerConfig( tags_in_world=tags_in_world, cameras_in_robot=cameras_in_robot, - use_imu_rotation=TagUseImuRotation.UNTIL_FIRST_NON_TAG_ROTATION, + use_imu_rotation=TagUseImuRotation.WHILE_NO_OTHER_ROTATION_DATA, april_tag_config=april_tag_cfg, ) ) ) - ctx = ExtrapolationContext( - x=np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]), - P=np.eye(7), - has_gotten_rotation=False, - ) - assert preparer.should_use_imu_rotation(ctx) is True + assert preparer.should_use_imu_rotation(_ctx(has_gotten_rotation=False)) is True diff --git a/src/backend/python/pos_extrapolator/data_prep.py b/src/backend/python/pos_extrapolator/data_prep.py index 2adfcaa..d2d05c8 100644 --- a/src/backend/python/pos_extrapolator/data_prep.py +++ b/src/backend/python/pos_extrapolator/data_prep.py @@ -4,52 +4,108 @@ import numpy as np from numpy.typing import NDArray +from backend.generated.thrift.config.common.ttypes import GenericVector from backend.generated.thrift.config.kalman_filter.ttypes import KalmanFilterSensorType +from backend.python.pos_extrapolator.filter_strat import GenericFilterStrategy + T = TypeVar("T") C = TypeVar("C", covariant=True) -@dataclass -class ProcessedData: - data: NDArray[np.float64] - R_multipl: float = 1.0 - R_add: float = 0.0 - - @dataclass class KalmanFilterInput: - input: ProcessedData | list[ProcessedData] + input: NDArray[np.float64] | GenericVector sensor_id: str sensor_type: KalmanFilterSensorType + R_mult: float = 1.0 + R_add: float = 0.0 jacobian_h: Callable[[NDArray[np.float64]], NDArray[np.float64]] | None = None hx: Callable[[NDArray[np.float64]], NDArray[np.float64]] | None = None - def get_input_list(self) -> list[ProcessedData]: - if isinstance(self.input, list): - return list(self.input) - else: - return [self.input] + def get_input(self) -> NDArray[np.float64]: + if isinstance(self.input, GenericVector): + return np.array(self.input.values) + return self.input.copy() @dataclass class ExtrapolationContext: - x: NDArray[np.float64] - P: NDArray[np.float64] + """ + Context object for extrapolation. This class is inputted into the prepare_data method of the DataPreparerManager. + """ + + filter: GenericFilterStrategy has_gotten_rotation: bool +@dataclass class ConfigProvider(Protocol[C]): - def get_config(self) -> C: ... + """ + Protocol class for providing a configuration object to DataPreparers. + + Classes inheriting from ConfigProvider are expected to hold and provide + access to a configuration instance, which is used for controlling filter + input preparation and behavior. + """ + + config: C + + def __init__(self, config: C): + self.config = config + + def get_config(self) -> C: + return self.config class DataPreparer(Protocol[T, C]): def __init__(self, config: C | None = None): pass + def _prepare( + self, data: T, sensor_id: str, context: ExtrapolationContext | None = None + ) -> list[KalmanFilterInput] | KalmanFilterInput | None: + """ + Prepares the data for the filter. Returns a list of KalmanFilterInputs if the preparer returns a list, + a single KalmanFilterInput if the preparer returns a single input, or None if the preparer returns None. + """ + ... + def prepare_input( self, data: T, sensor_id: str, context: ExtrapolationContext | None = None - ) -> KalmanFilterInput | None: ... + ) -> list[KalmanFilterInput] | None: + """ + Prepares the data for the filter and automatically converts the result to a standardized list of KalmanFilterInputs. + """ + + result = self._prepare(data, sensor_id, context) + if result is None: + return None + if isinstance(result, KalmanFilterInput): + return [result] + return result + + def _used_indices(self, *args, **kwargs) -> list[bool]: + """ + Returns the used indices for the preparer. + Example: [True, True, True, True, True, True] for a preparer that uses all states. + """ + ... + + def get_used_indices(self, *args, **kwargs) -> list[bool]: + """ + Returns the used indices for the preparer, checking that the number of indices is + correct and matches the number of states in the filter (GenericFilterStrategy.kNumStates). + """ + + indices = self._used_indices(*args, **kwargs) + + if len(indices) != GenericFilterStrategy.kNumStates: + raise ValueError( + f"Expected {GenericFilterStrategy.kNumStates} indices, got {len(indices)}" + ) + + return indices def get_data_type(self) -> type[T]: ... @@ -60,6 +116,15 @@ class DataPreparerManager: @classmethod def register(cls, proto_type: Type[T], config_instance: Any = None): + """ + Register a DataPreparer class for a given proto_type. + + This allows the DataPreparerManager to know which DataPreparer should be used + for a specific protobuf data type. + + Optionally, an initial config_instance can be set for the corresponding proto_type. + """ + def decorator(preparer_class: Type[DataPreparer[T, Any]]): cls._registry[proto_type.__name__] = preparer_class if config_instance is not None: @@ -70,11 +135,33 @@ def decorator(preparer_class: Type[DataPreparer[T, Any]]): @classmethod def set_config(cls, proto_type: type[T], config_instance: Any): + """ + Sets the configuration instance associated with the given proto_type in the DataPreparerManager. + + This method is used to specify which configuration object should be used by preparers registered for a particular data type. + The configuration will be passed to preparer instances when preparing input data. + + Args: + proto_type: The protobuf type for which the configuration is being set. + config_instance: The configuration object to associate with the proto_type. + """ cls._config_instances[proto_type.__name__] = config_instance def prepare_data( self, data: object, sensor_id: str, context: ExtrapolationContext | None = None - ) -> KalmanFilterInput | None: + ) -> list[KalmanFilterInput] | None: + """ + Prepares the data for the filter. + + Parameters: + data: The data to prepare. This is a protobuf object (NOT bytes). + sensor_id: The id of the sensor that the data is from. + context: The context for the extrapolation. + + Returns: + A list of KalmanFilterInputs if the preparer returns a list, a single KalmanFilterInput if the preparer returns a single input, or None if the preparer returns None. + """ + data_type_name = type(data).__name__ if data_type_name not in self._registry: diff --git a/src/backend/python/pos_extrapolator/filter_strat.py b/src/backend/python/pos_extrapolator/filter_strat.py index 762e225..69e9115 100644 --- a/src/backend/python/pos_extrapolator/filter_strat.py +++ b/src/backend/python/pos_extrapolator/filter_strat.py @@ -1,15 +1,34 @@ -from dataclasses import dataclass import numpy as np from numpy.typing import NDArray +from typing import TYPE_CHECKING -from backend.python.pos_extrapolator.data_prep import ( - ExtrapolationContext, - KalmanFilterInput, -) +from backend.generated.proto.python.util.vector_pb2 import Vector2 + +if TYPE_CHECKING: + from backend.python.pos_extrapolator.data_prep import KalmanFilterInput class GenericFilterStrategy: - def insert_data(self, data: KalmanFilterInput) -> None: + """ + Abstract base class for filter strategies. + """ + + kPosXIdx = 0 + kPosYIdx = 1 + kVelXIdx = 2 + kVelYIdx = 3 + kAngleRadIdx = 4 + kAngleVelRadSIdx = 5 + + kNumStates = 6 + kNumOutputs = 6 + + kStandardDeviationsAwayThreshold = 5.0 + + def __init__(self, x: NDArray[np.float64]): + self.x = x + + def insert_data(self, data: "KalmanFilterInput") -> None: raise NotImplementedError("insert_data not implemented") def get_state(self, future_s: float | None = None) -> NDArray[np.float64]: @@ -24,3 +43,35 @@ def get_P(self) -> NDArray[np.float64]: def _debug_set_state(self, x: NDArray[np.float64]) -> None: pass + + def pose_2d(self) -> Vector2: + return Vector2(x=self.x[self.kPosXIdx], y=self.x[self.kPosYIdx]) + + def velocity_2d(self) -> Vector2: + return Vector2(x=self.x[self.kVelXIdx], y=self.x[self.kVelYIdx]) + + def angle_vector(self) -> Vector2: + return Vector2( + x=np.cos(self.x[self.kAngleRadIdx]), y=np.sin(self.x[self.kAngleRadIdx]) + ) + + def angle_rad(self) -> float: + return self.x[self.kAngleRadIdx] + + def angle_matrix(self) -> NDArray[np.float64]: + cos = np.cos(self.x[self.kAngleRadIdx]) + sin = np.sin(self.x[self.kAngleRadIdx]) + return np.array([[cos, -sin], [sin, cos]]) + + def angle(self) -> NDArray[np.float64]: + return np.array( + [np.cos(self.x[self.kAngleRadIdx]), np.sin(self.x[self.kAngleRadIdx])] + ) + + def direction_vector(self) -> Vector2: + return Vector2( + x=np.cos(self.x[self.kAngleRadIdx]), y=np.sin(self.x[self.kAngleRadIdx]) + ) + + def angular_velocity_rad(self) -> float: + return self.x[self.kAngleVelRadSIdx] diff --git a/src/backend/python/pos_extrapolator/filters/extended_kalman_filter.py b/src/backend/python/pos_extrapolator/filters/extended_kalman_filter.py index ac1c9b5..2cd9f3b 100644 --- a/src/backend/python/pos_extrapolator/filters/extended_kalman_filter.py +++ b/src/backend/python/pos_extrapolator/filters/extended_kalman_filter.py @@ -1,36 +1,97 @@ import time from typing import Any, Callable +from enum import Enum from filterpy.kalman import ExtendedKalmanFilter import numpy as np import warnings from numpy.typing import NDArray -from scipy.linalg import cho_factor, cho_solve -from scipy.stats import chi2 -from backend.python.common.util.math import get_np_from_matrix, get_np_from_vector +from backend.python.common.debug.logger import warning +from backend.python.common.util.math import ( + get_np_from_matrix, + get_np_from_vector, + transform_matrix_to_size, + transform_vector_to_size, +) from backend.generated.thrift.config.kalman_filter.ttypes import ( KalmanFilterConfig, KalmanFilterSensorType, ) from backend.python.pos_extrapolator.data_prep import ( - ExtrapolationContext, KalmanFilterInput, ) from backend.python.pos_extrapolator.filter_strat import GenericFilterStrategy -class ExtendedKalmanFilterStrategy( # pyright: ignore[reportUnsafeMultipleInheritance] - ExtendedKalmanFilter, GenericFilterStrategy -): # pyright: ignore[reportUnsafeMultipleInheritance] +def _wrap_to_pi(angle_rad: float) -> float: + return float(np.arctan2(np.sin(angle_rad), np.cos(angle_rad))) + + +def _add_to_diagonal(mat: NDArray[np.float64], num: float): + for i in range(min(mat.shape[0], mat.shape[1])): + mat[i, i] += num + + +def _residual_with_angle_wrap( + z: NDArray[np.float64], + h_x: NDArray[np.float64], + angle_measurement_idx: int | None, +) -> NDArray[np.float64]: + """ + Residual with angle wrap. This is used to wrap the angle residual to the range [-π, π]. + Residual definition: + A function that returns the difference between the measurement and the prediction. + Essentially a vector subtraction function specific to the filter. + + Args: + z: Measurement vector. + h_x: Prediction vector. + + Returns: + Residual vector with angle wrapped to the range [-π, π]. + + Reason needed: + The angle residual is not automatically wrapped to the range [-π, π] by the filter so it will bug out when + the angle goes outside of this range or changes from -pi to pi (0 angle difference read as huge change). + """ + + residual = np.subtract(z, h_x) + if angle_measurement_idx is None: + return residual + + if 0 <= angle_measurement_idx < len(residual): + residual[angle_measurement_idx] = _wrap_to_pi( + float(residual[angle_measurement_idx]) + ) + + return residual + + +class FilterStateType(Enum): + POS_X = GenericFilterStrategy.kPosXIdx + POS_Y = GenericFilterStrategy.kPosYIdx + VEL_X = GenericFilterStrategy.kVelXIdx + VEL_Y = GenericFilterStrategy.kVelYIdx + ANGLE_RAD = GenericFilterStrategy.kAngleRadIdx + ANGLE_VEL_RAD_S = GenericFilterStrategy.kAngleVelRadSIdx + + +class ExtendedKalmanFilterStrategy(ExtendedKalmanFilter, GenericFilterStrategy): def __init__( self, config: KalmanFilterConfig, fake_dt: float | None = None, ): - super().__init__(dim_x=config.dim_x_z[0], dim_z=config.dim_x_z[1]) - self.hw = config.dim_x_z[0] - self.x = get_np_from_vector(config.state_vector) + ExtendedKalmanFilter.__init__( + self, + dim_x=GenericFilterStrategy.kNumStates, + dim_z=GenericFilterStrategy.kNumOutputs, + ) + GenericFilterStrategy.__init__(self, x=self.x) + + self.hw = GenericFilterStrategy.kNumStates + self.x = get_np_from_vector(config.initial_state_vector) self.P = get_np_from_matrix(config.uncertainty_matrix) self.Q = get_np_from_matrix(config.process_noise_matrix) self.config = config @@ -53,14 +114,51 @@ def get_R_sensors( return output - def jacobian_h(self, x: NDArray[np.float64]) -> NDArray[np.float64]: - return np.eye(7) + @staticmethod + def generic_jacobian_h( + used_indices: list[bool], + ) -> Callable[[NDArray[np.float64]], NDArray[np.float64]]: + """ + Returns a function that returns the Jacobian of the measurement function. This is generalized for common preparation steps. + """ + return lambda _: transform_matrix_to_size( + np.eye(len(used_indices)), used_indices + ) + + @staticmethod + def generic_hx( + used_indices: list[bool], + ) -> Callable[[NDArray[np.float64]], NDArray[np.float64]]: + """ + Returns a function that returns the measurement function. This is generalized for common preparation steps. + """ + return lambda x: transform_vector_to_size(x, used_indices) def get_R(self) -> NDArray[np.float64]: return self.R - def hx(self, x: NDArray[np.float64]) -> NDArray[np.float64]: - return x + def _infer_measurement_idx( + self, + jacobian_h: Callable[[NDArray[np.float64]], NDArray[np.float64]], + state_type: FilterStateType, + ) -> int | None: + """ + Infer which measurement index maps to a given state component from H Jacobian. + Returns None if this measurement does not include the requested state. + """ + state_idx = state_type.value + H = jacobian_h(self.x) + if H.ndim != 2 or H.shape[1] <= state_idx: + return None + + state_col = np.abs(H[:, state_idx]) + if state_col.size == 0: + return None + + idx = int(np.argmax(state_col)) + if float(state_col[idx]) <= 1e-12: + return None + return idx def prediction_step(self): if self.fake_dt is not None: @@ -71,13 +169,12 @@ def prediction_step(self): if dt > 5 or dt < 0: dt = 0.05 - self._update_transformation_delta_t_with_size(dt) + self.set_delta_t(dt) self.predict() + self.last_update_time = time.time() def insert_data(self, data: KalmanFilterInput) -> None: - self.prediction_step() - if data.sensor_type not in self.R_sensors: warnings.warn( f"Sensor type {data.sensor_type} not found in R_sensors, skipping update" @@ -90,98 +187,99 @@ def insert_data(self, data: KalmanFilterInput) -> None: ) return - R_sensor = self.R_sensors[data.sensor_type][data.sensor_id] + self.prediction_step() - for datapoint in data.get_input_list(): - R = R_sensor.copy() * datapoint.R_multipl - add_to_diagonal(R, datapoint.R_add) + R_sensor = self.R_sensors[data.sensor_type][data.sensor_id] - self.update( - datapoint.data, - data.jacobian_h if data.jacobian_h is not None else self.jacobian_h, - data.hx if data.hx is not None else self.hx, - R=R, - ) + jacobian_h = ( + data.jacobian_h + if data.jacobian_h is not None + else self.generic_jacobian_h(GenericFilterStrategy.kNumStates * [True]) + ) + hx = ( + data.hx + if data.hx is not None + else self.generic_hx(GenericFilterStrategy.kNumStates * [True]) + ) + angle_measurement_idx = self._infer_measurement_idx( + jacobian_h, FilterStateType.ANGLE_RAD + ) + + R = R_sensor.copy() * data.R_mult + _add_to_diagonal(R, data.R_add) + + residual_fn = lambda z, h_x: _residual_with_angle_wrap( + z, h_x, angle_measurement_idx + ) + self.update( + data.get_input(), + jacobian_h, + hx, + R=R, + residual=residual_fn, + ) + + def get_standard_deviations_away( + self, + state: NDArray[np.float64], + state_types: list[FilterStateType] | FilterStateType, + ) -> float: + """Mahalanobis distance (number of std devs) of state from current estimate.""" + types = ( + [state_types] + if isinstance(state_types, FilterStateType) + else list(state_types) + ) + idx = np.array([t.value for t in types]) + r = np.asarray(state, dtype=np.float64).flatten()[idx] - self.x[idx] + P_sub = self.P[np.ix_(idx, idx)] + if len(idx) == 1: + var = float(P_sub[0, 0]) + return float("inf") if var <= 0 else float(np.abs(r[0]) / np.sqrt(var)) + try: + d_sq = float(r @ np.linalg.solve(P_sub, r)) + except np.linalg.LinAlgError: + return float("inf") + return float(np.sqrt(max(0.0, d_sq))) def get_P(self) -> NDArray[np.float64]: return self.P def predict_x_no_update(self, dt: float) -> NDArray[np.float64]: - self._update_transformation_delta_t_with_size(dt) + self.set_delta_t(dt) return np.dot(self.F, self.x) + np.dot(self.B, 0) def get_state(self, future_s: float | None = None) -> NDArray[np.float64]: - predicted_x: NDArray[np.float64] + predicted_x: NDArray[np.float64] = self.x if future_s is not None and future_s > 0: predicted_x = self.predict_x_no_update(future_s) - self.prediction_step() - else: - self.prediction_step() - predicted_x = self.x - return predicted_x + self.prediction_step() - def get_position_confidence(self) -> float: - P_position = self.P[:2, :2] - if np.any(np.isnan(P_position)) or np.any(np.isinf(P_position)): - return 0.0 - try: - eigenvalues: NDArray[np.float64] = np.real( - np.linalg.eigvals(P_position) - ) # pyright: ignore[reportAssignmentType] - max_eigen = np.max(eigenvalues) - if np.isnan(max_eigen) or np.isinf(max_eigen) or max_eigen < 0: - return 0.0 - return 1.0 / (1.0 + np.sqrt(max_eigen)) - except np.linalg.LinAlgError: - return 0.0 + return predicted_x - def get_velocity_confidence(self) -> float: - P_velocity = self.P[2:4, 2:4] - if np.any(np.isnan(P_velocity)) or np.any(np.isinf(P_velocity)): - return 0.0 - try: - eigenvalues = np.linalg.eigvals(P_velocity) - max_eigen = np.max(eigenvalues) - if np.isnan(max_eigen) or np.isinf(max_eigen) or max_eigen < 0: - return 0.0 - return 1.0 / (1.0 + np.sqrt(max_eigen)) - except np.linalg.LinAlgError: - return 0.0 + def get_confidence(self) -> float: + return 1.0 - def get_rotation_confidence(self) -> float: - angle_var = self.P[4, 4] + self.P[5, 5] - if np.isnan(angle_var) or np.isinf(angle_var) or angle_var < 0: - return 0.0 - angle_uncertainty = np.sqrt(angle_var) - return 1.0 / (1.0 + angle_uncertainty) + def set_delta_t(self, delta_t: float): + """ + Sets the delta_t in the F matrix (state transition matrix which is multiplied by the state to get the next state). + This is used because the delta_t is not constant for the filter. + """ - def get_confidence(self) -> float: - pos_conf = self.get_position_confidence() - vel_conf = self.get_velocity_confidence() - rot_conf = self.get_rotation_confidence() - if pos_conf == 0.0 or vel_conf == 0.0 or rot_conf == 0.0: - return 0.0 - return (pos_conf * vel_conf * rot_conf) ** (1 / 3) - - def _update_transformation_delta_t_with_size(self, new_delta_t: float): try: - vel_idx_x = 2 # vx is at index 2 in [x, y, vx, vy, cos, sin, angular_velocity_rad_s] - vel_idx_y = 3 # vy is at index 3 in [x, y, vx, vy, cos, sin, angular_velocity_rad_s] - cos_idx = 4 # cos is at index 4 - sin_idx = 5 # sin is at index 5 - angular_vel_idx = 6 # angular velocity is at index 6 in [x, y, vx, vy, cos, sin, angular_velocity_rad_s] - - # Update position based on velocity - self.F[0][vel_idx_x] = new_delta_t - self.F[1][vel_idx_y] = new_delta_t - - # Update rotation (cos/sin) based on angular velocity - # d(cos)/dt = -sin * omega, d(sin)/dt = cos * omega - self.F[cos_idx][angular_vel_idx] = -self.x[sin_idx] * new_delta_t - self.F[sin_idx][angular_vel_idx] = self.x[cos_idx] * new_delta_t + self.F[GenericFilterStrategy.kPosXIdx][ + GenericFilterStrategy.kVelXIdx + ] = delta_t # vx innovation + self.F[GenericFilterStrategy.kPosYIdx][ + GenericFilterStrategy.kVelYIdx + ] = delta_t # vy innovation + + self.F[GenericFilterStrategy.kAngleRadIdx][ + GenericFilterStrategy.kAngleVelRadSIdx + ] = delta_t # angular velocity innovation except IndexError as e: - warnings.warn(f"Error updating F matrix: {e}") + warnings.warn(f"Error setting delta_t in F matrix: {e}") def _debug_set_state(self, x: NDArray[np.float64]) -> None: self.x = x @@ -199,6 +297,4 @@ def update( super().update(z, HJacobian, Hx, R, args, hx_args, residual) -def add_to_diagonal(mat: NDArray[np.float64], num: float): - for i in range(min(mat.shape[0], mat.shape[1])): - mat[i, i] += num +T_EKF = ExtendedKalmanFilterStrategy # alias for the class diff --git a/src/backend/python/pos_extrapolator/filters/kalman_filter.py b/src/backend/python/pos_extrapolator/filters/kalman_filter.py index e469c64..08a19cc 100644 --- a/src/backend/python/pos_extrapolator/filters/kalman_filter.py +++ b/src/backend/python/pos_extrapolator/filters/kalman_filter.py @@ -85,3 +85,6 @@ def update_transformation_delta_t_with_size( self.F[1][size_matrix_h // 2] = new_delta_t self.F[0][0] = size_matrix_w self.F[1][1] = size_matrix_h + + +T_KF = KalmanFilterStrategy diff --git a/src/backend/python/pos_extrapolator/main.py b/src/backend/python/pos_extrapolator/main.py index fec3c7e..a64342f 100644 --- a/src/backend/python/pos_extrapolator/main.py +++ b/src/backend/python/pos_extrapolator/main.py @@ -1,146 +1,29 @@ # TODO: need to add a better way to handle the non-used indices in sensors (config method). -import argparse import asyncio -import time -from autobahn_client import Address, Autobahn -import numpy as np - -from backend.generated.proto.python.sensor.apriltags_pb2 import AprilTagData from backend.generated.proto.python.sensor.general_sensor_data_pb2 import ( GeneralSensorData, ) -from backend.generated.proto.python.sensor.imu_pb2 import ImuData -from backend.generated.proto.python.sensor.odometry_pb2 import OdometryData -from backend.generated.thrift.config.ttypes import Config -from backend.python.common.config import from_uncertainty_config from backend.python.common.debug.logger import ( - LogLevel, - debug, error, info, - init_logging, -) -from backend.python.common.debug.pubsub_replay import ReplayAutobahn -from backend.python.common.debug.replay_recorder import ( - init_replay_recorder, - record_output, ) from backend.python.common.util.extension import subscribe_to_multiple_topics -from backend.python.common.util.parser import get_default_process_parser -from backend.python.common.util.system import ( - BasicSystemConfig, - SystemStatus, - get_system_name, - get_system_status, - load_configs, -) from backend.python.pos_extrapolator.data_prep import DataPreparerManager from backend.python.pos_extrapolator.filters.extended_kalman_filter import ( ExtendedKalmanFilterStrategy, ) from backend.python.pos_extrapolator.position_extrapolator import PositionExtrapolator -from backend.python.pos_extrapolator.preparers.AprilTagPreparer import ( - AprilTagPreparerConfig, - AprilTagDataPreparerConfig, -) -from backend.python.pos_extrapolator.preparers.ImuDataPreparer import ( - ImuDataPreparerConfig, -) -from backend.python.pos_extrapolator.preparers.OdomDataPreparer import ( - OdomDataPreparerConfig, -) - - -def init_utilities( - config: Config, basic_system_config: BasicSystemConfig, autobahn_server: Autobahn -): - init_logging( - "POSE_EXTRAPOLATOR", - LogLevel(basic_system_config.logging.global_logging_level), - system_pub_topic=basic_system_config.logging.global_log_pub_topic, - autobahn=autobahn_server, - system_name=get_system_name(), - ) - - if get_system_status() == SystemStatus.SIMULATION: - init_replay_recorder( - process_name="pose_extrapolator", replay_path="latest", mode="r" - ) - else: - init_replay_recorder(process_name="pose_extrapolator", mode="w") - - -def get_autobahn_server(system_config: BasicSystemConfig): - address = Address(system_config.autobahn.host, system_config.autobahn.port) - autobahn_server = Autobahn(address) - - if get_system_status() == SystemStatus.SIMULATION: - autobahn_server = ReplayAutobahn( - replay_path="latest", - publish_on_real_autobahn=True, - address=address, - ) - - return autobahn_server - - -def init_data_preparer_manager(config: Config): - if config.pos_extrapolator.enable_imu: - DataPreparerManager.set_config( - ImuData, ImuDataPreparerConfig(config.pos_extrapolator.imu_config) - ) - - if config.pos_extrapolator.enable_odom: - DataPreparerManager.set_config( - OdometryData, OdomDataPreparerConfig(config.pos_extrapolator.odom_config) - ) - - if config.pos_extrapolator.enable_tags: - DataPreparerManager.set_config( - AprilTagData, - AprilTagDataPreparerConfig( - config=AprilTagPreparerConfig( - tags_in_world=config.pos_extrapolator.april_tag_config.tag_position_config, - cameras_in_robot=config.pos_extrapolator.april_tag_config.camera_position_config, - use_imu_rotation=config.pos_extrapolator.april_tag_config.tag_use_imu_rotation, - april_tag_config=config.pos_extrapolator.april_tag_config, - ), - ), - ) - - -def get_subscribe_topics(config: Config): - subscribe_topics: list[str] = [] - if config.pos_extrapolator.enable_imu: - subscribe_topics.append( - config.pos_extrapolator.message_config.post_imu_input_topic - ) - if config.pos_extrapolator.enable_odom: - subscribe_topics.append( - config.pos_extrapolator.message_config.post_odometry_input_topic - ) - if config.pos_extrapolator.enable_tags: - subscribe_topics.append( - config.pos_extrapolator.message_config.post_tag_input_topic - ) - - return subscribe_topics +from backend.python.pos_extrapolator.util.init_stuff import main_init_phase async def main(): - system_config, config = load_configs() + _, config, autobahn_server, subscribe_topics = main_init_phase() - autobahn_server = get_autobahn_server(system_config) - init_utilities(config, system_config, autobahn_server) info(f"Starting Position Extrapolator...") await autobahn_server.begin() - init_data_preparer_manager(config) - - subscribe_topics = get_subscribe_topics(config) - position_extrapolator = PositionExtrapolator( config.pos_extrapolator, ExtendedKalmanFilterStrategy(config.pos_extrapolator.kalman_filter_config), @@ -160,12 +43,6 @@ async def process_data(message: bytes): f"Something went wrong when inserting data into Position Extrapolator: {e}" ) - if ( - hasattr(config.pos_extrapolator, "composite_publish_topic") - and config.pos_extrapolator.composite_publish_topic is not None - ): - subscribe_topics.append(config.pos_extrapolator.composite_publish_topic) - await subscribe_to_multiple_topics( autobahn_server, subscribe_topics, @@ -182,11 +59,6 @@ async def process_data(message: bytes): proto_position.SerializeToString(), ) - record_output( - config.pos_extrapolator.message_config.post_robot_position_output_topic, - proto_position, - ) - await asyncio.sleep( config.pos_extrapolator.time_s_between_position_sends if config.pos_extrapolator.time_s_between_position_sends diff --git a/src/backend/python/pos_extrapolator/position_extrapolator.py b/src/backend/python/pos_extrapolator/position_extrapolator.py index cbbe92c..b1259e4 100644 --- a/src/backend/python/pos_extrapolator/position_extrapolator.py +++ b/src/backend/python/pos_extrapolator/position_extrapolator.py @@ -1,11 +1,7 @@ import time import numpy as np -from backend.python.common.debug.logger import info from backend.generated.proto.python.util.position_pb2 import RobotPosition -from backend.generated.proto.python.util.vector_pb2 import Vector2, Vector3 -from backend.generated.thrift.config.camera.ttypes import CameraParameters -from backend.generated.thrift.config.common.ttypes import Point3 from backend.generated.thrift.config.kalman_filter.ttypes import KalmanFilterSensorType from backend.generated.thrift.config.pos_extrapolator.ttypes import ( PosExtrapolator, @@ -43,8 +39,7 @@ def __init__( def insert_sensor_data(self, data: object, sensor_id: str) -> None: context = ExtrapolationContext( - x=self.filter_strategy.get_state(), - P=self.filter_strategy.get_P(), + filter=self.filter_strategy, has_gotten_rotation=self.has_gotten_rotation, ) @@ -55,10 +50,10 @@ def insert_sensor_data(self, data: object, sensor_id: str) -> None: if prepared_data is None: return - if self.is_rotation_gotten(prepared_data.sensor_type, sensor_id): - self.has_gotten_rotation = True - - self.filter_strategy.insert_data(prepared_data) + for datapoint in prepared_data: + if self.is_rotation_gotten(datapoint.sensor_type, sensor_id): + self.has_gotten_rotation = True + self.filter_strategy.insert_data(datapoint) def is_rotation_gotten( self, sensor_type: KalmanFilterSensorType, sensor_id: str @@ -70,8 +65,7 @@ def is_rotation_gotten( return self.config.odom_config.use_rotation if sensor_type == KalmanFilterSensorType.IMU: - imu_cfg = self.config.imu_config[sensor_id] - return imu_cfg.use_rotation_absolute or imu_cfg.use_rotation_velocity + return self.config.imu_config[sensor_id].use_rotation return True @@ -89,9 +83,9 @@ def get_robot_position(self) -> RobotPosition: proto_position.position_2d.position.y = filtered_position[1] proto_position.position_2d.velocity.x = filtered_position[2] proto_position.position_2d.velocity.y = filtered_position[3] - proto_position.position_2d.direction.x = filtered_position[4] - proto_position.position_2d.direction.y = filtered_position[5] - proto_position.position_2d.rotation_speed_rad_s = filtered_position[6] + proto_position.position_2d.direction.x = np.cos(filtered_position[4]) + proto_position.position_2d.direction.y = np.sin(filtered_position[4]) + proto_position.position_2d.rotation_speed_rad_s = filtered_position[5] proto_position.P.extend(self.get_position_covariance()) diff --git a/src/backend/python/pos_extrapolator/preparers/AprilTagPreparer.py b/src/backend/python/pos_extrapolator/preparers/AprilTagPreparer.py index a71332f..773d1d8 100644 --- a/src/backend/python/pos_extrapolator/preparers/AprilTagPreparer.py +++ b/src/backend/python/pos_extrapolator/preparers/AprilTagPreparer.py @@ -1,8 +1,8 @@ from dataclasses import dataclass import math +from typing import TYPE_CHECKING import numpy as np from numpy.typing import NDArray -from backend.python.common.debug.logger import debug from backend.python.common.util.math import ( create_transformation_matrix, from_float_list, @@ -10,21 +10,16 @@ get_np_from_vector, get_robot_in_world, get_translation_rotation_components, - make_3d_rotation_from_yaw, make_transformation_matrix_p_d, - transform_matrix_to_size, - transform_vector_to_size, ) -from backend.generated.proto.python.sensor.apriltags_pb2 import AprilTagData -from backend.generated.proto.python.sensor.imu_pb2 import ImuData -from backend.generated.proto.python.sensor.odometry_pb2 import OdometryData +from backend.generated.proto.python.sensor.apriltags_pb2 import ( + AprilTagData, + ProcessedTag, +) from backend.generated.thrift.config.common.ttypes import Point3 from backend.generated.thrift.config.kalman_filter.ttypes import KalmanFilterSensorType from backend.generated.thrift.config.pos_extrapolator.ttypes import ( AprilTagConfig, - ImuConfig, - OdomConfig, - TagNoiseAdjustConfig, TagNoiseAdjustMode, TagUseImuRotation, ) @@ -34,27 +29,15 @@ DataPreparerManager, ExtrapolationContext, KalmanFilterInput, - ProcessedData, -) -from backend.python.pos_extrapolator.filters.gate.mahalanobis import ( - mahalanobis_distance, ) +from backend.python.pos_extrapolator.filters.extended_kalman_filter import T_EKF from backend.python.pos_extrapolator.position_extrapolator import PositionExtrapolator +# from typing import override -def _angle_difference_deg(x_hat: NDArray[np.float64], x: NDArray[np.float64]) -> float: - measured_angle = math.atan2(float(x_hat[3]), float(x_hat[2])) - state_angle = math.atan2(float(x[5]), float(x[4])) - diff = measured_angle - state_angle - while diff > math.pi: - diff -= 2 * math.pi - while diff < -math.pi: - diff += 2 * math.pi - return abs(math.degrees(diff)) - - -def _distance_difference_m(x_hat: NDArray[np.float64], x: NDArray[np.float64]) -> float: - return np.sqrt((x_hat[0] - x[0]) ** 2 + (x_hat[1] - x[1]) ** 2) +# rotation_angle_rad = np.atan2( <- correct rotation theta angle +# render_direction_vector[1] /*y*/, render_direction_vector[0] /*x*/ +# ) @dataclass @@ -66,11 +49,7 @@ class AprilTagPreparerConfig: class AprilTagDataPreparerConfig(ConfigProvider[AprilTagPreparerConfig]): - def __init__(self, config: AprilTagPreparerConfig): - self.config = config - - def get_config(self) -> AprilTagPreparerConfig: - return self.config + pass @DataPreparerManager.register(proto_type=AprilTagData) @@ -86,97 +65,71 @@ def __init__(self, config: AprilTagDataPreparerConfig): self.use_imu_rotation: TagUseImuRotation = conf.use_imu_rotation self.april_tag_config: AprilTagConfig = conf.april_tag_config - self.tag_noise_adjust_mode = self.april_tag_config.tag_noise_adjust_mode - - self.tag_noise_adjust_config: TagNoiseAdjustConfig = ( - self.april_tag_config.tag_noise_adjust_config - ) - - def get_data_type(self) -> type[AprilTagData]: - return AprilTagData - - def get_used_indices(self) -> list[bool]: - used_indices: list[bool] = [] + self.noise_change_modes = self.april_tag_config.noise_change_modes - used_indices.extend([True] * 2) - used_indices.extend([False] * 2) - used_indices.extend([True] * 2) - used_indices.extend([False]) - - return used_indices - - def jacobian_h(self, x: NDArray[np.float64]) -> NDArray[np.float64]: - return transform_matrix_to_size(self.get_used_indices(), np.eye(7)) - - def hx(self, x: NDArray[np.float64]) -> NDArray[np.float64]: - return transform_vector_to_size(x, self.get_used_indices()) - - def should_use_imu_rotation(self, context: ExtrapolationContext | None) -> bool: - if context is None: + def should_use_imu_rotation(self, context: ExtrapolationContext) -> bool: + if self.use_imu_rotation == TagUseImuRotation.NEVER: return False if self.use_imu_rotation == TagUseImuRotation.ALWAYS: return True - if self.use_imu_rotation == TagUseImuRotation.UNTIL_FIRST_NON_TAG_ROTATION: + if self.use_imu_rotation == TagUseImuRotation.WHILE_NO_OTHER_ROTATION_DATA: return context.has_gotten_rotation return False - def get_weight_add_config( - self, - *, - x_hat: NDArray[np.float64], - x: NDArray[np.float64] | None, - distance_from_tag_m: float, - tag_confidence: float, + def get_noise_change( + self, x_hat: NDArray[np.float64], tag_detection: ProcessedTag ) -> tuple[float, float]: - add = 0.0 - multiplier = 1.0 + total_add = 0.0 + total_mult = 1.0 + + if TagNoiseAdjustMode.ADD_WEIGHT_PER_M_DISTANCE_TAG in self.noise_change_modes: + distance = np.linalg.norm(x_hat[0:2]) + weight = ( + self.april_tag_config.tag_noise_adjust_config.weight_per_m_from_distance_from_tag + * distance + ) - for mode in self.tag_noise_adjust_mode: - if mode == TagNoiseAdjustMode.ADD_WEIGHT_PER_M_DISTANCE_TAG: - weight = ( - self.tag_noise_adjust_config.weight_per_m_from_distance_from_tag - ) - if weight is not None: - add += distance_from_tag_m * weight + total_add += float(weight) + if TagNoiseAdjustMode.ADD_WEIGHT_PER_TAG_CONFIDENCE in self.noise_change_modes: + total_add += float( + tag_detection.confidence + * self.april_tag_config.tag_noise_adjust_config.weight_per_confidence_tag + ) - elif mode == TagNoiseAdjustMode.ADD_WEIGHT_PER_DEGREE_ERROR_ANGLE_TAG: - weight = ( - self.tag_noise_adjust_config.weight_per_degree_from_angle_error_tag - ) - if weight is not None and x is not None: - add += _angle_difference_deg(x_hat, x) * weight - - elif mode == TagNoiseAdjustMode.ADD_WEIGHT_PER_TAG_CONFIDENCE: - weight = self.tag_noise_adjust_config.weight_per_confidence_tag - if ( - weight is not None - and tag_confidence is not None - and np.isfinite(tag_confidence) - ): - confidence_term = max(0.0, float(tag_confidence)) - add += confidence_term * weight - elif mode == TagNoiseAdjustMode.MULTIPLY_POW_BY_M_DISTANCE_FROM_TAG: - multiplier += ( - distance_from_tag_m - ** self.tag_noise_adjust_config.pow_distance_from_tag_coef - * self.tag_noise_adjust_config.multiply_coef_m_distance_from_tag - ) + return float(total_add), float(total_mult) + + # @override + def get_data_type(self) -> type[AprilTagData]: + return AprilTagData + + # @override + def _used_indices(self) -> list[bool]: + used_indices: list[bool] = [] - return multiplier, add + used_indices.extend([True] * 2) + used_indices.extend([False] * 2) + used_indices.extend([True]) + used_indices.extend([False]) + + return used_indices - def prepare_input( + # @override + def _prepare( self, data: AprilTagData, sensor_id: str, context: ExtrapolationContext | None = None, - ) -> KalmanFilterInput | None: + ) -> list[KalmanFilterInput] | KalmanFilterInput | None: + assert context is not None if data.WhichOneof("data") == "raw_tags": - raise ValueError("Tags are not in processed format") + raise ValueError( + "Tried to insert AprilTagData with raw tags, but tags are not in processed format" + ) - input_list: list[ProcessedData] = [] + input_list: list[KalmanFilterInput] = [] for tag in data.world_tags.tags: tag_id = tag.id if tag_id not in self.tags_in_world: @@ -215,13 +168,13 @@ def prepare_input( R_robot_rotation_world: NDArray[np.float64] | None = None if self.should_use_imu_rotation(context): - assert context is not None + direction_2d = context.filter.angle() + direction_3d = np.array([direction_2d[0], direction_2d[1], 0.0]) R_robot_rotation_world = make_transformation_matrix_p_d( - position=np.array([0, 0, 0]), - direction_vector=np.array([context.x[4], context.x[5], 0]), + direction_vector=direction_3d, )[:3, :3] - render_pose, render_rotation = get_translation_rotation_components( + pose, rotation = get_translation_rotation_components( get_robot_in_world( T_tag_in_camera=T_tag_in_camera, T_camera_in_robot=T_camera_in_robot, @@ -230,35 +183,39 @@ def prepare_input( ) ) - render_direction_vector = render_rotation[0:3, 0] - # rotation_angle_rad = np.atan2( <- correct rotation theta angle - # render_direction_vector[1] /*y*/, render_direction_vector[0] /*x*/ - # ) + if self.april_tag_config.insert_predicted_global_rotation: + _, rotation_pred = get_translation_rotation_components( + get_robot_in_world( + T_tag_in_camera=T_tag_in_camera, + T_camera_in_robot=T_camera_in_robot, + T_tag_in_world=T_tag_in_world, + ) + ) + + rotation = rotation_pred + direction_vector = rotation[0:3, 0] # extract cos and sin + angle_rad = np.atan2(direction_vector[1], direction_vector[0]) datapoint = np.array( [ - render_pose[0], - render_pose[1], - render_direction_vector[0], - render_direction_vector[1], + pose[0], + pose[1], + angle_rad, ] ) - multiplier, add = self.get_weight_add_config( - x_hat=datapoint, - x=context.x if context is not None else None, - distance_from_tag_m=float(np.linalg.norm(tag_in_camera_pose)), - tag_confidence=tag.confidence, - ) + add, mult = self.get_noise_change(datapoint, tag) input_list.append( - ProcessedData(data=datapoint, R_multipl=multiplier, R_add=add) + KalmanFilterInput( + input=datapoint, + sensor_id=sensor_id, + sensor_type=KalmanFilterSensorType.APRIL_TAG, + jacobian_h=T_EKF.generic_jacobian_h(self.get_used_indices()), + hx=T_EKF.generic_hx(self.get_used_indices()), + R_add=add, + R_mult=mult, + ) ) - return KalmanFilterInput( - input=input_list, - sensor_id=sensor_id, - sensor_type=KalmanFilterSensorType.APRIL_TAG, - jacobian_h=self.jacobian_h, - hx=self.hx, - ) + return input_list diff --git a/src/backend/python/pos_extrapolator/preparers/ImuDataPreparer.py b/src/backend/python/pos_extrapolator/preparers/ImuDataPreparer.py index 57f2682..ffeb667 100644 --- a/src/backend/python/pos_extrapolator/preparers/ImuDataPreparer.py +++ b/src/backend/python/pos_extrapolator/preparers/ImuDataPreparer.py @@ -1,8 +1,8 @@ -from typing import final +# from typing import final, overload, override import numpy as np from numpy.typing import NDArray from backend.python.common.util.math import ( - transform_matrix_to_size, + _transform_matrix_to_size, transform_vector_to_size, ) from backend.generated.proto.python.sensor.imu_pb2 import ImuData @@ -14,16 +14,12 @@ DataPreparerManager, ExtrapolationContext, KalmanFilterInput, - ProcessedData, ) +from backend.python.pos_extrapolator.filters.extended_kalman_filter import T_EKF class ImuDataPreparerConfig(ConfigProvider[dict[str, ImuConfig]]): - def __init__(self, config: dict[str, ImuConfig]): - self.config = config - - def get_config(self) -> dict[str, ImuConfig]: - return self.config + pass @DataPreparerManager.register(proto_type=ImuData) @@ -32,27 +28,23 @@ def __init__(self, config: ImuDataPreparerConfig): super().__init__(config) self.config: ImuDataPreparerConfig = config + # @override def get_data_type(self) -> type[ImuData]: return ImuData - def get_used_indices(self, sensor_id: str) -> list[bool]: + # @override + def _used_indices(self, sensor_id: str) -> list[bool]: used_indices: list[bool] = [] used_indices.extend([self.config.config[sensor_id].use_position] * 2) used_indices.extend([self.config.config[sensor_id].use_velocity] * 2) - used_indices.extend([self.config.config[sensor_id].use_rotation_absolute] * 2) - # Include angular velocity (last state index) when rotation is used - used_indices.extend([self.config.config[sensor_id].use_rotation_velocity]) + used_indices.extend([self.config.config[sensor_id].use_rotation] * 2) return used_indices - def jacobian_h(self, x: NDArray[np.float64], sensor_id: str) -> NDArray[np.float64]: - return transform_matrix_to_size(self.get_used_indices(sensor_id), np.eye(7)) - - def hx(self, x: NDArray[np.float64], sensor_id: str) -> NDArray[np.float64]: - return transform_vector_to_size(x, self.get_used_indices(sensor_id)) - - def prepare_input( + # @override + def _prepare( self, data: ImuData, sensor_id: str, context: ExtrapolationContext | None = None - ) -> KalmanFilterInput | None: + ) -> list[KalmanFilterInput] | KalmanFilterInput | None: + assert context is not None config = self.config.config[sensor_id] values: list[float] = [] @@ -60,20 +52,21 @@ def prepare_input( values.append(data.position.position.x) values.append(data.position.position.y) if config.use_velocity: - values.append(data.velocity.x) - values.append(data.velocity.y) - if config.use_rotation_absolute: - values.append(data.position.direction.x) - values.append(data.position.direction.y) - - if config.use_rotation_velocity: - # Use yaw rate (Z axis) in radians per second + velocity = context.filter.angle_matrix() @ np.array( + [data.velocity.x, data.velocity.y] + ) + values.append(velocity[0]) + values.append(velocity[1]) + if config.use_rotation: + values.append( + np.atan2(data.position.direction.y, data.position.direction.x) + ) values.append(data.angularVelocityXYZ.z) return KalmanFilterInput( - input=ProcessedData(data=np.array(values)), + input=np.array(values), sensor_id=sensor_id, sensor_type=KalmanFilterSensorType.IMU, - jacobian_h=lambda x: self.jacobian_h(x, sensor_id), - hx=lambda x: self.hx(x, sensor_id), + jacobian_h=T_EKF.generic_jacobian_h(self.get_used_indices(sensor_id)), + hx=T_EKF.generic_hx(self.get_used_indices(sensor_id)), ) diff --git a/src/backend/python/pos_extrapolator/preparers/OdomDataPreparer.py b/src/backend/python/pos_extrapolator/preparers/OdomDataPreparer.py index cd2b6d9..1bbca86 100644 --- a/src/backend/python/pos_extrapolator/preparers/OdomDataPreparer.py +++ b/src/backend/python/pos_extrapolator/preparers/OdomDataPreparer.py @@ -1,15 +1,10 @@ import math +from typing import TYPE_CHECKING import numpy as np from numpy.typing import NDArray -from backend.python.common.util.math import ( - transform_matrix_to_size, - transform_vector_to_size, -) -from backend.generated.proto.python.sensor.imu_pb2 import ImuData from backend.generated.proto.python.sensor.odometry_pb2 import OdometryData from backend.generated.thrift.config.kalman_filter.ttypes import KalmanFilterSensorType from backend.generated.thrift.config.pos_extrapolator.ttypes import ( - ImuConfig, OdomConfig, OdometryPositionSource, ) @@ -19,19 +14,15 @@ DataPreparerManager, ExtrapolationContext, KalmanFilterInput, - ProcessedData, ) +from backend.python.pos_extrapolator.filters.extended_kalman_filter import T_EKF -class OdomDataPreparerConfig(ConfigProvider[OdomConfig]): - def __init__(self, config: OdomConfig): - self.config = config - - def get_config(self) -> OdomConfig: - return self.config +# from typing import override -SHOULD_USE_ROTATION_MATRIX = True +class OdomDataPreparerConfig(ConfigProvider[OdomConfig]): + pass @DataPreparerManager.register(proto_type=OdometryData) @@ -39,82 +30,73 @@ class OdomDataPreparer(DataPreparer[OdometryData, OdomDataPreparerConfig]): def __init__(self, config: OdomDataPreparerConfig): super().__init__(config) self.config = config.get_config() - - def get_data_type(self) -> type[OdometryData]: - return OdometryData - - def get_used_indices(self) -> list[bool]: - used_indices: list[bool] = [] - used_indices.extend( - [self.config.position_source != OdometryPositionSource.DONT_USE] * 2 + self.use_position = ( + self.config.position_source != OdometryPositionSource.DONT_USE ) - used_indices.extend([True, True]) - used_indices.extend([self.config.use_rotation] * 2) - used_indices.extend([False]) - return used_indices - - def jacobian_h(self, x: NDArray[np.float64]) -> NDArray[np.float64]: - return transform_matrix_to_size(self.get_used_indices(), np.eye(7)) - - def hx(self, x: NDArray[np.float64]) -> NDArray[np.float64]: - return transform_vector_to_size(x, self.get_used_indices()) def calc_next_absolute_position( self, x: NDArray[np.float64], x_change: NDArray[np.float64], - rotation_matrix: NDArray[np.float64], ) -> NDArray[np.float64]: next_pos = x.copy() next_pos[0] += x_change[0] next_pos[1] += x_change[1] return next_pos - def prepare_input( + # @override + def get_data_type(self) -> type[OdometryData]: + return OdometryData + + # @override + def _used_indices(self) -> list[bool]: + used_indices: list[bool] = [] + + used_indices.extend([self.use_position] * 2) + used_indices.extend([True] * 2) + used_indices.extend([self.config.use_rotation]) + used_indices.extend([False]) + + return used_indices + + # @override + def _prepare( self, data: OdometryData, sensor_id: str, context: ExtrapolationContext | None = None, - ) -> KalmanFilterInput | None: + ) -> list[KalmanFilterInput] | KalmanFilterInput | None: assert context is not None - cos = context.x[4] - sin = context.x[5] - rotation_matrix = np.array( - [ - [cos, -sin], - [sin, cos], - ] - ) - - vel = np.array([data.velocity.x, data.velocity.y]) - if SHOULD_USE_ROTATION_MATRIX: - vel = rotation_matrix @ vel values: list[float] = [] + if self.config.position_source == OdometryPositionSource.ABSOLUTE: values.append(data.position.position.x) values.append(data.position.position.y) elif self.config.position_source == OdometryPositionSource.ABS_CHANGE: next_position = self.calc_next_absolute_position( - context.x, + context.filter.get_state(), np.array([data.position_change.x, data.position_change.y]), - rotation_matrix, ) - values.append(next_position[0]) values.append(next_position[1]) + vel = context.filter.angle_matrix() @ np.array( + [data.velocity.x, data.velocity.y] + ) + values.append(vel[0]) values.append(vel[1]) if self.config.use_rotation: - values.append(data.position.direction.x) - values.append(data.position.direction.y) + values.append( + np.atan2(data.position.direction.y, data.position.direction.x) + ) return KalmanFilterInput( - input=ProcessedData(data=np.array(values)), + input=np.array(values), sensor_id=sensor_id, sensor_type=KalmanFilterSensorType.ODOMETRY, - jacobian_h=self.jacobian_h, - hx=self.hx, + jacobian_h=T_EKF.generic_jacobian_h(self._used_indices()), + hx=T_EKF.generic_hx(self._used_indices()), ) diff --git a/src/backend/python/pos_extrapolator/util/init_stuff.py b/src/backend/python/pos_extrapolator/util/init_stuff.py new file mode 100644 index 0000000..c337a69 --- /dev/null +++ b/src/backend/python/pos_extrapolator/util/init_stuff.py @@ -0,0 +1,83 @@ +from autobahn_client.client import Autobahn +from autobahn_client.util import Address +from backend.generated.thrift.config.pos_extrapolator.ttypes import DataSources +from backend.python.common.debug.logger import LogLevel, init_logging +from backend.python.common.util.system import ( + BasicSystemConfig, + get_system_name, + load_configs, +) +from backend.python.pos_extrapolator.data_prep import DataPreparerManager +from backend.generated.proto.python.sensor.imu_pb2 import ImuData +from backend.generated.proto.python.sensor.odometry_pb2 import OdometryData +from backend.generated.proto.python.sensor.apriltags_pb2 import AprilTagData +from backend.generated.thrift.config.ttypes import Config +from backend.python.pos_extrapolator.preparers.ImuDataPreparer import ( + ImuDataPreparerConfig, +) +from backend.python.pos_extrapolator.preparers.OdomDataPreparer import ( + OdomDataPreparerConfig, +) +from backend.python.pos_extrapolator.preparers.AprilTagPreparer import ( + AprilTagDataPreparerConfig, + AprilTagPreparerConfig, +) + + +def _init_utilities(basic_system_config: BasicSystemConfig) -> Autobahn: + server = Autobahn( + Address(basic_system_config.autobahn.host, basic_system_config.autobahn.port) + ) + + init_logging( + "POSE_EXTRAPOLATOR", + LogLevel(basic_system_config.logging.global_logging_level), + system_pub_topic=basic_system_config.logging.global_log_pub_topic, + autobahn=server, + system_name=get_system_name(), + ) + + return server + + +def _init_data_preparers_and_get_topics(config: Config) -> list[str]: + enabled = config.pos_extrapolator.enabled_data_sources + msg_config = config.pos_extrapolator.message_config + topics: list[str] = [] + + if DataSources.IMU in enabled: + DataPreparerManager.set_config( + ImuData, ImuDataPreparerConfig(config.pos_extrapolator.imu_config) + ) + topics.append(msg_config.post_imu_input_topic) + + if DataSources.ODOMETRY in enabled: + DataPreparerManager.set_config( + OdometryData, OdomDataPreparerConfig(config.pos_extrapolator.odom_config) + ) + topics.append(msg_config.post_odometry_input_topic) + + if DataSources.APRIL_TAG in enabled: + DataPreparerManager.set_config( + AprilTagData, + AprilTagDataPreparerConfig( + AprilTagPreparerConfig( + tags_in_world=config.pos_extrapolator.april_tag_config.tag_position_config, + cameras_in_robot=config.pos_extrapolator.april_tag_config.camera_position_config, + use_imu_rotation=config.pos_extrapolator.april_tag_config.tag_use_imu_rotation, + april_tag_config=config.pos_extrapolator.april_tag_config, + ), + ), + ) + topics.append(msg_config.post_tag_input_topic) + + return topics + + +def main_init_phase() -> tuple[BasicSystemConfig, Config, Autobahn, list[str]]: + system_config, config = load_configs() + + autobahn_server = _init_utilities(system_config) + subscribe_topics = _init_data_preparers_and_get_topics(config) + + return system_config, config, autobahn_server, subscribe_topics diff --git a/src/backend/python/pos_extrapolator/filters/gate/mahalanobis.py b/src/backend/python/pos_extrapolator/util/mahalanobis.py similarity index 100% rename from src/backend/python/pos_extrapolator/filters/gate/mahalanobis.py rename to src/backend/python/pos_extrapolator/util/mahalanobis.py diff --git a/src/config/cameras/a-bot/front_left.ts b/src/config/cameras/a-bot/front_left.ts new file mode 100644 index 0000000..3616de3 --- /dev/null +++ b/src/config/cameras/a-bot/front_left.ts @@ -0,0 +1,37 @@ +import { + CameraParameters, + CameraType, +} from "generated/thrift/gen-nodejs/camera_types"; +import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; + +const front_left: CameraParameters = { + pi_to_run_on: "tynan", + name: "front_left", + camera_path: "/dev/usb_cam3", + flags: 0, + width: 800, + height: 600, + max_fps: 100, + camera_matrix: MatrixUtil.buildMatrix([ + [456.81085799059014, 0.0, 395.06984136844125], + [0.0, 456.7987342006777, 334.73571738807914], + [0.0, 0.0, 1.0], + ]), + dist_coeff: VectorUtil.fromArray([ + 0.052745830893280964, -0.08619099299637119, -0.00044316972116126193, + 0.00004422164981020342, 0.022592221476200467, + ]), + exposure_time: 8, + camera_type: CameraType.OV2311, + video_options: { + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, + publication_topic: "camera/front_left/video", + do_compression: true, + overlay_tags: true, + }, + do_detection: true, +}; + +export default front_left; diff --git a/src/config/cameras/a-bot/front_right.ts b/src/config/cameras/a-bot/front_right.ts new file mode 100644 index 0000000..d5acf38 --- /dev/null +++ b/src/config/cameras/a-bot/front_right.ts @@ -0,0 +1,37 @@ +import { + CameraParameters, + CameraType, +} from "generated/thrift/gen-nodejs/camera_types"; +import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; + +const front_right: CameraParameters = { + pi_to_run_on: "tynan", + name: "front_right", + camera_path: "/dev/usb_cam1", + flags: 0, + width: 800, + height: 600, + max_fps: 100, + camera_matrix: MatrixUtil.buildMatrix([ + [456.4566091243219, 0.0, 403.4510675692207], + [0.0, 456.4929611660038, 320.254620681183], + [0.0, 0.0, 1.0], + ]), + dist_coeff: VectorUtil.fromArray([ + 0.05147776259797679, -0.08376762888571426, -0.0005087791220038304, + -5.9848176245483235e-5, 0.020125747371733234, + ]), + exposure_time: 8, + camera_type: CameraType.OV2311, + video_options: { + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, + do_compression: true, + publication_topic: "camera/front_right/video", + overlay_tags: true, + }, + do_detection: true, +}; + +export default front_right; diff --git a/src/config/cameras/a-bot/rear_left.ts b/src/config/cameras/a-bot/rear_left.ts new file mode 100644 index 0000000..ee08075 --- /dev/null +++ b/src/config/cameras/a-bot/rear_left.ts @@ -0,0 +1,37 @@ +import { + CameraType, + type CameraParameters, +} from "generated/thrift/gen-nodejs/camera_types"; +import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; + +const rear_left: CameraParameters = { + pi_to_run_on: "agatha-king", + name: "rear_left", + camera_path: "/dev/usb_cam1", + flags: 0, + width: 800, + height: 600, + max_fps: 100, + camera_matrix: MatrixUtil.buildMatrix([ + [455.48175495087486, 0.0, 407.2173573090477], + [0.0, 455.4341511347836, 335.6999066346699], + [0.0, 0.0, 1.0], + ]), + dist_coeff: VectorUtil.fromArray([ + 0.04908778853478117, -0.08285586799580322, -0.0003984055550807527, + -0.0001698331827697349, 0.022084131827275894, + ]), + exposure_time: 8, + camera_type: CameraType.OV2311, + video_options: { + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, + overlay_tags: true, + publication_topic: "camera/rear_left/video", + do_compression: true, + }, + do_detection: true, +}; + +export default rear_left; diff --git a/src/config/cameras/a-bot/rear_right.ts b/src/config/cameras/a-bot/rear_right.ts new file mode 100644 index 0000000..13daf2d --- /dev/null +++ b/src/config/cameras/a-bot/rear_right.ts @@ -0,0 +1,37 @@ +import { + CameraType, + type CameraParameters, +} from "generated/thrift/gen-nodejs/camera_types"; +import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; + +const rear_right: CameraParameters = { + pi_to_run_on: "agatha-king", + name: "rear_right", + camera_path: "/dev/usb_cam3", + flags: 0, + width: 800, + height: 600, + max_fps: 100, + camera_matrix: MatrixUtil.buildMatrix([ + [456.10504968438045, 0.0, 403.6933383290121], + [0.0, 456.0604482158868, 341.09074241391494], + [0.0, 0.0, 1.0], + ]), + dist_coeff: VectorUtil.fromArray([ + 0.04841029488157198, -0.08174454831935413, 0.0001501040390929917, + 0.00011501008144279749, 0.021698542194869413, + ]), + exposure_time: 8, + camera_type: CameraType.OV2311, + video_options: { + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, + overlay_tags: true, + publication_topic: "camera/rear_right/video", + do_compression: true, + }, + do_detection: true, +}; + +export default rear_right; diff --git a/src/config/cameras/b-bot/front_left.ts b/src/config/cameras/b-bot/front_left.ts index 7a2f384..280361c 100644 --- a/src/config/cameras/b-bot/front_left.ts +++ b/src/config/cameras/b-bot/front_left.ts @@ -3,11 +3,12 @@ import { CameraType, } from "generated/thrift/gen-nodejs/camera_types"; import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; const front_left: CameraParameters = { pi_to_run_on: "agatha-king", name: "front_left", - camera_path: "/dev/usb_cam1", + camera_path: "/dev/usb_cam4", flags: 0, width: 800, height: 600, @@ -21,11 +22,11 @@ const front_left: CameraParameters = { 0.052745830893280964, -0.08619099299637119, -0.00044316972116126193, 0.00004422164981020342, 0.022592221476200467, ]), - exposure_time: 10, + exposure_time: 8, camera_type: CameraType.OV2311, video_options: { - send_feed: true, - compression_quality: 20, + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, publication_topic: "camera/front_left/video", do_compression: true, overlay_tags: true, diff --git a/src/config/cameras/b-bot/front_right.ts b/src/config/cameras/b-bot/front_right.ts index a9a76ad..0165966 100644 --- a/src/config/cameras/b-bot/front_right.ts +++ b/src/config/cameras/b-bot/front_right.ts @@ -3,11 +3,12 @@ import { CameraType, } from "generated/thrift/gen-nodejs/camera_types"; import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; const front_right: CameraParameters = { - pi_to_run_on: "tynan", + pi_to_run_on: "agatha-king", name: "front_right", - camera_path: "/dev/usb_cam3", + camera_path: "/dev/usb_cam2", flags: 0, width: 800, height: 600, @@ -21,14 +22,14 @@ const front_right: CameraParameters = { 0.05147776259797679, -0.08376762888571426, -0.0005087791220038304, -5.9848176245483235e-5, 0.020125747371733234, ]), - exposure_time: 10, + exposure_time: 8, camera_type: CameraType.OV2311, video_options: { - send_feed: true, + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, do_compression: true, publication_topic: "camera/front_right/video", overlay_tags: true, - compression_quality: 20, }, do_detection: true, }; diff --git a/src/config/cameras/b-bot/rear_left.ts b/src/config/cameras/b-bot/rear_left.ts index 959b954..5df509a 100644 --- a/src/config/cameras/b-bot/rear_left.ts +++ b/src/config/cameras/b-bot/rear_left.ts @@ -3,11 +3,12 @@ import { type CameraParameters, } from "generated/thrift/gen-nodejs/camera_types"; import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; const rear_left: CameraParameters = { pi_to_run_on: "nathan-hale", name: "rear_left", - camera_path: "/dev/usb_cam3", + camera_path: "/dev/usb_cam2", flags: 0, width: 800, height: 600, @@ -21,13 +22,13 @@ const rear_left: CameraParameters = { 0.04908778853478117, -0.08285586799580322, -0.0003984055550807527, -0.0001698331827697349, 0.022084131827275894, ]), - exposure_time: 10, + exposure_time: 8, camera_type: CameraType.OV2311, video_options: { - send_feed: false, + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, overlay_tags: true, publication_topic: "camera/rear_left/video", - compression_quality: 10, do_compression: true, }, do_detection: true, diff --git a/src/config/cameras/b-bot/rear_right.ts b/src/config/cameras/b-bot/rear_right.ts index 4cb7083..4b0c981 100644 --- a/src/config/cameras/b-bot/rear_right.ts +++ b/src/config/cameras/b-bot/rear_right.ts @@ -3,11 +3,12 @@ import { type CameraParameters, } from "generated/thrift/gen-nodejs/camera_types"; import { MatrixUtil, VectorUtil } from "../../util/math"; +import { CameraConstants } from "../camera_constants"; const rear_right: CameraParameters = { pi_to_run_on: "nathan-hale", name: "rear_right", - camera_path: "/dev/usb_cam1", + camera_path: "/dev/usb_cam4", flags: 0, width: 800, height: 600, @@ -21,13 +22,13 @@ const rear_right: CameraParameters = { 0.04841029488157198, -0.08174454831935413, 0.0001501040390929917, 0.00011501008144279749, 0.021698542194869413, ]), - exposure_time: 10, + exposure_time: 8, camera_type: CameraType.OV2311, video_options: { - send_feed: false, + send_feed: CameraConstants.kSendFeed, + compression_quality: CameraConstants.kCompressionQuality, overlay_tags: true, publication_topic: "camera/rear_right/video", - compression_quality: 10, do_compression: true, }, do_detection: true, diff --git a/src/config/cameras/b-bot/unused/logitech_cam.ts b/src/config/cameras/b-bot/unused/logitech_cam.ts index 41d3456..149aaa0 100644 --- a/src/config/cameras/b-bot/unused/logitech_cam.ts +++ b/src/config/cameras/b-bot/unused/logitech_cam.ts @@ -5,9 +5,9 @@ import { import { MatrixUtil, VectorUtil } from "../../../util/math"; const logitech_cam: CameraParameters = { - pi_to_run_on: "agatha-king", + pi_to_run_on: "jetson1", name: "logitech", - camera_path: "/dev/usb_cam3", + camera_path: "/dev/video0", flags: 0, width: 640, height: 480, @@ -25,8 +25,8 @@ const logitech_cam: CameraParameters = { camera_type: CameraType.ULTRAWIDE_100, brightness: 50, video_options: { - send_feed: false, - overlay_tags: false, + send_feed: true, + overlay_tags: true, publication_topic: "camera/logitech/video", compression_quality: 10, do_compression: true, diff --git a/src/config/cameras/camera_constants.ts b/src/config/cameras/camera_constants.ts new file mode 100644 index 0000000..5ebd607 --- /dev/null +++ b/src/config/cameras/camera_constants.ts @@ -0,0 +1,4 @@ +export class CameraConstants { + static readonly kCompressionQuality = 20; + static readonly kSendFeed = true; +} diff --git a/src/config/index.ts b/src/config/index.ts index fedaa0a..b50e1a2 100644 --- a/src/config/index.ts +++ b/src/config/index.ts @@ -4,10 +4,10 @@ import prod1 from "./cameras/prod_1"; import lidar_configs from "./lidar"; import pathfinding_config from "./pathfinding"; import { pose_extrapolator } from "./pos_extrapolator"; -import front_left from "./cameras/b-bot/front_left"; -import front_right from "./cameras/b-bot/front_right"; -import rear_left from "./cameras/b-bot/rear_left"; -import rear_right from "./cameras/b-bot/rear_right"; +import front_left from "./cameras/a-bot/front_left"; +import front_right from "./cameras/a-bot/front_right"; +import rear_left from "./cameras/a-bot/rear_left"; +import rear_right from "./cameras/a-bot/rear_right"; import jetson_cam from "./cameras/jetson_cam"; import logitech_cam from "./cameras/b-bot/unused/logitech_cam"; diff --git a/src/config/pos_extrapolator/april_tag_det_config/index.ts b/src/config/pos_extrapolator/april_tag_det_config/index.ts index 28639c0..2e8a3c7 100644 --- a/src/config/pos_extrapolator/april_tag_det_config/index.ts +++ b/src/config/pos_extrapolator/april_tag_det_config/index.ts @@ -1,10 +1,8 @@ import { AprilTagConfig, - TagDisambiguationMode, TagNoiseAdjustMode, TagUseImuRotation, } from "generated/thrift/gen-nodejs/pos_extrapolator_types"; -import { reefscape_field } from "../tag_config/reefscape"; import { MatrixUtil, VectorUtil } from "../../util/math"; import { rebuilt_welded_field } from "../tag_config/rebuilt_welded"; @@ -12,33 +10,34 @@ const april_tag_pos_config: AprilTagConfig = { tag_position_config: rebuilt_welded_field, camera_position_config: { front_left: { - position: VectorUtil.fromArray([0.38, 0.38, 0.0]), - rotation: MatrixUtil.buildRotationMatrixFromYaw(45), + position: VectorUtil.fromArray([0.078, 0.3241608, 0.0]), + rotation: MatrixUtil.buildRotationMatrixFromYaw(52.904), }, front_right: { - position: VectorUtil.fromArray([0.38, -0.38, 0.0]), + position: VectorUtil.fromArray([0.14, -0.3, 0.0]), rotation: MatrixUtil.buildRotationMatrixFromYaw(-45), }, rear_left: { - position: VectorUtil.fromArray([-0.38, 0.38, 0.0]), - rotation: MatrixUtil.buildRotationMatrixFromYaw(135), + position: VectorUtil.fromArray([-0.078, 0.3241608, 0.0]), + rotation: MatrixUtil.buildRotationMatrixFromYaw(127.096), }, rear_right: { - position: VectorUtil.fromArray([-0.38, -0.38, 0.0]), + position: VectorUtil.fromArray([-0.23, -0.33, 0.0]), rotation: MatrixUtil.buildRotationMatrixFromYaw(225), }, }, - tag_use_imu_rotation: TagUseImuRotation.UNTIL_FIRST_NON_TAG_ROTATION, - disambiguation_time_window_s: 0.05, - tag_disambiguation_mode: TagDisambiguationMode.LEAST_ANGLE_AND_DISTANCE, - tag_noise_adjust_mode: [], + tag_use_imu_rotation: TagUseImuRotation.WHILE_NO_OTHER_ROTATION_DATA, + noise_change_modes: [ + // TagNoiseAdjustMode.ADD_WEIGHT_PER_M_DISTANCE_TAG, + TagNoiseAdjustMode.ADD_WEIGHT_PER_TAG_CONFIDENCE, + ], tag_noise_adjust_config: { - weight_per_m_from_distance_from_tag: 10, // multiply by meters - weight_per_degree_from_angle_error_tag: 0.05, - weight_per_confidence_tag: 0.8, - multiply_coef_m_distance_from_tag: 1.0, - pow_distance_from_tag_coef: 2.0, + weight_per_m_from_distance_from_tag: 0.3, + weight_per_degree_from_angle_error_tag: 0.0, + weight_per_confidence_tag: 0.04, + min_distance_from_tag_to_use_noise_adjustment: 1.5, }, + insert_predicted_global_rotation: true, }; export default april_tag_pos_config; diff --git a/src/config/pos_extrapolator/imu_config/navx.ts b/src/config/pos_extrapolator/imu_config/navx.ts index 3525767..7bd9f15 100644 --- a/src/config/pos_extrapolator/imu_config/navx.ts +++ b/src/config/pos_extrapolator/imu_config/navx.ts @@ -1,32 +1,15 @@ +import { ImuConfig } from "generated/thrift/gen-nodejs/pos_extrapolator_types"; import { MatrixUtil, VectorUtil } from "../../util/math"; -export const nav_x_config = { - "0": { +export const nav_x_config: { [k: string]: ImuConfig } = { + "41": { use_position: false, - use_rotation_absolute: true, - use_rotation_velocity: true, + use_rotation: true, use_velocity: false, - imu_robot_position: { - position: VectorUtil.fromArray([0.0, 0.0, 0.0]), - rotation: MatrixUtil.buildMatrix([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 1], - ]), - }, }, - "1": { + "40": { use_position: false, - use_rotation_absolute: true, - use_rotation_velocity: true, + use_rotation: true, use_velocity: false, - imu_robot_position: { - position: VectorUtil.fromArray([0.0, 0.0, 0.0]), - rotation: MatrixUtil.buildMatrix([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 1], - ]), - }, }, }; diff --git a/src/config/pos_extrapolator/index.ts b/src/config/pos_extrapolator/index.ts index cdbc1c9..212148b 100644 --- a/src/config/pos_extrapolator/index.ts +++ b/src/config/pos_extrapolator/index.ts @@ -1,4 +1,5 @@ import { + DataSources, PosExtrapolator, TagUseImuRotation, } from "generated/thrift/gen-nodejs/pos_extrapolator_types"; @@ -13,13 +14,15 @@ import april_tag_det_config from "./april_tag_det_config"; export const pose_extrapolator: PosExtrapolator = { message_config: message_config, - enable_imu: true, - enable_odom: true, - enable_tags: true, + enabled_data_sources: [ + DataSources.APRIL_TAG, + DataSources.ODOMETRY, + DataSources.IMU, + ], odom_config: swerve_odom_config, imu_config: nav_x_config, kalman_filter_config: kalman_filter, - time_s_between_position_sends: 0.015, + time_s_between_position_sends: 0.02, future_position_prediction_margin_s: 0, april_tag_config: april_tag_det_config, }; diff --git a/src/config/pos_extrapolator/kalman_filter_config/index.ts b/src/config/pos_extrapolator/kalman_filter_config/index.ts index c5055f2..dea388f 100644 --- a/src/config/pos_extrapolator/kalman_filter_config/index.ts +++ b/src/config/pos_extrapolator/kalman_filter_config/index.ts @@ -5,59 +5,52 @@ import { import { MatrixUtil, VectorUtil } from "../../util/math"; export const kalman_filter: KalmanFilterConfig = { - dim_x_z: [7, 7], - state_vector: VectorUtil.fromArray([2.0, 5.0, 0.0, 0.0, 1.0, 0.0, 0.0]), // [x, y, vx, vy, cos, sin, angular_velocity_rad_s] + initial_state_vector: VectorUtil.fromArray([2.0, 5.0, 0.0, 0.0, 0.0, 0.0]), // [x, y, vx, vy, angle, angular_velocity_rad_s] uncertainty_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 10.0, 10.0, 10.0, 1.0, ]), process_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 0.0001, - 0.0001, - 1, - 1, - 1, - 1, - 1, // lower is worse BTW. The higher the number, the more the filter follows the measurements instead of the model (predict step) + 0.0005, 0.0005, 1, 1, 0.01, 1, ]), sensors: { [KalmanFilterSensorType.APRIL_TAG]: { front_left: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 1.0, 1.0, 5.0, 5.0, + 3.0, 3.0, 2.0, ]), }, front_right: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 1.0, 1.0, 5.0, 5.0, + 3.0, 3.0, 2.0, ]), }, rear_left: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 1.0, 1.0, 5.0, 5.0, + 3.0, 3.0, 2.0, ]), }, rear_right: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 1.0, 1.0, 5.0, 5.0, + 3.0, 3.0, 2.0, ]), }, }, [KalmanFilterSensorType.IMU]: { - 0: { + 40: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 0.01, 0.01, 0.001, + 0.15, 0.15, ]), }, - 1: { + 41: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 0.01, 0.01, 0.001, + 0.15, 0.15, ]), }, }, [KalmanFilterSensorType.ODOMETRY]: { odom: { measurement_noise_matrix: MatrixUtil.buildMatrixFromDiagonal([ - 0.001, 0.001, + 5, 5, 0.1, 0.1, ]), }, }, diff --git a/src/config/pos_extrapolator/odom_config/swerve_odom.ts b/src/config/pos_extrapolator/odom_config/swerve_odom.ts index 3f1541c..40b207a 100644 --- a/src/config/pos_extrapolator/odom_config/swerve_odom.ts +++ b/src/config/pos_extrapolator/odom_config/swerve_odom.ts @@ -5,6 +5,6 @@ import { import { MatrixUtil, VectorUtil } from "../../util/math"; export const swerve_odom_config: OdomConfig = { - position_source: OdometryPositionSource.DONT_USE, + position_source: OdometryPositionSource.ABS_CHANGE, use_rotation: false, }; diff --git a/src/main/deploy/pathplanner/autos/Ball Shooter Left.auto b/src/main/deploy/pathplanner/autos/Ball Shooter Left.auto index 0d49488..8431115 100644 --- a/src/main/deploy/pathplanner/autos/Ball Shooter Left.auto +++ b/src/main/deploy/pathplanner/autos/Ball Shooter Left.auto @@ -5,15 +5,34 @@ "data": { "commands": [ { - "type": "path", + "type": "parallel", "data": { - "pathName": "Ball Shooter Left" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ball Shooter Left" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousAimCommand" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousShooterCommand" + } + } + ] } } ] } }, "resetOdom": false, - "folder": null, + "folder": "Going Out", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Ball Shooter Right.auto b/src/main/deploy/pathplanner/autos/Ball Shooter Right.auto new file mode 100644 index 0000000..ef4a422 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Ball Shooter Right.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ball Shooter Right" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousAimCommand" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousShooterCommand" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "Going Out", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Climber Left.auto b/src/main/deploy/pathplanner/autos/Climber Left.auto deleted file mode 100644 index 8fb5ead..0000000 --- a/src/main/deploy/pathplanner/autos/Climber Left.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Climber Left" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Climber Middle.auto b/src/main/deploy/pathplanner/autos/Climber Middle.auto deleted file mode 100644 index 1b5b059..0000000 --- a/src/main/deploy/pathplanner/autos/Climber Middle.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Climber Middle" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Climber Right Left.auto b/src/main/deploy/pathplanner/autos/Climber Right Left.auto new file mode 100644 index 0000000..3f25a09 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Climber Right Left.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Climber Right Left" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousAimCommand" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousShooterCommand" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ClimberL1Command" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Climber", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Climber Right Middle.auto b/src/main/deploy/pathplanner/autos/Climber Right Middle.auto new file mode 100644 index 0000000..7a6979c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Climber Right Middle.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Climber Right Middle" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousAimCommand" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousShooterCommand" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ClimberL1Command" + } + } + ] + } + }, + "resetOdom": false, + "folder": "Climber", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Climber Right Right.auto b/src/main/deploy/pathplanner/autos/Climber Right Right.auto new file mode 100644 index 0000000..de86c50 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Climber Right Right.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Climber Right Right" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousAimCommand" + } + }, + { + "type": "named", + "data": { + "name": "ContinuousShooterCommand" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ClimberL1Command" + } + } + ] + } + }, + "resetOdom": false, + "folder": "Climber", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index ac5f521..acc861f 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ball Shooter Left.path b/src/main/deploy/pathplanner/paths/Ball Shooter Left.path index 8b52553..2d1c7fa 100644 --- a/src/main/deploy/pathplanner/paths/Ball Shooter Left.path +++ b/src/main/deploy/pathplanner/paths/Ball Shooter Left.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 13.84744187186324, - "y": 0.8109428913111674 + "x": 14.291603443287038, + "y": 1.342671802662037 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 0.7066097965025095 }, "prevControl": { - "x": 13.742322192602678, - "y": 0.7103657489505717 + "x": 14.453399016203704, + "y": 0.8608936631944448 }, "nextControl": { - "x": 12.533609482826222, - "y": 0.6925623921737769 + "x": 12.545441510262824, + "y": 0.5561902920186 }, "isLocked": false, "linkedName": null @@ -36,28 +36,28 @@ "y": 0.7066097965025095 }, "prevControl": { - "x": 10.785552266311168, - "y": 0.6822046541718951 + "x": 10.779076267839024, + "y": 0.6052446187932409 }, "nextControl": { - "x": 9.78512617507504, - "y": 0.7147345004221098 + "x": 9.726028935185186, + "y": 0.7486996527777785 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.733372542134832, - "y": 1.2587261311951068 + "x": 8.912655164930555, + "y": 1.024526331018519 }, "prevControl": { - "x": 8.710842607934593, - "y": 1.0097433967235538 + "x": 9.535909957106012, + "y": 0.6564552235075098 }, "nextControl": { - "x": 8.755902476335072, - "y": 1.5077088656666617 + "x": 8.335610532407408, + "y": 1.3653074363425934 }, "isLocked": false, "linkedName": null @@ -126,7 +126,14 @@ "name": "Point Towards Zone" } ], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "IntakeCommand", + "waypointRelativePos": 3.131281146179404, + "endWaypointRelativePos": 4.0, + "command": null + } + ], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, @@ -140,7 +147,7 @@ "rotation": 89.09650867272647 }, "reversed": false, - "folder": null, + "folder": "Other", "idealStartingState": { "velocity": 0, "rotation": 148.3362765939237 diff --git a/src/main/deploy/pathplanner/paths/Ball Shooter Right.path b/src/main/deploy/pathplanner/paths/Ball Shooter Right.path new file mode 100644 index 0000000..f0cb559 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ball Shooter Right.path @@ -0,0 +1,128 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.459, + "y": 4.039 + }, + "prevControl": null, + "nextControl": { + "x": 14.20117751837195, + "y": 4.4187590847407705 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.650850983796296, + "y": 7.075785734953704 + }, + "prevControl": { + "x": 14.480758608217593, + "y": 6.600240596064815 + }, + "nextControl": { + "x": 12.867573686312653, + "y": 7.524611243069874 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.916895833333333, + "y": 7.481193214699074 + }, + "prevControl": { + "x": 12.213521050347222, + "y": 7.448059895833334 + }, + "nextControl": { + "x": 11.111595917432368, + "y": 7.571145981239656 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.52873806423611, + "y": 7.075785734953704 + }, + "prevControl": { + "x": 9.940378544560184, + "y": 7.341639612268518 + }, + "nextControl": { + "x": 8.382539517923878, + "y": 6.33552489503385 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.75676923227729, + "y": 1.2206736688362616 + }, + "prevControl": { + "x": 8.89080685763889, + "y": 1.5084696180555555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.398026315789474, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.392960963455146, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeCommand", + "waypointRelativePos": 2.9135174418604635, + "endWaypointRelativePos": 4.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Other", + "idealStartingState": { + "velocity": 0, + "rotation": -48.430731805286314 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Climber Left.path b/src/main/deploy/pathplanner/paths/Climber Left.path index e586732..93bc3b3 100644 --- a/src/main/deploy/pathplanner/paths/Climber Left.path +++ b/src/main/deploy/pathplanner/paths/Climber Left.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Climber", "idealStartingState": { "velocity": 0.0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Climber Middle.path b/src/main/deploy/pathplanner/paths/Climber Middle.path index 8f8636d..5693715 100644 --- a/src/main/deploy/pathplanner/paths/Climber Middle.path +++ b/src/main/deploy/pathplanner/paths/Climber Middle.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Climber", "idealStartingState": { "velocity": 0, "rotation": -179.4469495212779 diff --git a/src/main/deploy/pathplanner/paths/Climber Right Left.path b/src/main/deploy/pathplanner/paths/Climber Right Left.path new file mode 100644 index 0000000..2290fd2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Climber Right Left.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.499588831018519, + "y": 3.0669822048611115 + }, + "prevControl": null, + "nextControl": { + "x": 14.048975694444445, + "y": 4.291734013310185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.002572550641021, + "y": 4.760284791619958 + }, + "prevControl": { + "x": 14.44408232060185, + "y": 4.807432798032408 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.25, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Climber", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Climber Right Middle.path b/src/main/deploy/pathplanner/paths/Climber Right Middle.path new file mode 100644 index 0000000..2f502cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Climber Right Middle.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.010003038194446, + "y": 4.059472728587963 + }, + "prevControl": null, + "nextControl": { + "x": 13.945507884837964, + "y": 4.675883680555555 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.002572550641021, + "y": 4.760284791619958 + }, + "prevControl": { + "x": 14.44408232060185, + "y": 4.807432798032408 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5385279605263157, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.25, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Climber", + "idealStartingState": { + "velocity": 0, + "rotation": 140.36618013835135 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Climber Right Right.path b/src/main/deploy/pathplanner/paths/Climber Right Right.path new file mode 100644 index 0000000..8dd3a70 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Climber Right Right.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.037165798604896, + "y": 5.366500144679221 + }, + "prevControl": null, + "nextControl": { + "x": 13.994650173611113, + "y": 4.668469690393518 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.002572550641021, + "y": 4.760284791619958 + }, + "prevControl": { + "x": 14.458923838605466, + "y": 4.82786492304795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.25, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Climber", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index 2f68e3c..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,107 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 14.45930396412037, - "y": 4.038674189814815 - }, - "prevControl": null, - "nextControl": { - "x": 14.201481521353433, - "y": 4.418433300938807 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 13.962435402199073, - "y": 7.214486400462963 - }, - "prevControl": { - "x": 14.197200303568607, - "y": 7.128547811430138 - }, - "nextControl": { - "x": 13.589363464987958, - "y": 7.351053986608218 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 11.94104050925926, - "y": 7.391766059027777 - }, - "prevControl": { - "x": 12.206481078627007, - "y": 7.395836866135808 - }, - "nextControl": { - "x": 11.504664858217593, - "y": 7.385073784722222 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.75676923227729, - "y": 7.214486400462963 - }, - "prevControl": { - "x": 8.883867233510239, - "y": 7.432840911215207 - }, - "nextControl": { - "x": 8.507916797365118, - "y": 6.786957634096613 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.75676923227729, - "y": 1.2206736688362616 - }, - "prevControl": { - "x": 8.76130352101631, - "y": 1.591951899702008 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.1873881932021477, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 179.89395573923747 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 88a92f3..23f09a8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,14 +2,20 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "Climber", + "Other" + ], + "autoFolders": [ + "Climber", + "Going Out" + ], "defaultMaxVel": 1.0, "defaultMaxAccel": 2.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.089, + "robotMass": 100.089, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, diff --git a/src/main/java/frc/robot/JobApplication.job b/src/main/java/frc/robot/JobApplication.job new file mode 100644 index 0000000..4fa16eb --- /dev/null +++ b/src/main/java/frc/robot/JobApplication.job @@ -0,0 +1 @@ +scary \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8484730..0d3e4d8 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -4,7 +4,8 @@ public final class Main { - private Main() {} + private Main() { + } public static void main(String... args) { RobotBase.startRobot(Robot::new); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b46991a..73f3efe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,7 +1,6 @@ package frc.robot; import java.io.IOException; -import java.util.ArrayList; import java.util.List; import org.littletonrobotics.junction.LoggedRobot; @@ -10,32 +9,39 @@ import autobahn.client.Address; import autobahn.client.AutobahnClient; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.constant.PiConstants; +import frc.robot.constant.CommunicationConstants; import frc.robot.util.RPC; import lombok.Getter; +import pwrup.frc.core.constant.RaspberryPiConstants; import pwrup.frc.core.online.raspberrypi.OptionalAutobahn; import pwrup.frc.core.online.raspberrypi.discovery.PiDiscoveryUtil; import pwrup.frc.core.online.raspberrypi.discovery.PiInfo; public class Robot extends LoggedRobot { + private static final int kNetworkRetryTicks = 80; @Getter private static OptionalAutobahn communicationClient = new OptionalAutobahn(); @Getter - private static boolean onlineStatus; + private static NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); + @Getter + private static NetworkTable dashboard = networkTableInstance.getTable(CommunicationConstants.kSharedTable); + + private int retryCounter; + private volatile boolean networkAttemptInProgress; private RobotContainer m_robotContainer; private Command m_autonomousCommand; - public static OptionalAutobahn getAutobahnClient() { - return communicationClient; - } - public Robot() { Logger.addDataReceiver(new NT4Publisher()); Logger.start(); + this.networkAttemptInProgress = false; + this.retryCounter = 0; RPC.SetClient(communicationClient); } @@ -50,7 +56,13 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); - Logger.recordOutput("Autobahn/Connected", communicationClient.isConnected() && onlineStatus); + boolean currentlyConnected = communicationClient.isConnected(); + Logger.recordOutput("Autobahn/Connected", currentlyConnected); + + retryCounter = (retryCounter + 1) % kNetworkRetryTicks; + if (!currentlyConnected && !networkAttemptInProgress && retryCounter == 0) { + initializeNetwork(); + } } @Override @@ -68,8 +80,6 @@ public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { CommandScheduler.getInstance().schedule(m_autonomousCommand); - } else { - System.out.println("WARNING: getAutonomousCommand() returned null; nothing scheduled for auton."); } } @@ -102,26 +112,35 @@ public void testPeriodic() { } private void initializeNetwork() { - new Thread(() -> { - List pisFound = new ArrayList<>(); + if (networkAttemptInProgress || communicationClient.isConnected()) { + return; + } + networkAttemptInProgress = true; + new Thread(() -> { try { - pisFound = PiDiscoveryUtil.discover(PiConstants.networkInitializeTimeSec); + List pisFound = PiDiscoveryUtil.discover(4); + if (pisFound.isEmpty()) { + System.out.println("NO PIS FOUND!"); + networkAttemptInProgress = false; + return; + } + + var pi = pisFound.get(0); + var address = new Address(pi.getHostnameLocal(), + pi.getAutobahnPort().orElse(RaspberryPiConstants.DEFAULT_PORT_AUTOB)); + var realClient = new AutobahnClient(address); + realClient.begin().join(); + + communicationClient.setAutobahnClient(realClient); + retryCounter = 0; + + System.out.println("[PiConnect] Connected to Pi Autobahn at " + address); } catch (IOException | InterruptedException e) { - e.printStackTrace(); + // e.printStackTrace(); + } finally { + networkAttemptInProgress = false; } - - var mainPi = pisFound.get(0); - - System.out.println(mainPi); - - var address = new Address(mainPi.getHostnameLocal(), mainPi.getAutobahnPort().get()); - System.out.println(address); - var realClient = new AutobahnClient(address); - realClient.begin().join(); - communicationClient.setAutobahnClient(realClient); - onlineStatus = true; - System.out.println("SET UP CLIENT"); }).start(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fae0ba7..2f33bd6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,75 +1,195 @@ package frc.robot; -import com.pathplanner.lib.commands.PathPlannerAuto; +import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj2.command.Command; +import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.command.SwerveMoveTeleop; -import frc.robot.constant.BotConstants; -import frc.robot.hardware.AHRSGyro; -import frc.robot.subsystem.CameraSubsystem; +import frc.robot.command.intake.IntakeCommand; +import frc.robot.command.scoring.ContinuousAimCommand; +import frc.robot.command.scoring.ManualAimCommand; +import frc.robot.command.shooting.ContinuousManualShooter; +import frc.robot.command.shooting.ContinuousShooter; +import frc.robot.command.testing.IndexCommand; +import frc.robot.constant.IndexConstants; +import frc.robot.constant.IntakeConstants.WristRaiseLocation; +import frc.robot.constant.PathPlannerConstants; +import frc.robot.hardware.PigeonGyro; +import frc.robot.hardware.UnifiedGyro; import frc.robot.subsystem.GlobalPosition; +import frc.robot.subsystem.IndexSubsystem; +import frc.robot.subsystem.IntakeSubsystem; import frc.robot.subsystem.OdometrySubsystem; +import frc.robot.subsystem.PathPlannerSubsystem; +import frc.robot.subsystem.ShooterSubsystem; import frc.robot.subsystem.SwerveSubsystem; -import frc.robot.util.PathPlannerSetup; +import frc.robot.subsystem.TurretSubsystem; +import frc.robot.util.AimPoint; import pwrup.frc.core.controller.FlightModule; import pwrup.frc.core.controller.FlightStick; -import pwrup.frc.core.controller.LogitechController; import pwrup.frc.core.controller.OperatorPanel; import pwrup.frc.core.online.PublicationSubsystem; public class RobotContainer { - - final LogitechController m_controller = new LogitechController(0); final OperatorPanel m_operatorPanel = new OperatorPanel(1); final FlightStick m_leftFlightStick = new FlightStick(2); final FlightStick m_rightFlightStick = new FlightStick(3); final FlightModule m_flightModule = new FlightModule( m_leftFlightStick, m_rightFlightStick); - static final String kPathPlannerAutoName = "Ball Shooter Left"; public RobotContainer() { + PublicationSubsystem.GetInstance(Robot.getCommunicationClient()); + GlobalPosition.GetInstance(); - OdometrySubsystem.GetInstance(); - AHRSGyro.GetInstance(); - SwerveSubsystem.GetInstance(); - CameraSubsystem.GetInstance(); - // Initialize publication subsystem for sending data to Pi - PublicationSubsystem.GetInstance(Robot.getCommunicationClient()); - PathPlannerSetup.configure(); + UnifiedGyro.GetInstance(); + OdometrySubsystem.GetInstance(UnifiedGyro.GetInstance()); + SwerveSubsystem.GetInstance(UnifiedGyro.GetInstance()); + + TurretSubsystem.GetInstance(); + ShooterSubsystem.GetInstance(); + IndexSubsystem.GetInstance(); + IntakeSubsystem.GetInstance(); setSwerveCommands(); + setTurretCommands(); + setShooterCommands(); + setIntakeCommands(); } private void setSwerveCommands() { SwerveSubsystem swerveSubsystem = SwerveSubsystem.GetInstance(); - swerveSubsystem.setDefaultCommand(new SwerveMoveTeleop(swerveSubsystem, m_flightModule)); + swerveSubsystem + .setDefaultCommand( + new SwerveMoveTeleop(swerveSubsystem, m_flightModule, PathPlannerConstants.kLanes, + swerveSubsystem::getIsGpsAssist)); + + // Toggle gps-based driving assist features + m_leftFlightStick.B5().onTrue(new InstantCommand(() -> { + swerveSubsystem.setGpsAssist(!swerveSubsystem.getIsGpsAssist()); + Logger.recordOutput("SwerveSubsystem/GPSAssistFeaturesEnabled", swerveSubsystem.getIsGpsAssist()); + })); + // Reset gyro rotation of the swerve dynamically m_rightFlightStick .B5() .onTrue(swerveSubsystem.runOnce(() -> { - swerveSubsystem.resetGyro(0); + swerveSubsystem.resetDriverRelative(); })); + + // Reset gyro rotation everywhere (including backend with button) + m_operatorPanel.blackButton().whileTrue(Commands.run(() -> { + var position = GlobalPosition.Get(); + if (position != null) { + UnifiedGyro.GetInstance().resetRotation(position.getRotation()); + } + })).onTrue(new InstantCommand(() -> { + Logger.recordOutput("UnifiedGyro/ResettingRotation", false); + })).onFalse(new InstantCommand(() -> { + Logger.recordOutput("UnifiedGyro/ResettingRotation", true); + })); + Logger.recordOutput("UnifiedGyro/ResettingRotation", false); + } + + private void setTurretCommands() { + var continuousAimCommand = new ContinuousAimCommand( + () -> AimPoint.getTarget()); + + var manualAimCommand = new ManualAimCommand( + () -> ManualAimCommand.ReverseDirection(m_operatorPanel.getWheel())); + + TurretSubsystem.GetInstance().setDefaultCommand(Commands.either( + continuousAimCommand, + manualAimCommand, + TurretSubsystem::getIsGpsAssistEnabled)); + + m_operatorPanel.greenButton().onTrue(new InstantCommand(() -> { + TurretSubsystem.setGpsAssistEnabled(!TurretSubsystem.getIsGpsAssistEnabled()); + ShooterSubsystem.setGpsAssistEnabled(!ShooterSubsystem.getIsGpsAssistEnabled()); + + var current = TurretSubsystem.GetInstance().getCurrentCommand(); + if (current != null) { + current.cancel(); + } + + var currentShooterCommand = ShooterSubsystem.GetInstance().getCurrentCommand(); + if (currentShooterCommand != null) { + currentShooterCommand.cancel(); + } + + Logger.recordOutput("TurretSubsystem/GPSAssistFeaturesEnabled", TurretSubsystem.getIsGpsAssistEnabled()); + Logger.recordOutput("ShooterSubsystem/GPSAssistFeaturesEnabled", ShooterSubsystem.getIsGpsAssistEnabled()); + })); + + NamedCommands.registerCommand("ContinuousAimCommand", new ContinuousAimCommand(() -> AimPoint.getTarget())); + } + + private void setClimberCommands() { + // TODO: Implement climber commands + // NamedCommands.registerCommand("ClimberL1Command", new + // ClimberCommand(climberSubsystem, () -> + // m_rightFlightStick.trigger().getAsBoolean())); + } + + private void setIntakeCommands() { + IntakeSubsystem intakeSubsystem = IntakeSubsystem.GetInstance(); + IntakeCommand intakeCommand = new IntakeCommand(intakeSubsystem, + () -> m_operatorPanel.metalSwitchDown().getAsBoolean() || m_rightFlightStick.trigger().getAsBoolean(), + () -> m_rightFlightStick.B17().getAsBoolean()); + + intakeSubsystem + .setDefaultCommand(intakeCommand); + + m_operatorPanel.toggleWheelMiddle().onTrue(new InstantCommand(() -> { + intakeCommand.setAlternateRaiseLocation(WristRaiseLocation.TOP); + })); + m_operatorPanel.toggleWheelMidDown().onTrue(new InstantCommand(() -> { + intakeCommand.setAlternateRaiseLocation(WristRaiseLocation.MIDDLE); + })); + + NamedCommands.registerCommand("IntakeCommand", + new IntakeCommand(intakeSubsystem, () -> true, + () -> false)); + } + + private void setShooterCommands() { + var continuousShooter = new ContinuousShooter(() -> AimPoint.getTarget()); + var continuousManualShooter = new ContinuousManualShooter( + ContinuousManualShooter.GetBaseSpeedSupplier(m_rightFlightStick::getRightSlider)); + + // Enable shooter with metal switch down. While up, run motor base speed. + // When enabled, run indexer only when shooter up to speed. + m_operatorPanel.metalSwitchDown() + .whileTrue(Commands.either( + continuousShooter, + continuousManualShooter, + ShooterSubsystem::getIsGpsAssistEnabled)) + .whileFalse(new InstantCommand(() -> { + ShooterSubsystem.GetInstance().runMotorBaseSpeed(); + })); + + NamedCommands.registerCommand("ContinuousShooterCommand", new ContinuousShooter(() -> AimPoint.getTarget())); } public Command getAutonomousCommand() { - return new PathPlannerAuto(kPathPlannerAutoName); + return PathPlannerSubsystem.GetInstance().getAndInitAutoCommand(true); } public void onAnyModeStart() { - var position = GlobalPosition.Get(); - if (position != null) { - AHRSGyro.GetInstance().setAngleAdjustment(position.getRotation().getDegrees()); - OdometrySubsystem.GetInstance().setOdometryPosition(position); + PublicationSubsystem.ClearAll(); + var globalPosition = GlobalPosition.Get(); + if (globalPosition != null) { + UnifiedGyro.GetInstance().resetRotation(globalPosition.getRotation()); + OdometrySubsystem.GetInstance().setOdometryPosition(globalPosition); } - if (BotConstants.currentMode == BotConstants.Mode.REAL) { - PublicationSubsystem.addDataClasses( - OdometrySubsystem.GetInstance(), - AHRSGyro.GetInstance()); - } + UnifiedGyro.Register(); + PublicationSubsystem.addDataClass(OdometrySubsystem.GetInstance()); } } diff --git a/src/main/java/frc/robot/command/SwerveMoveTeleop.java b/src/main/java/frc/robot/command/SwerveMoveTeleop.java index dcc34a2..b26c36d 100644 --- a/src/main/java/frc/robot/command/SwerveMoveTeleop.java +++ b/src/main/java/frc/robot/command/SwerveMoveTeleop.java @@ -1,12 +1,27 @@ package frc.robot.command; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; import org.pwrup.util.Vec2; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constant.ControllerConstants; -import frc.robot.constant.swerve.SwerveConstants; +import frc.robot.subsystem.GlobalPosition; import frc.robot.subsystem.SwerveSubsystem; -import frc.robot.util.CustomMath; +import frc.robot.util.LocalMath; +import lombok.Getter; +import lombok.Setter; import pwrup.frc.core.controller.FlightModule; import pwrup.frc.core.controller.FlightStick; @@ -15,44 +30,241 @@ */ public class SwerveMoveTeleop extends Command { + public enum AxisConstraint { + X, + Y, + XY; + + public Translation2d getConversion() { + return switch (this) { + case X -> new Translation2d(1, 0); + case Y -> new Translation2d(0, 1); + case XY -> new Translation2d(1, 1); + }; + } + } + + /** + * Lane segment: pose = center, length = total length (extends length/2 each + * way). + */ + public record Lane(Pose2d pose, double length, AxisConstraint axisConstant) { + public void log() { + Translation2d direction = new Translation2d(1, 0).rotateBy(pose.getRotation()); + Translation2d half = direction.times(length / 2); + Pose2d startPose = new Pose2d(pose.getTranslation().minus(half), pose.getRotation()); + Pose2d endPose = new Pose2d(pose.getTranslation().plus(half), pose.getRotation()); + Logger.recordOutput("SwerveMoveTeleop/Lane/Endpoints", new Pose2d[] { startPose, endPose }); + Logger.recordOutput("SwerveMoveTeleop/Lane/Length", length); + } + + public Pose2d nearestPoint(Pose2d otherPose) { + Pose2d lanePose = this.pose(); + double laneLength = this.length(); + + Translation2d dir = new Translation2d(1, 0).rotateBy(lanePose.getRotation()); + Translation2d a = lanePose.getTranslation().minus(dir.times(laneLength / 2)); + Translation2d ab = dir.times(laneLength); + Translation2d p = otherPose.getTranslation(); + Translation2d ap = p.minus(a); + double ab2 = ab.getX() * ab.getX() + ab.getY() * ab.getY(); + if (ab2 < 1e-9) { + return new Pose2d(a, lanePose.getRotation()); + } + double t = (ap.getX() * ab.getX() + ap.getY() * ab.getY()) / ab2; + t = Math.max(0, Math.min(1, t)); + Translation2d closest = a.plus(ab.times(t)); + return new Pose2d(closest, lanePose.getRotation()); + } + } + private final SwerveSubsystem m_swerveSubsystem; private final FlightModule controller; + private final List lanes; + + private final PIDController lanePullPid; + private ChassisSpeeds filteredPercent = new ChassisSpeeds(); + + private static final double kLanePullMaxSpeed = 0.4; + private static final Distance minDistance = Distance.ofRelativeUnits(0.8, Units.Meters); + + private static final double kNearLaneJoystickAlpha = 0.2; + private static final double kFarLaneJoystickAlpha = 0.6; + + private static final double kMaxPerpendicularScale = 1.0; + private static final double kMinPerpendicularScale = 0.2; + + private Supplier areGpsFeaturesEnabled; + public SwerveMoveTeleop( SwerveSubsystem swerveSubsystem, FlightModule controller) { + this(swerveSubsystem, controller, new ArrayList<>(), () -> true); + } + + public SwerveMoveTeleop( + SwerveSubsystem swerveSubsystem, + FlightModule controller, + Lane[] lanes, Supplier shouldAdjustVelocity) { + this(swerveSubsystem, controller, Arrays.asList(lanes), shouldAdjustVelocity); + } + + public SwerveMoveTeleop( + SwerveSubsystem swerveSubsystem, + FlightModule controller, + List lanes, Supplier shouldAdjustVelocity) { this.m_swerveSubsystem = swerveSubsystem; this.controller = controller; + this.lanes = lanes; + this.lanePullPid = new PIDController(0.5, 0, 0); + this.areGpsFeaturesEnabled = shouldAdjustVelocity; addRequirements(m_swerveSubsystem); } @Override public void execute() { - double r = CustomMath.deadband( + double rawR = LocalMath.deadband( controller.leftFlightStick.getRawAxis( - FlightStick.AxisEnum.JOYSTICKROTATION.value) * -1, + FlightStick.AxisEnum.JOYSTICKROTATION.value), ControllerConstants.kRotDeadband, ControllerConstants.kRotMinValue); - double x = CustomMath.deadband( + double rawX = LocalMath.deadband( controller.rightFlightStick.getRawAxis( FlightStick.AxisEnum.JOYSTICKY.value), ControllerConstants.kXSpeedDeadband, ControllerConstants.kXSpeedMinValue); - double y = CustomMath.deadband( + double rawY = LocalMath.deadband( controller.rightFlightStick.getRawAxis( FlightStick.AxisEnum.JOYSTICKX.value), ControllerConstants.kYSpeedDeadband, ControllerConstants.kYSpeedMinValue); - var velocity = SwerveSubsystem.fromPercentToVelocity(new Vec2(x, y), r); + var velocity = SwerveSubsystem.fromPercentToVelocity( + new Vec2(rawX, rawY), + rawR); + + if (areGpsFeaturesEnabled.get()) { + velocity = applyGpsFeatures(velocity); + } - m_swerveSubsystem.drive(velocity, SwerveSubsystem.DriveType.GYRO_RELATIVE); + m_swerveSubsystem.drive(velocity, SwerveSubsystem.DriveType.FIELD_RELATIVE); + } + + private ChassisSpeeds applyGpsFeatures(ChassisSpeeds rawVelocityInput) { + rawVelocityInput = adjustVelocityForLane(rawVelocityInput); + return rawVelocityInput; + } + + private ChassisSpeeds adjustVelocityForLane(ChassisSpeeds rawVelocityInput) { + Pose2d currentPose = GlobalPosition.Get(); + if (currentPose == null) { + return rawVelocityInput; + } + + Lane nearestLane = getNearestLanePoint(currentPose, minDistance); + if (nearestLane == null) { + return rawVelocityInput; + } + + Pose2d nearestLanePoint = nearestLane.nearestPoint(currentPose); + double distanceRatio = MathUtil.clamp( + currentPose.getTranslation().getDistance(nearestLanePoint.getTranslation()) / minDistance.in(Units.Meters), + 0, + 1); + + rawVelocityInput = smoothVelocity(rawVelocityInput.vxMetersPerSecond, rawVelocityInput.vyMetersPerSecond, + rawVelocityInput.omegaRadiansPerSecond, distanceRatio); + + Logger.recordOutput("SwerveMoveTeleop/NearestLanePoint", nearestLanePoint); + nearestLane.log(); + + Translation2d lanePullVelocity = getLanePull(nearestLanePoint, currentPose, nearestLane.axisConstant()); + Translation2d laneDriveScale = getLaneDriveScale(distanceRatio, nearestLane.axisConstant()); + + rawVelocityInput.vxMetersPerSecond *= laneDriveScale.getX() == 0 ? 1.0 : laneDriveScale.getX(); + rawVelocityInput.vyMetersPerSecond *= laneDriveScale.getY() == 0 ? 1.0 : laneDriveScale.getY(); + + rawVelocityInput.vxMetersPerSecond += lanePullVelocity.getX(); + rawVelocityInput.vyMetersPerSecond += lanePullVelocity.getY(); + + return rawVelocityInput; + } + + private ChassisSpeeds smoothVelocity(double x, double y, double r, double distanceRatio) { + double laneSmoothingAlpha = MathUtil.interpolate(kNearLaneJoystickAlpha, kFarLaneJoystickAlpha, distanceRatio); + + filteredPercent.vxMetersPerSecond = MathUtil.interpolate(filteredPercent.vxMetersPerSecond, x, laneSmoothingAlpha); + filteredPercent.vyMetersPerSecond = MathUtil.interpolate(filteredPercent.vyMetersPerSecond, y, laneSmoothingAlpha); + filteredPercent.omegaRadiansPerSecond = MathUtil.interpolate(filteredPercent.omegaRadiansPerSecond, r, + laneSmoothingAlpha); + + return new ChassisSpeeds( + filteredPercent.vxMetersPerSecond, + filteredPercent.vyMetersPerSecond, + filteredPercent.omegaRadiansPerSecond); } @Override public void end(boolean interrupted) { m_swerveSubsystem.stop(); } + + /** + * Returns a velocity vector (m/s) that pulls the robot toward the nearest lane + * point, independent of lane orientation. + */ + private Translation2d getLanePull(Pose2d nearestLanePoint, Pose2d relativeTo, AxisConstraint axisConstraint) { + Translation2d error = nearestLanePoint.getTranslation().minus(relativeTo.getTranslation()); + double errorDistance = error.getNorm(); + if (errorDistance < 1e-6) { + return new Translation2d(); + } + + double pullSpeed = MathUtil.clamp( + Math.abs(lanePullPid.calculate(errorDistance, 0)), + 0, + kLanePullMaxSpeed); + Translation2d pullDirection = error.div(errorDistance); + + Translation2d conversionFactor = axisConstraint.getConversion(); + pullDirection = pullDirection.times(pullSpeed); + + return new Translation2d( + pullDirection.getX() * conversionFactor.getX(), + pullDirection.getY() * conversionFactor.getY()); + } + + private Translation2d getLaneDriveScale(double distanceRatio, AxisConstraint axisConstraint) { + double constrainedScale = MathUtil.interpolate(kMinPerpendicularScale, kMaxPerpendicularScale, distanceRatio); + Translation2d conversionFactor = axisConstraint.getConversion(); + return new Translation2d( + constrainedScale * conversionFactor.getX(), + constrainedScale * conversionFactor.getY()); + } + + public Lane getNearestLanePoint(Pose2d relativeTo, Distance distanceLimit) { + if (relativeTo == null) { + return null; + } + + Lane nearestLane = null; + double nearestDistance = Double.POSITIVE_INFINITY; + double maxDistanceMeters = distanceLimit.in(Units.Meters); + + for (Lane lane : lanes) { + Pose2d lanePoint = lane.nearestPoint(relativeTo); + double laneDistance = lanePoint.getTranslation().getDistance(relativeTo.getTranslation()); + if (laneDistance < maxDistanceMeters) { + if (laneDistance < nearestDistance) { + nearestLane = lane; + nearestDistance = laneDistance; + } + } + } + + return nearestLane; + } } diff --git a/src/main/java/frc/robot/command/climber/ExecuteClimbSequence.java b/src/main/java/frc/robot/command/climber/ExecuteClimbSequence.java new file mode 100644 index 0000000..89edead --- /dev/null +++ b/src/main/java/frc/robot/command/climber/ExecuteClimbSequence.java @@ -0,0 +1,29 @@ +package frc.robot.command.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.ClimberConstants.ClimberPosition; +import frc.robot.subsystem.ClimberSubsystem; + +public class ExecuteClimbSequence extends Command { + + private final ClimberSubsystem climberSubsystem; + private final ClimberPosition[] positions; + + public ExecuteClimbSequence(ClimberSubsystem climberSubsystem, ClimberPosition[] positions) { + this.climberSubsystem = climberSubsystem; + addRequirements(climberSubsystem); + this.positions = positions; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + for (ClimberPosition position : positions) { + climberSubsystem.setHeight(position.positionMoveSequence[0]); + climberSubsystem.setHeight(position.positionMoveSequence[1]); + } + } +} diff --git a/src/main/java/frc/robot/command/intake/IntakeCommand.java b/src/main/java/frc/robot/command/intake/IntakeCommand.java new file mode 100644 index 0000000..4c75d59 --- /dev/null +++ b/src/main/java/frc/robot/command/intake/IntakeCommand.java @@ -0,0 +1,53 @@ +package frc.robot.command.intake; + +import java.util.function.Supplier; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.IntakeConstants; +import frc.robot.constant.IntakeConstants.WristRaiseLocation; +import frc.robot.subsystem.IntakeSubsystem; + +public class IntakeCommand extends Command { + + private final IntakeSubsystem m_intakeSubsystem; + + private final Supplier joystickSupplier; + private final Supplier extakeOverrideSupplier; + private WristRaiseLocation alternateRaiseLocation = WristRaiseLocation.MIDDLE; + + public void setAlternateRaiseLocation(WristRaiseLocation location) { + alternateRaiseLocation = location; + } + + public IntakeCommand(IntakeSubsystem baseSubsystem, Supplier joystickSupplier, + Supplier extakeOverrideSupplier) { + m_intakeSubsystem = baseSubsystem; + this.joystickSupplier = joystickSupplier; + this.extakeOverrideSupplier = extakeOverrideSupplier; + addRequirements(m_intakeSubsystem); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + boolean joystickValue = joystickSupplier.get(); + if (joystickValue) { + m_intakeSubsystem.setWristPosition(WristRaiseLocation.BOTTOM); + m_intakeSubsystem + .runIntakeMotor( + extakeOverrideSupplier.get() ? IntakeConstants.extakeMotorSpeed : IntakeConstants.intakeMotorSpeed); + + } else { + m_intakeSubsystem + .setWristPosition(alternateRaiseLocation); + m_intakeSubsystem.stopIntakeMotor(); + } + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/lighting/AutonomousStateLighting.java b/src/main/java/frc/robot/command/lighting/AutonomousStateLighting.java new file mode 100644 index 0000000..4bb4969 --- /dev/null +++ b/src/main/java/frc/robot/command/lighting/AutonomousStateLighting.java @@ -0,0 +1,110 @@ +package frc.robot.command.lighting; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.LightsSubsystem; +import frc.robot.subsystem.PathPlannerSubsystem; +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.EffectHandle; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightsApi; + +public class AutonomousStateLighting extends Command { + private static final LedColor kChaseColor = new LedColor(255, 0, 0, 0); + private static final LedRange kAutonomousRangeL = new LedRange(15, 75); + private static final LedRange kAutonomousRangeR = new LedRange(90, 150); + private static final LedColor kSolidColor = new LedColor(0, 255, 0, 0); + + private static final double kHz = 70; + private static final int kWidth = 30; + + private final LightsApi lightsApi; + private EffectHandle chaseHandleL; + private EffectHandle solidHandleL; + + private EffectHandle chaseHandleR; + private EffectHandle solidHandleR; + + public AutonomousStateLighting() { + this(LightsSubsystem.GetInstance()); + } + + public AutonomousStateLighting(LightsSubsystem lightsSubsystem) { + super(); + this.lightsApi = lightsSubsystem; + } + + @Override + public void initialize() { + chaseHandleL = lightsApi.addChase( + kAutonomousRangeL, + kChaseColor, + kWidth, + kHz, + true, + 10, + BlendMode.OVERWRITE); + + solidHandleL = lightsApi.addSolid( + kAutonomousRangeL, + kSolidColor, + 20, + BlendMode.OVERWRITE); + + chaseHandleR = lightsApi.addChase( + kAutonomousRangeR, + kChaseColor, + kWidth, + kHz, + true, + 10, + BlendMode.OVERWRITE); + + solidHandleR = lightsApi.addSolid( + kAutonomousRangeR, + kSolidColor, + 20, + BlendMode.OVERWRITE); + } + + @Override + public void execute() { + if (PathPlannerSubsystem.GetInstance().currentAutoCommand == null) { + lightsApi.setEnabled(chaseHandleL, false); + lightsApi.setEnabled(chaseHandleR, false); + + lightsApi.setEnabled(solidHandleL, true); + lightsApi.setEnabled(solidHandleR, true); + + return; + } + + if (PathPlannerSubsystem.GetInstance().currentAutoCommand.isScheduled()) { + lightsApi.setEnabled(chaseHandleL, true); + lightsApi.setEnabled(solidHandleL, false); + + lightsApi.setEnabled(chaseHandleR, true); + lightsApi.setEnabled(solidHandleR, false); + } else { + lightsApi.setEnabled(chaseHandleL, false); + lightsApi.setEnabled(solidHandleL, true); + + lightsApi.setEnabled(chaseHandleR, false); + lightsApi.setEnabled(solidHandleR, true); + } + } + + @Override + public void end(boolean interrupted) { + if (chaseHandleL != null) { + lightsApi.removeEffect(chaseHandleL); + lightsApi.removeEffect(solidHandleL); + lightsApi.removeEffect(chaseHandleR); + lightsApi.removeEffect(solidHandleR); + chaseHandleL = null; + solidHandleL = null; + chaseHandleR = null; + solidHandleR = null; + } + } +} diff --git a/src/main/java/frc/robot/command/lighting/MorseCodeLighting.java b/src/main/java/frc/robot/command/lighting/MorseCodeLighting.java new file mode 100644 index 0000000..0105993 --- /dev/null +++ b/src/main/java/frc/robot/command/lighting/MorseCodeLighting.java @@ -0,0 +1,60 @@ +package frc.robot.command.lighting; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.LEDConstants; +import frc.robot.subsystem.LightsSubsystem; +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.EffectHandle; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightsApi; + +public class MorseCodeLighting extends Command { + private static final String kMessage = "Jay STOP SAYING THE N WORD!"; + private static final LedRange kRange = new LedRange(LEDConstants.onboardStartIndex, LEDConstants.ledEndIndex); + private static final LedColor kOnColor = new LedColor(0, 200, 255, 0); + private static final LedColor kOffColor = LedColor.BLACK; + private static final double kMorseUnitSeconds = 0.12; + private static final int kPriority = 50; + + private final LightsApi lightsApi; + private EffectHandle morseHandle; + + public MorseCodeLighting() { + this(LightsSubsystem.GetInstance()); + } + + public MorseCodeLighting(LightsSubsystem lightsSubsystem) { + super(); + this.lightsApi = lightsSubsystem; + } + + @Override + public void initialize() { + morseHandle = lightsApi.addMorseCode( + kRange, + kMessage, + kOnColor, + kOffColor, + kMorseUnitSeconds, + kPriority, + BlendMode.ADD); + + if (morseHandle != null) { + lightsApi.setInput(morseHandle, kMessage); + } + } + + @Override + public void execute() { + // Morse rendering is handled internally by the effect. + } + + @Override + public void end(boolean interrupted) { + if (morseHandle != null) { + lightsApi.removeEffect(morseHandle); + morseHandle = null; + } + } +} diff --git a/src/main/java/frc/robot/command/lighting/PulsingLightingCommand.java b/src/main/java/frc/robot/command/lighting/PulsingLightingCommand.java new file mode 100644 index 0000000..0b47122 --- /dev/null +++ b/src/main/java/frc/robot/command/lighting/PulsingLightingCommand.java @@ -0,0 +1,57 @@ +package frc.robot.command.lighting; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.LEDConstants; +import frc.robot.subsystem.LightsSubsystem; +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.EffectHandle; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightsApi; + +public class PulsingLightingCommand extends Command { + private static final int kPulseStartIndex = 150; + private static final LedRange kPulseRange = new LedRange(kPulseStartIndex, LEDConstants.ledEndIndex); + private static final LedColor kPulseColor = new LedColor(255, 100, 0, 0); + private static final double kHz = 5; + private static final double kMinScalar = 0.15; + private static final double kMaxScalar = 1.0; + private static final int kPriority = 15; + + private final LightsApi lightsApi; + private EffectHandle breatheHandle; + + public PulsingLightingCommand() { + this(LightsSubsystem.GetInstance()); + } + + public PulsingLightingCommand(LightsSubsystem lightsSubsystem) { + super(); + this.lightsApi = lightsSubsystem; + } + + @Override + public void initialize() { + breatheHandle = lightsApi.addBreathe( + kPulseRange, + kPulseColor, + kHz, + kMinScalar, + kMaxScalar, + kPriority, + BlendMode.OVERWRITE); + } + + @Override + public void execute() { + // Breathe runs on its own; no per-cycle updates needed. + } + + @Override + public void end(boolean interrupted) { + if (breatheHandle != null) { + lightsApi.removeEffect(breatheHandle); + breatheHandle = null; + } + } +} diff --git a/src/main/java/frc/robot/command/lighting/ShooterSpeedLighting.java b/src/main/java/frc/robot/command/lighting/ShooterSpeedLighting.java new file mode 100644 index 0000000..1d40d21 --- /dev/null +++ b/src/main/java/frc/robot/command/lighting/ShooterSpeedLighting.java @@ -0,0 +1,103 @@ +package frc.robot.command.lighting; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.ShooterConstants; +import frc.robot.subsystem.LightsSubsystem; +import frc.robot.subsystem.ShooterSubsystem; +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.EffectHandle; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightsApi; + +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; +import pwrup.frc.core.controller.FlightStick; + +/** + * Infinitely-running command that reads the speed slider and updates the + * shooter speed display. + */ +public class ShooterSpeedLighting extends Command { + private static final LedRange SHOOTER_BAR_RANGE = new LedRange(8, 67); + private static final LedColor TARGET_FILL_COLOR = new LedColor(0, 255, 0, 0); + private static final LedColor TARGET_EMPTY_COLOR = new LedColor(15, 15, 15, 0); + private static final LedColor ACTUAL_FILL_COLOR = new LedColor(100, 0, 255, 0); + + private static AngularVelocity targetShooterSpeed = ShooterConstants.kShooterMinVelocity; + + private final LightsApi lightsApi; + private final DoubleSupplier sliderSupplier; + private final DoubleSupplier actualShooterRpsSupplier; + + private EffectHandle targetBarHandle; + private EffectHandle actualBarHandle; + + public ShooterSpeedLighting(FlightStick flightStick) { + this(LightsSubsystem.GetInstance(), flightStick); + } + + public ShooterSpeedLighting(LightsSubsystem lightsSubsystem, FlightStick flightStick) { + this( + lightsSubsystem, + flightStick::getRightSlider, + () -> ShooterSubsystem.GetInstance().getCurrentShooterVelocity().in(Units.RotationsPerSecond)); + } + + ShooterSpeedLighting( + LightsApi lightsApi, + DoubleSupplier sliderSupplier, + DoubleSupplier actualShooterRpsSupplier) { + super(); + this.lightsApi = lightsApi; + this.sliderSupplier = sliderSupplier; + this.actualShooterRpsSupplier = actualShooterRpsSupplier; + } + + @Override + public void initialize() { + targetBarHandle = lightsApi.addProgressBar( + SHOOTER_BAR_RANGE, + TARGET_FILL_COLOR, + TARGET_EMPTY_COLOR, + 10, + BlendMode.OVERWRITE); + + actualBarHandle = lightsApi.addProgressBar( + SHOOTER_BAR_RANGE, + ACTUAL_FILL_COLOR, + null, + 20, + BlendMode.ADD); + } + + @Override + public void execute() { + double sliderPercent = Math.max(0, Math.min(1, (sliderSupplier.getAsDouble() + 1) / 2)); + double min = ShooterConstants.kShooterMinVelocity.in(Units.RotationsPerSecond); + double max = ShooterConstants.kShooterMaxVelocity.in(Units.RotationsPerSecond); + targetShooterSpeed = Units.RotationsPerSecond.of(min + (max - min) * sliderPercent); + + if (targetBarHandle != null) { + lightsApi.setProgress(targetBarHandle, sliderPercent); + } + + double actualPercent = actualShooterRpsSupplier.getAsDouble() / max; + if (actualBarHandle != null) { + lightsApi.setProgress(actualBarHandle, Math.max(0, Math.min(1, actualPercent))); + } + + Logger.recordOutput("Shooter/NeededSpeed", targetShooterSpeed); + } + + @Override + public boolean isFinished() { + return false; + } + + public static AngularVelocity getTargetShooterSpeed() { + return targetShooterSpeed; + } +} diff --git a/src/main/java/frc/robot/command/lighting/TurretStateLighting.java b/src/main/java/frc/robot/command/lighting/TurretStateLighting.java new file mode 100644 index 0000000..e404f61 --- /dev/null +++ b/src/main/java/frc/robot/command/lighting/TurretStateLighting.java @@ -0,0 +1,69 @@ +package frc.robot.command.lighting; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.command.shooting.ContinuousShooter; +import frc.robot.subsystem.LightsSubsystem; +import frc.robot.subsystem.TurretSubsystem; +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.EffectHandle; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightsApi; + +public class TurretStateLighting extends Command { + private static final int kMaxAimTimeMs = 50; + private static final int kInRangeThresholdMs = 20; + private static final LedColor kTargetColorRed = new LedColor(255, 0, 0, 0); + private static final LedColor kTargetColorGreen = new LedColor(0, 255, 0, 0); + private static final LedRange kTargetRange = new LedRange(75, 90); + + private final LightsApi lightsApi; + private final TurretSubsystem turretSubsystem; + + private EffectHandle redHandle; + private EffectHandle greenHandle; + + public TurretStateLighting() { + this(LightsSubsystem.GetInstance(), TurretSubsystem.GetInstance()); + } + + public TurretStateLighting(LightsSubsystem lightsSubsystem, TurretSubsystem turretSubsystem) { + super(); + this.lightsApi = lightsSubsystem; + this.turretSubsystem = turretSubsystem; + } + + @Override + public void initialize() { + redHandle = lightsApi.addConvergingArrows( + kTargetRange, + kTargetColorRed, + true, + 10, + BlendMode.OVERWRITE); + greenHandle = lightsApi.addConvergingArrows( + kTargetRange, + kTargetColorGreen, + true, + 10, + BlendMode.OVERWRITE); + } + + @Override + public void execute() { + lightsApi.setEnabled(redHandle, !ContinuousShooter.isShooting()); + lightsApi.setEnabled(greenHandle, ContinuousShooter.isShooting()); + } + + @Override + public void end(boolean interrupted) { + if (redHandle != null) { + lightsApi.removeEffect(redHandle); + redHandle = null; + } + if (greenHandle != null) { + lightsApi.removeEffect(greenHandle); + greenHandle = null; + } + } +} diff --git a/src/main/java/frc/robot/command/scoring/ContinuousAimCommand.java b/src/main/java/frc/robot/command/scoring/ContinuousAimCommand.java new file mode 100644 index 0000000..cb296fb --- /dev/null +++ b/src/main/java/frc/robot/command/scoring/ContinuousAimCommand.java @@ -0,0 +1,359 @@ +package frc.robot.command.scoring; + +import java.util.function.Supplier; + +import org.ejml.simple.SimpleMatrix; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.ShooterConstants; +import frc.robot.constant.TurretConstants; +import frc.robot.subsystem.GlobalPosition; +import frc.robot.subsystem.TurretSubsystem; +import frc.robot.subsystem.GlobalPosition.GMFrame; + +public class ContinuousAimCommand extends Command { + private final TurretSubsystem turretSubsystem; + private final Supplier targetGlobalPoseSupplier; + + public ContinuousAimCommand(Supplier targetGlobalPoseSupplier) { + this.turretSubsystem = TurretSubsystem.GetInstance(); + this.targetGlobalPoseSupplier = targetGlobalPoseSupplier; + addRequirements(this.turretSubsystem); + } + + @Override + public void execute() { + Pose2d selfPose = GlobalPosition.Get(); + Translation2d targetGlobal = targetGlobalPoseSupplier.get(); + ChassisSpeeds robotFieldSpeeds = GlobalPosition.Velocity(GMFrame.kFieldRelative); + + Pose2d targetInRobotFrame = new Pose2d(targetGlobal, new Rotation2d()).relativeTo(selfPose); + double distanceToTarget = targetInRobotFrame.getTranslation().getNorm(); + double flyTime = ShooterConstants.DistanceFromTargetToTime(distanceToTarget); + Translation2d compensatedTargetInRobot = GetCompensatedSpeed(selfPose, targetGlobal, robotFieldSpeeds); + Translation2d leadCompensation = compensatedTargetInRobot.minus(targetInRobotFrame.getTranslation()); + + double turretAngle = Math.atan2(compensatedTargetInRobot.getY(), compensatedTargetInRobot.getX()); + + double yawRateRadPerSec = robotFieldSpeeds.omegaRadiansPerSecond; + double ff = -yawRateRadPerSec * TurretConstants.kFFCommand; + + turretSubsystem.setTurretPosition(Units.Radians.of(turretAngle), Units.Volts.of(ff)); + + Logger.recordOutput("Turret/TargetGlobal", targetGlobal); + + Logger.recordOutput("Turret/DistanceToTarget", distanceToTarget); + Logger.recordOutput("Turret/FlyTime", flyTime); + + Logger.recordOutput("Turret/LeadCompensation", leadCompensation); + Logger.recordOutput("Turret/CompensatedTargetRobotRelative", compensatedTargetInRobot); + Logger.recordOutput("Turret/YawRateRadPerSec", yawRateRadPerSec); + Logger.recordOutput("Turret/Angle", turretAngle); + Logger.recordOutput("Turret/FF", ff); + } + + public static Translation2d GetCompensatedSpeed(Pose2d selfPose, Translation2d targetGlobal, + ChassisSpeeds robotFieldSpeeds) { + Pose2d targetInRobotFrame = new Pose2d(targetGlobal, new Rotation2d()).relativeTo(selfPose); + double distanceToTarget = targetInRobotFrame.getTranslation().getNorm(); + double flyTime = ShooterConstants.DistanceFromTargetToTime(distanceToTarget); + + double robotTheta = selfPose.getRotation().getRadians(); + double robotVXRobot = robotFieldSpeeds.vxMetersPerSecond * Math.cos(-robotTheta) + - robotFieldSpeeds.vyMetersPerSecond * Math.sin(-robotTheta); + double robotVYRobot = robotFieldSpeeds.vxMetersPerSecond * Math.sin(-robotTheta) + + robotFieldSpeeds.vyMetersPerSecond * Math.cos(-robotTheta); + + Translation2d leadCompensation = new Translation2d(-robotVXRobot * flyTime, -robotVYRobot * flyTime); + return targetInRobotFrame.getTranslation().plus(leadCompensation); + } + + // No longer needed in this form – lead compensation is now done inline. + + /* + * Translation2d velocity = currentRobotVelocitySupplier.get(); + * + * double t_f = calculateTf(target.getNorm(), targetGlobal.getZ(), + * TurretConstants.kTurretTheta); + * + * // Predict where the target appears in the robot frame at impact time: + * // T_new = target - v_r * t_f - L + * // T_robot = R(w * t_f) * T_new + * // ------------------------------------------------------------ + * Translation2d T_new = target + * .minus(velocity.times(t_f)) + * .minus(TurretConstants.turretPositionInRobot); + * + * Matrix R = fromAngularVelToMat(currentRobotYawVelocitySupplier.get(), + * t_f); + * Matrix T_robot = R.times(T_new.toVector()); + * Translation2d aimPoint = new Translation2d(T_robot.get(0, 0), T_robot.get(1, + * 0)); + * // ------------------------------------------------------------ + * + * Angle newAngle = Units.Radians.of(aimPoint.getAngle().getRadians()); + * + * AngularVelocity newAngleRate = calculateAimAngleRate(target.getNorm(), + * targetGlobal.getZ(), + * TurretConstants.kTurretTheta, velocity, + * target, TurretConstants.turretPositionInRobot, + * currentRobotAccelerationSupplier.get(), + * currentRobotYawVelocitySupplier.get(), + * currentRobotYawAccelerationSupplier.get()); + * + * double feedForwardV = newAngleRate.in(Units.RadiansPerSecond) * + * TurretConstants.feedForwardFactor; + * + * turretSubsystem.setTurretPosition(newAngle, Units.Volts.of(feedForwardV)); + * + * logEverything(selfPose, targetGlobal, target, velocity, aimPoint, newAngle); + */ + + private Matrix fromAngularVelToMat(AngularVelocity w, double time) { + double delta = w.in(Units.RadiansPerSecond) * time; + + return new Matrix(new SimpleMatrix(new double[][] { + { Math.cos(delta), -Math.sin(delta) }, + { Math.sin(delta), Math.cos(delta) } + })); + } + + private double calculateTf(double X, double Y, Angle theta) { + return Math.sqrt((X * Math.tan(theta.in(Units.Radians)) - Y) / 4.9); + } + + private double sec(double x) { + return 1 / Math.cos(x); + } + + /** + * Computes the instantaneous angular rate (time derivative) of the + * predicted impact-relative aim direction to a target. + * + *

+ * This method exists to answer: "If I fire right now, how fast is my + * required aim angle changing right now?" It differentiates the predicted + * impact-relative 2D vector and then converts that vector derivative into an + * angular derivative {@code d(alpha)/dt}. + *

+ * + *

What was added / what this accounts for

+ *
    + *
  • Ballistic time-of-flight {@code t_f} (simplified no-drag model) + * and its derivative {@code d(t_f)/dt}.
    • + *
  • Robot translation during flight: subtracts {@code v_r * t_f} and + * includes translational acceleration {@code a_r} when differentiating.
    • + *
  • Robot yaw rotation during flight: rotates by + * {@code delta = w * t_f} + * and includes both yaw-rate {@code w} and yaw angular acceleration {@code a} + * in {@code d(delta)/dt}.
    • + *
  • Turret/launcher offset {@code L} from the robot reference + * point.
    • + *
+ * + *

+ * Conceptually this method differentiates, with respect to time at the instant + * of firing, the + * following predicted relative vector (expressed in the robot frame) at the + * moment the projectile + * reaches the target range: + *

+ * + * 
+   * T_new   = target - v_r * t_f - L
+   * delta   = w * t_f
+   * T_robot = R(delta) * T_new
+   * alpha   = atan2(T_robot.y, T_robot.x)
+   * return d(alpha)/dt
+   *
+ * + *

+ * where {@code t_f} is the flight time derived from a simplified ballistic + * model (no drag, constant + * gravity) and {@code R(delta)} is the 2D rotation matrix for the yaw change + * that occurs over the + * flight time. + *

+ * + *

Ballistic model

+ *

+ * The flight time is computed from the vertical displacement equation: + *

+ * + * 
+   * Y = X * tan(theta) - (g / 2) * t_f ^ 2
+   *
+ * + *

+ * using {@code g/2 = 4.9} (i.e., {@code g ≈ 9.8 m/s^2}) and solving for + * {@code t_f}: + *

+ * + * 
+   * t_f = sqrt((X * tan(theta) - Y) / 4.9)
+   *
+ * + *

+ * The method then computes {@code d_t_f} (the time-derivative of flight time) + * via the chain rule, + * using the current robot translational velocity components ({@code vx, vy}) + * and the derivative + * of {@code tan(theta)} w.r.t. time (via {@code sec^2(theta)} terms) as encoded + * in the expression. + *

+ * + *

Frames, sign conventions, and units

+ *
    + *
  • {@code X}, {@code Y}: scalar components used by the ballistic + * model. In typical usage, + * {@code X} is horizontal range to the target (meters) and {@code Y} is + * vertical height difference + * (meters, positive up). They must be consistent with {@code theta}.
    • + *
  • {@code theta}: launch/elevation angle (radians) used in the + * ballistic equation.
    • + *
  • {@code target}: 2D target translation in the robot's reference + * frame at the current + * time (meters).
    • + *
  • {@code L}: 2D translation from robot reference point to + * launcher/turret origin (meters).
    • + *
  • {@code v_r}: robot translational velocity in the same 2D frame as + * {@code target} (m/s).
    • + *
  • {@code a_r}: robot translational acceleration in the same 2D frame + * as {@code target} (m/s^2).
    • + *
  • {@code w}: robot yaw rate (rad/s). Positive direction must match + * the rotation used + * elsewhere in the codebase.
    • + *
  • {@code a}: robot yaw angular acceleration (rad/s^2).
    • + *
+ * + *

Return value

+ *

+ * Returns {@code d(alpha)/dt}, the instantaneous time-derivative of the aim + * angle {@code alpha}. + *

+ * + *

+ * Important: This method returns an {@link Angle} object for + * convenience, + * but the numeric value represents an angular rate (units of + * radians/second), not a static angle. If you use this in feedforward, treat + * {@code result.in(Units.Radians)} as {@code rad/s}. + *

+ * + * @param X + * Horizontal range used by the ballistic model (meters). + * @param Y + * Vertical displacement used by the ballistic model (meters, + * positive up). + * @param theta + * Launch/elevation angle used by the ballistic model (radians). + * @param v_r + * Robot translational velocity in the same 2D frame as + * {@code target} (m/s). + * @param target + * Target translation in the robot frame at the current instant + * (meters). + * @param L + * Offset from robot reference point to launcher/turret origin + * (meters). + * @param a_r + * Robot translational acceleration in the same 2D frame as + * {@code target} (m/s^2). + * @param w + * Robot yaw angular velocity (rad/s). + * @param a + * Robot yaw angular acceleration (rad/s^2). + * @return An {@link Angle} whose numeric value (in radians) should be + * interpreted + * as {@code d(alpha)/dt} in radians/second. + * + *

Numerical / domain caveats

+ *
    + *
  • Sqrt domain: requires + * {@code (X * tan(theta) - Y) / 4.9 >= 0}. If + * not, {@code t_f} + * becomes NaN.
    • + *
  • Sensitivity: when {@code X * tan(theta) ≈ Y}, {@code t_f} + * approaches 0 and + * {@code d_t_f} can become very large (division by a small sqrt + * term).
    • + *
  • Model limitations: ignores aerodynamic drag, spin, and + * changes in + * projectile speed; + * assumes robot velocities/accelerations are approximately constant + * over the + * flight.
    • + *
+ */ + @SuppressWarnings("unused") + private AngularVelocity calculateAimAngleRate( + double X, double Y, Angle theta, Translation2d v_r, Translation2d target, + Translation2d L, Translation2d a_r, AngularVelocity w, AngularAcceleration a) { + double t_f = calculateTf(X, Y, theta); + // double v_b = X / (Math.cos(theta.in(Units.Radians)) * t_f); + + double vx = v_r.getX(); + double vy = v_r.getY(); + + double d_t_f = (1 / (2 * Math.sqrt((X * Math.tan(theta.in(Units.Radians)) - Y) / 4.9))) + * (((vx * Math.tan(theta.in(Units.Radians))) + + (X * Math.pow(sec(theta.in(Units.Radians)), 2)) - vy) + / 4.9); + + Vector T_v = v_r.toVector().times(-1); + Vector A_r = a_r.toVector(); + + Vector T_new = target.toVector().minus(v_r.toVector().times(t_f)).minus(L.toVector()); + Vector d_T_new = T_v.minus(A_r).minus(v_r.toVector().times(d_t_f)); + + double delta = w.in(Units.RadiansPerSecond) * t_f; + double d_delta = a.in(Units.RadiansPerSecondPerSecond) * t_f + w.in(Units.RadiansPerSecond) * d_t_f; + + Matrix R = new Matrix(new SimpleMatrix(new double[][] { + { Math.cos(delta), -Math.sin(delta) }, + { Math.sin(delta), Math.cos(delta) } + })); + + Matrix d_R = new Matrix(new SimpleMatrix(new double[][] { + { -Math.sin(delta) * d_delta, -Math.cos(delta) * d_delta }, + { Math.cos(delta) * d_delta, -Math.sin(delta) * d_delta } + })); + + Matrix T_new_R_mat = R.times(T_new); + Vector T_new_R = new Vector(new SimpleMatrix(new double[][] { + { T_new_R_mat.get(0, 0) }, + { T_new_R_mat.get(1, 0) } + })); + + Matrix d_T_new_r_mat = d_R.times(T_new).plus(R.times(d_T_new)); + + Vector d_T_new_r = new Vector(new SimpleMatrix(new double[][] { + { d_T_new_r_mat.get(0, 0) }, + { d_T_new_r_mat.get(1, 0) } + })); + + double T_new_R_x = T_new_R.get(0, 0); + double T_new_R_y = T_new_R.get(1, 0); + + double d_T_new_R_x = d_T_new_r.get(0, 0); + double d_T_new_R_y = d_T_new_r.get(1, 0); + + double d_alpha_dt = (1 / (1 + Math.pow(T_new_R_y / T_new_R_x, 2))) + * ((T_new_R_x * d_T_new_R_y - T_new_R_y * d_T_new_R_x) / Math.pow(T_new_R_x, 2)); + + return Units.RadiansPerSecond.of(d_alpha_dt); + } +} diff --git a/src/main/java/frc/robot/command/scoring/ManualAimCommand.java b/src/main/java/frc/robot/command/scoring/ManualAimCommand.java new file mode 100644 index 0000000..b1925aa --- /dev/null +++ b/src/main/java/frc/robot/command/scoring/ManualAimCommand.java @@ -0,0 +1,74 @@ +package frc.robot.command.scoring; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.ShooterConstants; +import frc.robot.constant.TurretConstants; +import frc.robot.subsystem.TurretSubsystem; + +public class ManualAimCommand extends Command { + private final TurretSubsystem turretSubsystem; + private final DoubleSupplier joystickAxisSupplier; + private final double deadband; + + public ManualAimCommand(DoubleSupplier joystickAxisSupplier) { + this(TurretSubsystem.GetInstance(), joystickAxisSupplier); + } + + public ManualAimCommand(TurretSubsystem turretSubsystem, DoubleSupplier joystickAxisSupplier) { + this(turretSubsystem, joystickAxisSupplier, 0.05); + } + + public ManualAimCommand( + TurretSubsystem turretSubsystem, + DoubleSupplier joystickAxisSupplier, + double deadband) { + this.turretSubsystem = turretSubsystem; + this.joystickAxisSupplier = joystickAxisSupplier; + this.deadband = deadband; + addRequirements(turretSubsystem); + } + + @Override + public void execute() { + double rawAxis = joystickAxisSupplier.getAsDouble(); + double axis = MathUtil.applyDeadband(rawAxis, deadband); + double clampedAxis = MathUtil.clamp(axis, -1.0, 1.0); + + double minRotations = TurretConstants.kTurretMinAngle.in(Units.Rotations); + double maxRotations = TurretConstants.kTurretMaxAngle.in(Units.Rotations); + double targetRotations = MathUtil.interpolate(minRotations, maxRotations, (-clampedAxis + 1.0) / 2.0); + + turretSubsystem.setTurretPosition( + Units.Rotations.of(targetRotations), + Units.Volts.of(0.0)); + + Logger.recordOutput("Turret/ManualAimCommand/TargetRotations", targetRotations); + Logger.recordOutput("Turret/ManualAimCommand/TargetRaw", rawAxis); + } + + @Override + public void initialize() { + // No state needed. Setpoint is directly recomputed from joystick every loop. + } + + @Override + public boolean isFinished() { + return false; + } + + public static double ReverseDirection(double speed) { + if (speed > 0) { + return 1 - speed; + } else { + return -1 + Math.abs(speed); + } + } +} diff --git a/src/main/java/frc/robot/command/shooting/ContinuousManualShooter.java b/src/main/java/frc/robot/command/shooting/ContinuousManualShooter.java new file mode 100644 index 0000000..d313dde --- /dev/null +++ b/src/main/java/frc/robot/command/shooting/ContinuousManualShooter.java @@ -0,0 +1,70 @@ +package frc.robot.command.shooting; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.ShooterConstants; +import frc.robot.subsystem.IndexSubsystem; +import frc.robot.subsystem.ShooterSubsystem; +import lombok.Getter; + +/** + * Shooter command with manual speed: sets shooter velocity from a supplier + * (e.g. joystick axis) and runs the index when the shooter is up to speed. + * Does not check turret aim; use with ManualAimCommand for full manual control. + */ +public class ContinuousManualShooter extends Command { + private final ShooterSubsystem shooterSubsystem; + private final IndexSubsystem indexSubsystem; + private final Supplier speedSupplier; + + @Getter + private static boolean isShooting = false; + + public ContinuousManualShooter(Supplier speedSupplier) { + this.speedSupplier = speedSupplier; + this.shooterSubsystem = ShooterSubsystem.GetInstance(); + this.indexSubsystem = IndexSubsystem.GetInstance(); + addRequirements(this.shooterSubsystem, this.indexSubsystem); + } + + @Override + public void execute() { + Logger.recordOutput("ContinuousManualShooter/Time", System.currentTimeMillis()); + AngularVelocity speed = speedSupplier.get(); + shooterSubsystem.setShooterVelocity(speed); + + if (!shooterSubsystem.isShooterSpunUp()) { + isShooting = false; + indexSubsystem.stopMotor(); + return; + } + + isShooting = true; + indexSubsystem.runMotor(); + } + + @Override + public void end(boolean interrupted) { + isShooting = false; + shooterSubsystem.runMotorBaseSpeed(); + indexSubsystem.stopMotor(); + } + + public static Supplier GetBaseSpeedSupplier(Supplier sliderSupplier) { + return () -> { + double sliderRaw = sliderSupplier.get(); + double slider = MathUtil.clamp((sliderRaw + 1.0) / 2.0, 0.0, 1.0); + double rps = MathUtil.interpolate( + ShooterConstants.kShooterMinVelocity.in(Units.RotationsPerSecond), + ShooterConstants.kShooterMaxVelocity.in(Units.RotationsPerSecond), + slider); + return Units.RotationsPerSecond.of(rps); + }; + } +} diff --git a/src/main/java/frc/robot/command/shooting/ContinuousShooter.java b/src/main/java/frc/robot/command/shooting/ContinuousShooter.java new file mode 100644 index 0000000..d330add --- /dev/null +++ b/src/main/java/frc/robot/command/shooting/ContinuousShooter.java @@ -0,0 +1,92 @@ +package frc.robot.command.shooting; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.command.scoring.ContinuousAimCommand; +import frc.robot.constant.ShooterConstants; +import frc.robot.constant.TurretConstants; +import frc.robot.subsystem.GlobalPosition; +import frc.robot.subsystem.GlobalPosition.GMFrame; +import frc.robot.subsystem.IndexSubsystem; +import frc.robot.subsystem.ShooterSubsystem; +import frc.robot.subsystem.TurretSubsystem; +import frc.robot.util.LocalMath; +import lombok.Getter; + +public class ContinuousShooter extends Command { + private final Supplier targetGlobalPoseSupplier; + private final Supplier selfGlobalPoseSupplier; + private final ShooterSubsystem shooterSubsystem; + private final TurretSubsystem turretSubsystem; + private final IndexSubsystem indexSubsystem; + + @Getter + private static boolean isShooting = false; + + public ContinuousShooter(Supplier targetGlobalPoseSupplier, + Supplier selfGlobalPoseSupplier) { + this.targetGlobalPoseSupplier = targetGlobalPoseSupplier; + this.selfGlobalPoseSupplier = selfGlobalPoseSupplier; + this.shooterSubsystem = ShooterSubsystem.GetInstance(); + this.turretSubsystem = TurretSubsystem.GetInstance(); + this.indexSubsystem = IndexSubsystem.GetInstance(); + + addRequirements(this.shooterSubsystem, this.indexSubsystem); + } + + public ContinuousShooter(Supplier targetGlobalPoseSupplier) { + this(targetGlobalPoseSupplier, () -> { + return GlobalPosition.Get().getTranslation(); + }); + } + + public ContinuousShooter() { + this(() -> new Translation2d()); + } + + @Override + public void execute() { + Logger.recordOutput("ContinuousShooter/Time", System.currentTimeMillis()); + Translation2d target = targetGlobalPoseSupplier.get(); + Translation2d self = selfGlobalPoseSupplier.get(); + Translation2d targetRelative = LocalMath.fromGlobalToRelative(self, target); + Pose2d selfPose = new Pose2d(self, GlobalPosition.Get().getRotation()); + ChassisSpeeds robotFieldSpeeds = GlobalPosition.Velocity(GMFrame.kFieldRelative); + Translation2d compensatedTargetRelative = ContinuousAimCommand.GetCompensatedSpeed( + selfPose, + target, + robotFieldSpeeds); + + double rawDistance = targetRelative.getNorm(); + double compensatedDistance = compensatedTargetRelative.getNorm(); + shooterSubsystem.setShooterVelocity( + ShooterConstants.DistanceFromTargetToVelocity(compensatedDistance)); + + Logger.recordOutput("ContinuousShooter/TargetRelative", targetRelative); + Logger.recordOutput("ContinuousShooter/CompensatedTargetRelative", compensatedTargetRelative); + Logger.recordOutput("ContinuousShooter/RawDistanceToTarget", rawDistance); + Logger.recordOutput("ContinuousShooter/CompensatedDistanceToTarget", compensatedDistance); + + if (turretSubsystem.getAimTimeLeftMs() > TurretConstants.kTurretOffByMs + || !shooterSubsystem.isShooterSpunUp()) { + isShooting = false; + return; + } + + isShooting = true; + indexSubsystem.runMotor(); + } + + @Override + public void end(boolean interrupted) { + isShooting = false; + indexSubsystem.stopMotor(); + } + +} diff --git a/src/main/java/frc/robot/command/shooting/ShooterCommand.java b/src/main/java/frc/robot/command/shooting/ShooterCommand.java new file mode 100644 index 0000000..907d054 --- /dev/null +++ b/src/main/java/frc/robot/command/shooting/ShooterCommand.java @@ -0,0 +1,29 @@ +package frc.robot.command.shooting; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.ShooterSubsystem; + +import java.util.function.Supplier; + +public class ShooterCommand extends Command { + private final ShooterSubsystem shooterSubsystem; + private final Supplier speed; + + public ShooterCommand(ShooterSubsystem shooterSubsystem, Supplier speed) { + this.shooterSubsystem = shooterSubsystem; + this.speed = speed; + addRequirements(shooterSubsystem); + } + + @Override + public void execute() { + shooterSubsystem.setShooterVelocity(speed.get()); + } + + @Override + public void end(boolean interrupted) { + shooterSubsystem.stopShooter(); + } +} diff --git a/src/main/java/frc/robot/command/shooting/Unclog.java b/src/main/java/frc/robot/command/shooting/Unclog.java new file mode 100644 index 0000000..f06958e --- /dev/null +++ b/src/main/java/frc/robot/command/shooting/Unclog.java @@ -0,0 +1,38 @@ +package frc.robot.command.shooting; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.IndexSubsystem; + +public class Unclog extends Command { + private final IndexSubsystem indexSubsystem; + private static final double unclogTimeSeconds = 1.0; + private final Timer timer; + + public Unclog() { + this.indexSubsystem = IndexSubsystem.GetInstance(); + this.timer = new Timer(); + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + indexSubsystem.reverseRunMotor(); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(unclogTimeSeconds); + } + + @Override + public void end(boolean interrupted) { + indexSubsystem.stopMotor(); + timer.stop(); + } +} diff --git a/src/main/java/frc/robot/command/testing/IndexCommand.java b/src/main/java/frc/robot/command/testing/IndexCommand.java new file mode 100644 index 0000000..fce79a6 --- /dev/null +++ b/src/main/java/frc/robot/command/testing/IndexCommand.java @@ -0,0 +1,30 @@ +package frc.robot.command.testing; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.IndexSubsystem; + +public class IndexCommand extends Command { + private final IndexSubsystem m_indexSubsystem; + private final double m_speed; + + public IndexCommand(IndexSubsystem baseSubsystem, double speed) { + m_indexSubsystem = baseSubsystem; + m_speed = speed; + addRequirements(m_indexSubsystem); + } + + @Override + public void execute() { + m_indexSubsystem.runMotor(m_speed); + } + + @Override + public void end(boolean interrupted) { + m_indexSubsystem.stopMotor(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/util/PollingCommand.java b/src/main/java/frc/robot/command/util/PollingCommand.java new file mode 100644 index 0000000..720e319 --- /dev/null +++ b/src/main/java/frc/robot/command/util/PollingCommand.java @@ -0,0 +1,76 @@ +package frc.robot.command.util; + +import java.util.HashSet; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * Default command for {@link frc.robot.subsystem.LightsSubsystem} that runs a + * list of sub-commands every cycle, simulating scheduler lifecycle: initialize + * once per sub-command, execute until finished, end on finish or when this + * command is interrupted. + */ +public class PollingCommand extends Command { + + private final Supplier> commandSupplier; + private final Set initializedCommandIds = new HashSet<>(); + + public PollingCommand(Subsystem subsystem, Supplier> commandSupplier) { + this.commandSupplier = commandSupplier; + addRequirements(subsystem); + } + + @Override + public void initialize() { + // Sub-commands are lazily initialized on first execute when they appear in the + // list + } + + @Override + public void execute() { + for (Command command : commandSupplier.get()) { + int commandId = command.getName().hashCode(); + if (!initializedCommandIds.contains(commandId) && !command.isFinished()) { + command.initialize(); + initializedCommandIds.add(commandId); + + } + + if (command.isFinished()) { + command.end(false); + initializedCommandIds.remove(commandId); + } else { + command.execute(); + } + } + } + + @Override + public void end(boolean interrupted) { + for (Command command : commandSupplier.get()) { + if (initializedCommandIds.contains(command.getName().hashCode())) { + command.end(interrupted); + } + } + + initializedCommandIds.clear(); + } + + @Override + public boolean isFinished() { + return false; + } + + public boolean isCommandAlreadyInserted(Command command) { + for (Command existing : commandSupplier.get()) { + if (existing.getName().equals(command.getName())) { + return true; + } + } + return false; + } +} diff --git a/src/main/java/frc/robot/constant/BotConstants.java b/src/main/java/frc/robot/constant/BotConstants.java index 94a862b..03d371e 100644 --- a/src/main/java/frc/robot/constant/BotConstants.java +++ b/src/main/java/frc/robot/constant/BotConstants.java @@ -1,14 +1,24 @@ package frc.robot.constant; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.wpilibj.DriverStation.Alliance; public class BotConstants { + + public static final AprilTagFieldLayout kFieldLayout = AprilTagFieldLayout + .loadField(AprilTagFields.k2026RebuiltWelded); + + // public static final Alliance alliance = Alliance.Red; // + // DriverStation.getAlliance().orElse(Alliance.Blue); + public static enum RobotVariant { ABOT, BBOT } - public static final RobotVariant robotType = RobotVariant.BBOT; + public static final RobotVariant robotType = RobotVariant.ABOT; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/constant/ClimberConstants.java b/src/main/java/frc/robot/constant/ClimberConstants.java new file mode 100644 index 0000000..726ab99 --- /dev/null +++ b/src/main/java/frc/robot/constant/ClimberConstants.java @@ -0,0 +1,51 @@ +package frc.robot.constant; + +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; + +public class ClimberConstants { + + public static enum ClimberPosition { + L1(new Distance[] { Units.Meters.of(0.0), Units.Meters.of(1.0) }), + L2(new Distance[] { Units.Meters.of(1.0), Units.Meters.of(0.0) }); + + public final Distance[] positionMoveSequence; + + ClimberPosition(Distance[] positionMoveSequence) { + this.positionMoveSequence = positionMoveSequence; + } + } + + public final static double kGearRatio = 0.0; + public final static double kAxleToHeightRatio = 0.0; + public final static double kGearHeightRatio = kGearRatio * kAxleToHeightRatio; + + public static final int kLeftMotorID = 8; + public static final int kRightMotorID = 9; + + public final static double kP = 0.6; + public final static double kI = 0.08; + public final static double kD = 0; + public final static double kIZone = Double.POSITIVE_INFINITY; + public final static double kDifSpeedMultiplier = 0; + public final static double kS = 0; + public final static double kV = 0; + public final static double kG = 0.02; + public final static double kA = 0; + public final static double kTolerance = 0.1; + + public final static boolean kSetpointRamping = true; + public final static double kMaxSetpointRamp = 0.15; + + public static final MotorType kMotorType = MotorType.kBrushless; + + public static final boolean kLeftMotorInverted = true; + public static final boolean kRightMotorInverted = false; + + public static final Distance kMaxHeight = Units.Meters.of(10.0); + public static final Distance kMinHeight = Units.Meters.of(0.0); + + public static final Distance kStartingHeight = Units.Meters.of(0.0); +} diff --git a/src/main/java/frc/robot/constant/CommunicationConstants.java b/src/main/java/frc/robot/constant/CommunicationConstants.java new file mode 100644 index 0000000..5f9b7a0 --- /dev/null +++ b/src/main/java/frc/robot/constant/CommunicationConstants.java @@ -0,0 +1,16 @@ +package frc.robot.constant; + +import edu.wpi.first.networktables.NetworkTable; +import frc.robot.Robot; + +public class CommunicationConstants { + public static final String kPoseSubscribeTopic = "pos-extrapolator/robot-position"; + public static final String kPiTechnicalLogTopic = "pi-technical-log"; + public static final String kOdometryPublishTopic = "robot/odometry"; + public static final String kCameraViewTopic = "apriltag/camera"; + public static final String kCameraTagsViewTopic = "apriltag/tag"; + + public static final String kOdometrySensorId = "odom"; + + public static final String kSharedTable = "Shared"; +} diff --git a/src/main/java/frc/robot/constant/HardwareConstants.java b/src/main/java/frc/robot/constant/HardwareConstants.java new file mode 100644 index 0000000..fe1c03d --- /dev/null +++ b/src/main/java/frc/robot/constant/HardwareConstants.java @@ -0,0 +1,18 @@ +package frc.robot.constant; + +public class HardwareConstants { + public record PigeonConfig(int canId, double mountPoseYawDeg, double mountPosePitchDeg, double mountPoseRollDeg) { + } + + public static final PigeonConfig[] kPigeonConfigs = { + new PigeonConfig(40, 0.0, 0.0, 0.0), + }; + + public enum RobotMainGyro { + GlobalClosest, + One, + Two, + } + + public static final RobotMainGyro kRobotMainGyro = RobotMainGyro.One; +} diff --git a/src/main/java/frc/robot/constant/IndexConstants.java b/src/main/java/frc/robot/constant/IndexConstants.java new file mode 100644 index 0000000..b5500de --- /dev/null +++ b/src/main/java/frc/robot/constant/IndexConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constant; + +public class IndexConstants { + public static final int kIndexMotorID = 35; + public static final double kIndexMotorSpeed = 1; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constant/IntakeConstants.java b/src/main/java/frc/robot/constant/IntakeConstants.java new file mode 100644 index 0000000..4a147e7 --- /dev/null +++ b/src/main/java/frc/robot/constant/IntakeConstants.java @@ -0,0 +1,41 @@ +package frc.robot.constant; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class IntakeConstants { + public static final int intakeIntakerMotorID = 33; + public static final boolean intakeIntakerInverted = false; + public static final double intakeMotorSpeed = 0.7; + public static final double extakeMotorSpeed = -0.7; + + public static final int intakeWristMotorID = 34; + public static final boolean intakeWristInverted = false; + + public static final double intakeWristP = 3; + public static final double intakeWristI = 0.0; + public static final double intakeWristD = 0.0; + public static final double intakeWristIZone = 0.0; + public final static double intakeWristFeedForwardK = 0.2; + public final static Rotation2d intakeWristFFOffset = Rotation2d.fromRotations(0); // TODO + + public final static int intakeWristCurrentLimit = 20; + public final static double intakeWristGearingRatio = 1.0 / 80.0; + public final static Rotation2d intakeWristOffset = Rotation2d.fromRotations(0.005); // when the wrist is fully down + + public static enum WristRaiseLocation { + BOTTOM(Rotation2d.fromRotations(0)), + MIDDLE(Rotation2d.fromRotations(0.16)), + TOP(Rotation2d.fromRotations(0.338)); + + public final Rotation2d position; + + WristRaiseLocation(Rotation2d position) { + this.position = position; + } + } + + // public final static Rotation2d intakeWristIntakingAngle = + // Rotation2d.fromRotations(0); + + public final static Rotation2d kTolerance = Rotation2d.fromDegrees(5); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constant/LEDConstants.java b/src/main/java/frc/robot/constant/LEDConstants.java new file mode 100644 index 0000000..1b00e2c --- /dev/null +++ b/src/main/java/frc/robot/constant/LEDConstants.java @@ -0,0 +1,30 @@ +package frc.robot.constant; + +public class LEDConstants { + private LEDConstants() { + } + + /** CAN ID of the CANdle device. Configure in Phoenix Tuner. */ + public static final int candleCANId = 26; + + /** Full LED address space on CANdle: onboard [0..7] + strip [8..399]. */ + public static final int ledStartIndex = 0; + public static final int ledEndIndex = 399; + public static final int ledCount = ledEndIndex - ledStartIndex + 1; + + /** Maximum number of contiguous solid-color writes per periodic cycle. */ + public static final int maxSolidWritesPerCycle = 48; + + /** Foundational named zones. */ + public static final int onboardStartIndex = 0; + public static final int onboardEndIndex = 7; + + public static final int externalStripStartIndex = 8; + public static final int externalStripEndIndex = 399; + + public static final int leftHalfStartIndex = 8; + public static final int leftHalfEndIndex = 203; + + public static final int rightHalfStartIndex = 204; + public static final int rightHalfEndIndex = 399; +} diff --git a/src/main/java/frc/robot/constant/PathPlannerConstants.java b/src/main/java/frc/robot/constant/PathPlannerConstants.java new file mode 100644 index 0000000..b8d7d21 --- /dev/null +++ b/src/main/java/frc/robot/constant/PathPlannerConstants.java @@ -0,0 +1,124 @@ +package frc.robot.constant; + +import java.util.Optional; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; +import frc.robot.command.SwerveMoveTeleop.AxisConstraint; +import frc.robot.command.SwerveMoveTeleop.Lane; +import frc.robot.util.PathedAuto; +import frc.robot.util.SharedStringTopic; + +public class PathPlannerConstants { + /** + * Holds the currently selected autonomous routine and its path. + * Subscribes to the auto selection NetworkTables topic and updates + * the auto, path, and path poses when the selection changes. + */ + public static class SelectedAuto { + private static final String kNoneSelection = "NONE"; + + private String name; + private Optional currentAuto; + + private final BooleanSupplier shouldFlip; + private final SharedStringTopic kAutoSelect; + + public SelectedAuto(BooleanSupplier shouldFlip) { + this(shouldFlip, kAutoSelectTopic); + } + + /** + * Creates a SelectedAuto that listens for auto selection changes. + * + * @param shouldFlip whether path preview data should be flipped for the current + * alliance + */ + public SelectedAuto(BooleanSupplier shouldFlip, String topicBase) { + this.kAutoSelect = new SharedStringTopic(topicBase); + this.shouldFlip = shouldFlip; + this.name = kNoneSelection; + this.currentAuto = Optional.empty(); + + kAutoSelect.setState(this.name); + } + + private void clearSelection() { + name = kNoneSelection; + currentAuto = Optional.empty(); + kAutoSelect.setState(name); + } + + private void updateFromSelection(String selected) { + if (selected.equals(name)) + return; + + if (selected == null || selected.isEmpty() || kNoneSelection.equalsIgnoreCase(selected)) { + clearSelection(); + return; + } + + name = selected; + currentAuto = Optional.of(new PathedAuto(name)); + kAutoSelect.setState(name); + } + + private void updateFromSelection() { + updateFromSelection(kAutoSelect.getState()); + } + + public Pose2d[] getPathPoses(int index) { + updateFromSelection(); + + if (!currentAuto.isPresent()) { + return new Pose2d[0]; + } + + if (index < 0 || index >= currentAuto.get().getPaths().size()) { + return new Pose2d[0]; + } + + var path = currentAuto.get().getPaths().get(index); + if (shouldFlip.getAsBoolean()) { + path = path.flipPath(); + } + + return path.getPathPoses().toArray(new Pose2d[0]); + } + + public Pose2d[] getAllPathPoses() { + return getPathPoses(0); + } + + public String getName() { + updateFromSelection(); + return name; + } + + public Optional getCurrentAuto() { + updateFromSelection(); + return currentAuto; + } + } + + public static final PPHolonomicDriveController defaultPathfindingController = new PPHolonomicDriveController( + new PIDConstants(3.0, 0.0, 0.1), + new PIDConstants(0.5, 0.0, 0.3)); + + public static final PathConstraints defaultPathfindingConstraints = new PathConstraints(1.0, 1.0, + Units.degreesToRadians(360), Units.degreesToRadians(720)); + + public static final Distance distanceConsideredOffTarget = edu.wpi.first.units.Units.Meters.of(1.0); + + public static final String kAutoSelectTopic = "PathPlanner/SelectedPath"; + + public static final Lane[] kLanes = { + new Lane(new Pose2d(11.94, 7.52, new Rotation2d(1, 0)), 1.0, AxisConstraint.Y), + }; +} diff --git a/src/main/java/frc/robot/constant/PiConstants.java b/src/main/java/frc/robot/constant/PiConstants.java deleted file mode 100644 index 10b283f..0000000 --- a/src/main/java/frc/robot/constant/PiConstants.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.constant; - -import java.io.File; - -import edu.wpi.first.wpilibj.Filesystem; - -public class PiConstants { - public static File configFilePath = new File( - Filesystem.getDeployDirectory().getAbsolutePath() + "/config"); - - public static int networkInitializeTimeSec = 4; -} diff --git a/src/main/java/frc/robot/constant/ShooterConstants.java b/src/main/java/frc/robot/constant/ShooterConstants.java new file mode 100644 index 0000000..126d44d --- /dev/null +++ b/src/main/java/frc/robot/constant/ShooterConstants.java @@ -0,0 +1,51 @@ +package frc.robot.constant; + +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; + +public class ShooterConstants { + public static final int kShooterCurrentLimit = 60; + + public static final double kShooterP = 0.0005; + public static final double kShooterFollowerP = kShooterP; // 0.0025; + public static final double kShooterI = 0.00000001; + public static final double kShooterD = 0.1 / 2; + public static final double kShooterIZ = 0.0; + public static final double kFF = 0.0018; + + public static final boolean kShooterLeaderReversed = true; + public static final boolean kShooterFollowerReversed = false; + public static final double kShooterMotorRotationsPerRotation = 1.0; + + public static final int kShooterCanId = 31; + public static final MotorType kShooterMotorType = MotorType.kBrushless; + + public static final int kShooterCanIdFollower = 32; + public static final MotorType kShooterMotorTypeFollower = MotorType.kBrushless; + public static final AngularVelocity kShooterMinVelocity = Units.RotationsPerSecond.of(25); + public static final AngularVelocity kShooterMaxVelocity = Units.RotationsPerSecond.of(50.0); + public static final AngularVelocity kShooterVelocityTolerance = Units.RotationsPerSecond.of(0.6); + + public static final AngularVelocity kShooterBaseSpeed = Units.RotationsPerSecond.of(25.0); + + ///////////////// AIMING CONSTANTS ///////////////// + + public static final double kTimeVsDistanceSlope = 0.175; + public static final double kTimeVsDistanceIntercept = 0.306; + + public static final double kRPMVsDistanceSlope = 112; + public static final double kRPMVsDistanceIntercept = 1594; + + public static final double kOutMult = 1.025; + + public static double DistanceFromTargetToTime(double distance) { + return kTimeVsDistanceSlope * distance + kTimeVsDistanceIntercept; + } + + public static AngularVelocity DistanceFromTargetToVelocity(double distance) { + double rpm = kRPMVsDistanceSlope * distance + kRPMVsDistanceIntercept; + return Units.RotationsPerSecond.of((rpm * kOutMult / 60)); + } +} diff --git a/src/main/java/frc/robot/constant/TopicConstants.java b/src/main/java/frc/robot/constant/TopicConstants.java deleted file mode 100644 index 035ccaa..0000000 --- a/src/main/java/frc/robot/constant/TopicConstants.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot.constant; - -public class TopicConstants { - public static String kPoseSubscribeTopic = "pos-extrapolator/robot-position"; - public static String kPiTechnicalLogTopic = "pi-technical-log"; - public static String kOdometryPublishTopic = "robot/odometry"; - public static String kCameraViewTopic = "apriltag/camera"; - public static String kCameraTagsViewTopic = "apriltag/tag"; -} diff --git a/src/main/java/frc/robot/constant/TurretConstants.java b/src/main/java/frc/robot/constant/TurretConstants.java new file mode 100644 index 0000000..22e7b52 --- /dev/null +++ b/src/main/java/frc/robot/constant/TurretConstants.java @@ -0,0 +1,36 @@ +package frc.robot.constant; + +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; + +public class TurretConstants { + public static final int kTurretCanId = 30; + + public static final int kTurretCurrentLimit = 100; + public static final Angle kTurretTheta = Units.Degrees.of(45.0); + public static final MotorType kTurretMotorType = MotorType.kBrushless; + + public static final double kTurretP = 6 * 1; // 3; + public static final double kTurretI = 0.0; + public static final double kTurretD = 2; + public static final double kTurretIZ = 0.0; + public static final double kFFCommand = 0.5; + public static final Voltage kFFBase = Units.Volts.of(0.5); + + public static final AngularVelocity kTurretMaxVelocity = Units.RadiansPerSecond.of(4.0); + public static final AngularAcceleration kTurretMaxAcceleration = Units.RadiansPerSecondPerSecond.of(4.0); + public static final Angle kTurretMinAngle = Units.Degrees.of(-180.0); + public static final Angle kTurretMaxAngle = Units.Degrees.of(180.0); + + public static final int kTurretOffByMs = 20; + + public static final boolean kMotorInverted = true; + + public static final Angle kTurretOffset = Units.Rotations.of(0.744); +} diff --git a/src/main/java/frc/robot/constant/swerve/SwerveConstants.java b/src/main/java/frc/robot/constant/swerve/SwerveConstants.java index c6ba225..4b5d3bd 100644 --- a/src/main/java/frc/robot/constant/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/constant/swerve/SwerveConstants.java @@ -22,6 +22,13 @@ * should consume the active set via {@link #INSTANCE}. */ public final class SwerveConstants { + public static final AngularVelocity kRobotMaxTurnSpeed = Units.RadiansPerSecond.of(Math.PI); // 180 deg/s + public static final LinearVelocity kRobotMaxSpeed = Units.MetersPerSecond.of(4); + public static final AngularAcceleration kRobotMaxTurnAcceleration = Units.RadiansPerSecondPerSecond.of(2 * Math.PI); + public static final LinearAcceleration kRobotMaxLinearAcceleration = Units.MetersPerSecondPerSecond.of(5.0); + + ////////////////////////////////////// + public Translation2d rearLeftTranslation; public Translation2d rearRightTranslation; public Translation2d frontRightTranslation; @@ -82,8 +89,8 @@ public final class SwerveConstants { public double kDriveI; public double kDriveD; public double kDriveIZ; - public Voltage kDriveV; - public double kDriveStatorLimit; + public double kDriveV; + public Current kDriveStatorLimit; public Current kDriveSupplyLimit; public double kDriveMinOutput; public double kDriveMaxOutput; @@ -199,13 +206,13 @@ private static SwerveConstants fromTalonFX() { c.frontRightTranslation = SwerveConstantsTalonFX.frontRightTranslation; c.frontLeftTranslation = SwerveConstantsTalonFX.frontLeftTranslation; - c.kMaxSpeed = Units.MetersPerSecond.of(SwerveConstantsTalonFX.tempMaxSpeed); + c.kMaxSpeed = SwerveConstantsTalonFX.kMaxSpeed; c.kMaxLinearAcceleration = SwerveConstantsTalonFX.kMaxLinearAcceleration; c.kMaxLinearJerk = SwerveConstantsTalonFX.kMaxLinearJerk; - c.kMaxTurnSpeed = SwerveConstantsTalonFX.kMaxTurnSpeed; - c.kMaxTurnAcceleration = SwerveConstantsTalonFX.kMaxTurnAcceleration; - c.kMaxTurnJerk = SwerveConstantsTalonFX.kMaxTurnJerk; + c.kMaxTurnSpeed = SwerveConstantsTalonFX.kTurnMotionMagicCruiseVelocity; + c.kMaxTurnAcceleration = SwerveConstantsTalonFX.kTurnMotionMagicAcceleration; + c.kMaxTurnJerk = 0; c.kTurnCurrentLimit = SwerveConstantsTalonFX.kTurnCurrentLimit; c.kDriveCurrentLimit = SwerveConstantsTalonFX.kDriveCurrentLimit; diff --git a/src/main/java/frc/robot/constant/swerve/SwerveConstantsSpark.java b/src/main/java/frc/robot/constant/swerve/SwerveConstantsSpark.java index 5713695..12171fc 100644 --- a/src/main/java/frc/robot/constant/swerve/SwerveConstantsSpark.java +++ b/src/main/java/frc/robot/constant/swerve/SwerveConstantsSpark.java @@ -101,12 +101,12 @@ public final class SwerveConstantsSpark { public static final double kDriveI = 0; public static final double kDriveD = 0; public static final double kDriveIZ = 0; - public static final Voltage kDriveV = Units.Volts.of(0.8); // Velocity feedforward - critical for velocity control + public static final double kDriveV = 0.8; // Velocity feedforward - critical for velocity control public static final double kDriveMinOutput = -1; public static final double kDriveMaxOutput = 1; - public static final double kDriveStatorLimit = 70; // TEMP - public static final Current kDriveSupplyLimit = Units.Amps.of(40); // TEMP + public static final Current kDriveStatorLimit = Units.Amps.of(70); // TEMP + public static final Current kDriveSupplyLimit = Units.Amps.of(30); // TEMP // PID values for the turning public static final double kTurnP = 1.5; diff --git a/src/main/java/frc/robot/constant/swerve/SwerveConstantsTalonFX.java b/src/main/java/frc/robot/constant/swerve/SwerveConstantsTalonFX.java index 1957a59..f0a0026 100644 --- a/src/main/java/frc/robot/constant/swerve/SwerveConstantsTalonFX.java +++ b/src/main/java/frc/robot/constant/swerve/SwerveConstantsTalonFX.java @@ -12,58 +12,50 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; -import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.units.measure.LinearVelocity; public class SwerveConstantsTalonFX { public static final Translation2d rearLeftTranslation = new Translation2d( - 0.38, - 0.38); + 0.3429, + 0.3429); public static final Translation2d rearRightTranslation = new Translation2d( - 0.38, - -0.38); + 0.3429, + -0.3429); public static final Translation2d frontRightTranslation = new Translation2d( - -0.38, - -0.38); + -0.3429, + -0.3429); public static final Translation2d frontLeftTranslation = new Translation2d( - -0.38, - 0.38); + -0.3429, + 0.3429); - public static final double kMaxSpeedMPSNormElevator = 2; - public static final double kMaxSpeedMPSTopElevator = 0.6; - public static double tempMaxSpeed = kMaxSpeedMPSNormElevator; - - public static final AngularVelocity kMaxTurnSpeed = Units.RadiansPerSecond.of(Math.PI / 1.3); // 180 deg/s - public static final AngularAcceleration kMaxTurnAcceleration = Units.RadiansPerSecondPerSecond.of(10.0); - /** Units: radians/sec^3 */ - public static final double kMaxTurnJerk = 100.0; - - public static final LinearAcceleration kMaxLinearAcceleration = Units.MetersPerSecondPerSecond.of(3.0); + public static final LinearVelocity kMaxSpeed = Units.MetersPerSecond.of(0); + public static final LinearAcceleration kMaxLinearAcceleration = Units.MetersPerSecondPerSecond.of(0); /** Units: meters/sec^3 */ public static final double kMaxLinearJerk = 20.0; - public static final Current kTurnCurrentLimit = Units.Amps.of(10); + public static final Current kTurnCurrentLimit = Units.Amps.of(30); public static final Current kDriveCurrentLimit = Units.Amps.of(30); // the driving motor ports public static final int kFrontLeftDriveMotorPort = 7; - public static final int kFrontRightDriveMotorPort = 9; - public static final int kRearLeftDriveMotorPort = 11; - public static final int kRearRightDriveMotorPort = 13; + public static final int kFrontRightDriveMotorPort = 13; + public static final int kRearLeftDriveMotorPort = 9; + public static final int kRearRightDriveMotorPort = 11; // whether the driving encoders are flipped public static final InvertedValue kFrontLeftDriveMotorReversed = InvertedValue.Clockwise_Positive; public static final InvertedValue kRearLeftDriveMotorReversed = InvertedValue.Clockwise_Positive; public static final InvertedValue kFrontRightDriveMotorReversed = InvertedValue.Clockwise_Positive; - public static final InvertedValue kRearRightDriveMotorReversed = InvertedValue.Clockwise_Positive; + public static final InvertedValue kRearRightDriveMotorReversed = InvertedValue.CounterClockwise_Positive; // the turning motor ports public static final int kFrontLeftTurningMotorPort = 6; - public static final int kFrontRightTurningMotorPort = 8; - public static final int kRearLeftTurningMotorPort = 10; - public static final int kRearRightTurningMotorPort = 12; + public static final int kFrontRightTurningMotorPort = 12; + public static final int kRearLeftTurningMotorPort = 8; + public static final int kRearRightTurningMotorPort = 10; // whether the turning enoders are flipped public static final InvertedValue kFrontLeftTurningMotorReversed = InvertedValue.Clockwise_Positive; @@ -73,9 +65,9 @@ public class SwerveConstantsTalonFX { // the CANCoder turning encoder ports - updated 2/12/24 public static final int kFrontLeftCANcoderPort = 2; - public static final int kFrontRightCANcoderPort = 3; - public static final int kRearLeftCANcoderPort = 4; - public static final int kRearRightCANcoderPort = 5; + public static final int kFrontRightCANcoderPort = 5; + public static final int kRearLeftCANcoderPort = 3; + public static final int kRearRightCANcoderPort = 4; // whether the turning CANCoders are flipped @@ -89,10 +81,10 @@ public class SwerveConstantsTalonFX { // opening the Phoenix Tuner app, and taking snapshots of // the rotational values of the CANCoders while in they are in the forward state // units: rotations - public static final double kFrontLeftCANcoderMagnetOffset = -0.184; - public static final double kFrontRightCANcoderMagnetOffset = -0.18; - public static final double kRearLeftCANcoderMagnetOffset = 0.302; - public static final double kRearRightCANcoderMagnetOffset = 0.459; + public static final double kFrontLeftCANcoderMagnetOffset = -0.316; + public static final double kFrontRightCANcoderMagnetOffset = -0.208; + public static final double kRearLeftCANcoderMagnetOffset = 0.177; + public static final double kRearRightCANcoderMagnetOffset = 0.441; // stats used by SwerveSubsystem for math public static final Distance kWheelDiameter = Units.Meters.of(0.089); @@ -116,12 +108,12 @@ public class SwerveConstantsTalonFX { public static final double kDirectionMultiplier = 0.01; // PID values for the driving - public static final double kDriveP = 0.01; - public static final double kDriveI = 0; + public static final double kDriveP = 0.5; + public static final double kDriveI = 1; public static final double kDriveD = 0; public static final double kDriveIZ = 0; public static final double kDriveFF = 0; - public static final Voltage kDriveV = Units.Volts.of(0.6); // Velocity feedforward - critical for velocity control + public static final double kDriveV = 0.6; // Velocity feedforward - critical for velocity control public static final double kDriveMinOutput = -1; public static final double kDriveMaxOutput = 1; @@ -132,8 +124,8 @@ public class SwerveConstantsTalonFX { public static final double kAutonSpeedMultiplier = 0.5; public static final double kDriveMaxRPM = 5700; - public static final double kDriveStatorLimit = 70; // TEMP - public static final Current kDriveSupplyLimit = Units.Amps.of(40); // TEMP + public static final Current kDriveStatorLimit = Units.Amps.of(70); // TEMP + public static final Current kDriveSupplyLimit = Units.Amps.of(30); // TEMP // PID values for the turning public static final double kTurnP = 1.5 * 12; @@ -170,9 +162,8 @@ public class SwerveConstantsTalonFX { // Motion Magic configuration for turn motors (position control with trapezoid // profiling) - public static final double kTurnMotionMagicCruiseVelocity = 100; // rotations/sec - max turn speed - public static final double kTurnMotionMagicAcceleration = 200; // rotations/sec² - turn acceleration - public static final double kTurnMotionMagicJerk = 2000; // rotations/sec³ - smoothness of turn acceleration changes + public static final AngularVelocity kTurnMotionMagicCruiseVelocity = Units.RotationsPerSecond.of(0); + public static final AngularAcceleration kTurnMotionMagicAcceleration = Units.RotationsPerSecondPerSecond.of(0); public static final int kPigeonCANId = 40; } diff --git a/src/main/java/frc/robot/hardware/AHRSGyro.java b/src/main/java/frc/robot/hardware/AHRSGyro.java index 5677e2d..cea4acd 100644 --- a/src/main/java/frc/robot/hardware/AHRSGyro.java +++ b/src/main/java/frc/robot/hardware/AHRSGyro.java @@ -1,12 +1,12 @@ package frc.robot.hardware; -import org.littletonrobotics.junction.Logger; - import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.I2C; -import frc.robot.util.CustomMath; +import frc.robot.util.LocalMath; import frc4765.proto.sensor.GeneralSensorDataOuterClass.GeneralSensorData; import frc4765.proto.sensor.GeneralSensorDataOuterClass.SensorName; import frc4765.proto.sensor.Imu.ImuData; @@ -24,11 +24,15 @@ public class AHRSGyro implements IGyroscopeLike, IDataClass { private double yOffset = 0; private double zOffset = 0; private double yawSoftOffsetDeg = 0.0; + private boolean hasYawRateSample = false; + private double lastYawRateSampleDeg = 0.0; + private long lastYawRateSampleNanos = 0L; public AHRSGyro(I2C.Port i2c_port_id) { this.m_gyro = new AHRS(i2c_port_id); m_gyro.reset(); yawSoftOffsetDeg = 0.0; + resetYawRateState(); } /** @@ -50,146 +54,96 @@ public AHRS getGyro() { return m_gyro; } - @Override - public double[] getYPR() { - double yawAdj = CustomMath.wrapTo180(m_gyro.getYaw() + yawSoftOffsetDeg); - return new double[] { - yawAdj, - m_gyro.getPitch(), - m_gyro.getRoll(), - }; + private void resetYawRateState() { + hasYawRateSample = false; + lastYawRateSampleDeg = 0.0; + lastYawRateSampleNanos = 0L; } - @Override - public void setPositionAdjustment(double x, double y, double z) { - xOffset = x; - yOffset = y; - zOffset = z; - m_gyro.resetDisplacement(); + private double getRawYawDegrees() { + return m_gyro.getRotation2d().getDegrees(); } - @Override - public double[] getLinearAccelerationXYZ() { - return new double[] { - m_gyro.getWorldLinearAccelX(), - m_gyro.getWorldLinearAccelY(), - m_gyro.getWorldLinearAccelZ(), - }; + private double getAdjustedYawDegrees() { + return LocalMath.wrapTo180(getRawYawDegrees() + yawSoftOffsetDeg); } - @Override - public double[] getAngularVelocityXYZ() { - return new double[] { 0, 0, Math.toRadians(m_gyro.getRate()) }; - } + private double getYawRateRadPerSec() { + final long nowNanos = System.nanoTime(); + final double nowYawDeg = getAdjustedYawDegrees(); - @Override - public double[] getQuaternion() { - return new double[] { - m_gyro.getQuaternionW(), - m_gyro.getQuaternionX(), - m_gyro.getQuaternionY(), - m_gyro.getQuaternionZ(), - }; - } + if (!hasYawRateSample) { + hasYawRateSample = true; + lastYawRateSampleDeg = nowYawDeg; + lastYawRateSampleNanos = nowNanos; + return 0.0; + } - @Override - public double[] getLinearVelocityXYZ() { - return new double[] { - m_gyro.getVelocityX(), - m_gyro.getVelocityY(), - m_gyro.getVelocityZ(), - }; + final double dtS = (nowNanos - lastYawRateSampleNanos) * 1e-9; + final double deltaYawDeg = LocalMath.wrapTo180(nowYawDeg - lastYawRateSampleDeg); + + lastYawRateSampleDeg = nowYawDeg; + lastYawRateSampleNanos = nowNanos; + + if (dtS <= 1e-6) { + return 0.0; + } + + return Math.toRadians(deltaYawDeg / dtS); } - @Override - public double[] getPoseXYZ() { - return new double[] { - m_gyro.getDisplacementX() + xOffset, - m_gyro.getDisplacementY() + yOffset, - m_gyro.getDisplacementZ() + zOffset, - }; + public void setYawDegrees(double yawDeg) { + yawSoftOffsetDeg = LocalMath.wrapTo180(yawDeg - getRawYawDegrees()); + resetYawRateState(); } - @Override - public void reset() { - m_gyro.reset(); - yawSoftOffsetDeg = 0.0; + public double getYawDegrees() { + return getAdjustedYawDegrees(); } - @Override - public void setAngleAdjustment(double angle) { - m_gyro.zeroYaw(); - yawSoftOffsetDeg = -angle; + public double getYawRadians() { + return Math.toRadians(getYawDegrees()); } public void setYawDeg(double targetDeg) { - setAngleAdjustment(targetDeg); + setYawDegrees(targetDeg); } public Rotation2d getNoncontinuousAngle() { - return Rotation2d.fromDegrees(CustomMath.wrapTo180(m_gyro.getAngle())); + return Rotation2d.fromDegrees(getYawDegrees()); } @Override public byte[] getRawConstructedProtoData() { - var poseXYZ = getPoseXYZ(); - var velocityXYZ = getLinearVelocityXYZ(); - var accelerationXYZ = getLinearAccelerationXYZ(); - var yaw = Rotation2d.fromDegrees(getYPR()[0]); - var angularVelocity = getAngularVelocityXYZ(); - - Logger.recordOutput("Imu/AngularVel", angularVelocity[2]); - - Logger.recordOutput("Imu/yaw", yaw.getDegrees()); - - var position = Vector3.newBuilder() - .setX((float) poseXYZ[0]) - .setY((float) poseXYZ[1]) - .setZ((float) poseXYZ[2]) - .build(); - - var direction = Vector3.newBuilder() - .setX((float) yaw.getCos()) - .setY((float) -yaw.getSin()) - .setZ(0) - .build(); - - var position2d = Position3d.newBuilder() - .setPosition(position) - .setDirection(direction) - .build(); - - var velocity = Vector3.newBuilder() - .setX((float) velocityXYZ[0]) - .setY((float) velocityXYZ[1]) - .setZ((float) velocityXYZ[2]) - .build(); - - var acceleration = Vector3.newBuilder() - .setX((float) accelerationXYZ[0]) - .setY((float) accelerationXYZ[1]) - .setZ((float) accelerationXYZ[2]) - .build(); - - var angularVel = Vector3.newBuilder().setX((float) angularVelocity[0]).setY((float) angularVelocity[1]) - .setZ((float) angularVelocity[2]) - .build(); - - var imuData = ImuData.newBuilder() - .setPosition(position2d) - .setVelocity(velocity) - .setAcceleration(acceleration) - .setAngularVelocityXYZ(angularVel) - .build(); - - var all = GeneralSensorData.newBuilder().setImu(imuData).setSensorName(SensorName.IMU).setSensorId("0") - .setTimestamp(System.currentTimeMillis()).setProcessingTimeMs(0); - - return all.build().toByteArray(); + return null; } @Override public String getPublishTopic() { return "imu/imu"; } + + @Override + public ChassisSpeeds getVelocity() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getVelocity'"); + } + + @Override + public ChassisSpeeds getAcceleration() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getAcceleration'"); + } + + @Override + public Rotation3d getRotation() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getRotation'"); + } + + @Override + public void resetRotation(Rotation3d newRotation) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'resetRotation'"); + } } diff --git a/src/main/java/frc/robot/hardware/PigeonGyro.java b/src/main/java/frc/robot/hardware/PigeonGyro.java index 7ab96e8..4ed356e 100644 --- a/src/main/java/frc/robot/hardware/PigeonGyro.java +++ b/src/main/java/frc/robot/hardware/PigeonGyro.java @@ -2,11 +2,18 @@ import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.util.CustomMath; +import frc.robot.constant.BotConstants; +import frc.robot.constant.HardwareConstants; +import frc.robot.constant.HardwareConstants.PigeonConfig; import frc4765.proto.sensor.GeneralSensorDataOuterClass.GeneralSensorData; import frc4765.proto.sensor.GeneralSensorDataOuterClass.SensorName; import frc4765.proto.sensor.Imu.ImuData; @@ -17,141 +24,81 @@ public class PigeonGyro extends SubsystemBase implements IGyroscopeLike, IDataClass { private static PigeonGyro instance; - private static int defaultCanId = 0; + private final Pigeon2 pigeon; - private double xOffset = 0; - private double yOffset = 0; - private double zOffset = 0; - private double yawSoftOffsetDeg = 0.0; + private Rotation2d yawAdjustment = new Rotation2d(); + private PigeonConfig hardwareConfig; - public PigeonGyro(int canId) { - this.pigeon = new Pigeon2(canId); + public PigeonGyro(PigeonConfig config) { + this.hardwareConfig = config; + this.pigeon = new Pigeon2(config.canId()); + applyMountPose(); pigeon.reset(); - yawSoftOffsetDeg = 0.0; - } - - /** - * Set the default CAN ID used by GetInstance(). - * Call this before the first call to GetInstance(). - */ - public static void setDefaultCanId(int canId) { - defaultCanId = canId; + yawAdjustment = new Rotation2d(); } - public static PigeonGyro GetInstance() { + public static PigeonGyro GetInstance(PigeonConfig config) { if (instance == null) { - instance = new PigeonGyro(defaultCanId); + instance = new PigeonGyro(config); } - return instance; } - public Pigeon2 getGyro() { - return pigeon; - } - - @Override - public double[] getYPR() { - double yawAdj = CustomMath.wrapTo180(pigeon.getYaw().getValueAsDouble() + yawSoftOffsetDeg); - return new double[] { - yawAdj, - pigeon.getPitch().getValueAsDouble(), - pigeon.getRoll().getValueAsDouble(), - }; - } - - @Override - public void setPositionAdjustment(double x, double y, double z) { - xOffset = x; - yOffset = y; - zOffset = z; - } - @Override - public double[] getLinearAccelerationXYZ() { - return new double[] { - pigeon.getAccelerationX().getValueAsDouble(), - pigeon.getAccelerationY().getValueAsDouble(), - pigeon.getAccelerationZ().getValueAsDouble(), - }; + public ChassisSpeeds getVelocity() { + return new ChassisSpeeds(0.0, 0.0, pigeon.getAngularVelocityZWorld().getValue().in(Units.RadiansPerSecond)); } @Override - public double[] getAngularVelocityXYZ() { - return new double[] { - Math.toRadians(pigeon.getAngularVelocityXWorld().getValueAsDouble()), - Math.toRadians(pigeon.getAngularVelocityYWorld().getValueAsDouble()), - Math.toRadians(pigeon.getAngularVelocityZWorld().getValueAsDouble()), - }; + public ChassisSpeeds getAcceleration() { + return new ChassisSpeeds(0.0, 0.0, 0.0); } @Override - public double[] getQuaternion() { - return new double[] { - pigeon.getQuatW().getValueAsDouble(), - pigeon.getQuatX().getValueAsDouble(), - pigeon.getQuatY().getValueAsDouble(), - pigeon.getQuatZ().getValueAsDouble(), - }; + public Rotation3d getRotation() { + return new Rotation3d(0.0, 0.0, getYawRotation2d().getRadians()); } @Override - public double[] getLinearVelocityXYZ() { - // Pigeon2 doesn't provide velocity directly, return zeros - return new double[] { 0, 0, 0 }; + public void resetRotation(Rotation3d newRotation) { + yawAdjustment = newRotation.toRotation2d().minus(getRawYawRotation2d()); } - @Override - public double[] getPoseXYZ() { - // Pigeon2 doesn't provide displacement directly, return offsets - return new double[] { - xOffset, - yOffset, - zOffset, - }; + private Rotation2d getRawYawRotation2d() { + return pigeon.getRotation2d(); } - @Override - public void reset() { - pigeon.reset(); - yawSoftOffsetDeg = 0.0; + private Rotation2d getYawRotation2d() { + return getRawYawRotation2d().plus(yawAdjustment); } - @Override - public void setAngleAdjustment(double angle) { - pigeon.setYaw(0); - yawSoftOffsetDeg = -angle; - } + private void applyMountPose() { + var config = new Pigeon2Configuration(); + config.MountPose.withMountPoseYaw(hardwareConfig.mountPoseYawDeg()); + config.MountPose.withMountPosePitch(hardwareConfig.mountPosePitchDeg()); + config.MountPose.withMountPoseRoll(hardwareConfig.mountPoseRollDeg()); - public void setYawDeg(double targetDeg) { - setAngleAdjustment(targetDeg); - } - - public Rotation2d getNoncontinuousAngle() { - return Rotation2d.fromDegrees(CustomMath.wrapTo180(pigeon.getYaw().getValueAsDouble())); + var status = pigeon.getConfigurator().apply(config); + if (!status.isOK()) { + DriverStation.reportWarning("Failed to apply Pigeon mount pose: " + status, false); + } } @Override public byte[] getRawConstructedProtoData() { - var poseXYZ = getPoseXYZ(); - var velocityXYZ = getLinearVelocityXYZ(); - var accelerationXYZ = getLinearAccelerationXYZ(); - var yaw = Rotation2d.fromDegrees(getYPR()[0]); - var angularVelocity = getAngularVelocityXYZ(); - - Logger.recordOutput("Imu/AngularVel", angularVelocity[2]); - - Logger.recordOutput("Imu/yaw", yaw.getDegrees()); + Rotation2d rotation = getRotation().toRotation2d(); + ChassisSpeeds velocity = getVelocity(); + ChassisSpeeds acceleration = getAcceleration(); var position = Vector3.newBuilder() - .setX((float) poseXYZ[0]) - .setY((float) poseXYZ[1]) - .setZ((float) poseXYZ[2]) + .setX(0.0f) + .setY(0.0f) + .setZ(0.0f) .build(); var direction = Vector3.newBuilder() - .setX((float) yaw.getCos()) - .setY((float) -yaw.getSin()) + .setX((float) rotation.getCos()) + .setY((float) rotation.getSin()) .setZ(0) .build(); @@ -160,31 +107,35 @@ public byte[] getRawConstructedProtoData() { .setDirection(direction) .build(); - var velocity = Vector3.newBuilder() - .setX((float) velocityXYZ[0]) - .setY((float) velocityXYZ[1]) - .setZ((float) velocityXYZ[2]) + var vel = Vector3.newBuilder() + .setX((float) velocity.vxMetersPerSecond) + .setY((float) velocity.vyMetersPerSecond) + .setZ(0.0f) .build(); - var acceleration = Vector3.newBuilder() - .setX((float) accelerationXYZ[0]) - .setY((float) accelerationXYZ[1]) - .setZ((float) accelerationXYZ[2]) + var acc = Vector3.newBuilder() + .setX((float) acceleration.vxMetersPerSecond) + .setY((float) acceleration.vyMetersPerSecond) + .setZ(0.0f) .build(); - var angularVel = Vector3.newBuilder().setX((float) angularVelocity[0]).setY((float) angularVelocity[1]) - .setZ((float) angularVelocity[2]) + var angularVel = Vector3.newBuilder().setX((float) 0.0).setY((float) 0.0) + .setZ((float) velocity.omegaRadiansPerSecond) .build(); var imuData = ImuData.newBuilder() .setPosition(position2d) - .setVelocity(velocity) - .setAcceleration(acceleration) + .setVelocity(vel) + .setAcceleration(acc) .setAngularVelocityXYZ(angularVel) .build(); - var all = GeneralSensorData.newBuilder().setImu(imuData).setSensorName(SensorName.IMU).setSensorId("1") - .setTimestamp(System.currentTimeMillis()).setProcessingTimeMs(0); + var all = GeneralSensorData.newBuilder() + .setImu(imuData) + .setSensorName(SensorName.IMU) + .setSensorId(String.valueOf(hardwareConfig.canId())) + .setTimestamp(System.currentTimeMillis()) + .setProcessingTimeMs(0); return all.build().toByteArray(); } @@ -196,6 +147,11 @@ public String getPublishTopic() { @Override public void periodic() { - Logger.recordOutput("PigeonGyro/yaw", getYPR()[0]); + Logger.recordOutput("PigeonGyro/velocity", getVelocity()); + Logger.recordOutput("PigeonGyro/acceleration", getAcceleration()); + Logger.recordOutput("PigeonGyro/Rotation/rotation", getRotation()); + Logger.recordOutput("PigeonGyro/Rotation/rotation2d", getRotation().toRotation2d()); + Logger.recordOutput("PigeonGyro/Rotation/cos", getRotation().toRotation2d().getCos()); + Logger.recordOutput("PigeonGyro/Rotation/sin", getRotation().toRotation2d().getSin()); } } diff --git a/src/main/java/frc/robot/hardware/UnifiedGyro.java b/src/main/java/frc/robot/hardware/UnifiedGyro.java new file mode 100644 index 0000000..afc2097 --- /dev/null +++ b/src/main/java/frc/robot/hardware/UnifiedGyro.java @@ -0,0 +1,112 @@ +package frc.robot.hardware; + +import java.util.Arrays; +import java.util.List; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constant.HardwareConstants; +import frc.robot.subsystem.GlobalPosition; +import pwrup.frc.core.hardware.sensor.IGyroscopeLike; +import pwrup.frc.core.online.PublicationSubsystem; +import pwrup.frc.core.proto.IDataClass; + +public class UnifiedGyro extends SubsystemBase implements IGyroscopeLike { + private static UnifiedGyro instance; + + private final List gyros; + + public UnifiedGyro(IGyroscopeLike... gyros) { + this.gyros = Arrays.asList(gyros); + } + + public static UnifiedGyro GetInstance() { + if (instance == null) { + instance = new UnifiedGyro(Arrays.stream(HardwareConstants.kPigeonConfigs) + .map(PigeonGyro::GetInstance) + .toArray(IGyroscopeLike[]::new)); + } + + return instance; + } + + private IGyroscopeLike getGlobalRotationClosest(Pose2d pose) { + if (gyros.isEmpty()) { + throw new IllegalStateException("No gyros configured"); + } + + if (pose == null || gyros.size() == 1) { + return gyros.get(0); + } + + var targetYaw = pose.getRotation(); + IGyroscopeLike closestGyro = gyros.get(0); + double smallestError = Math.abs(closestGyro.getRotation().toRotation2d().minus(targetYaw).getRadians()); + + for (int i = 1; i < gyros.size(); i++) { + var gyro = gyros.get(i); + double error = Math.abs(gyro.getRotation().toRotation2d().minus(targetYaw).getRadians()); + if (error < smallestError) { + smallestError = error; + closestGyro = gyro; + } + } + + return closestGyro; + } + + private IGyroscopeLike getMainGyro() { + if (HardwareConstants.kRobotMainGyro == HardwareConstants.RobotMainGyro.GlobalClosest + && !GlobalPosition.isValid()) { + return gyros.get(0); + } + + switch (HardwareConstants.kRobotMainGyro) { + case GlobalClosest: + return getGlobalRotationClosest(GlobalPosition.Get()); + case One: + return gyros.get(0); + case Two: + return gyros.get(1); + default: + throw new IllegalArgumentException("Invalid robot main gyro: " + HardwareConstants.kRobotMainGyro); + } + } + + /** + * Registers all contained gyros that implement IDataClass with the publication + * subsystem, so each is published separately with its own sensor ID. + */ + public static void Register() { + UnifiedGyro unified = GetInstance(); + IDataClass[] dataClasses = unified.gyros.stream() + .filter(IDataClass.class::isInstance) + .map(IDataClass.class::cast) + .toArray(IDataClass[]::new); + PublicationSubsystem.addDataClasses(dataClasses); + } + + @Override + public ChassisSpeeds getVelocity() { + return getMainGyro().getVelocity(); + } + + @Override + public ChassisSpeeds getAcceleration() { + return getMainGyro().getAcceleration(); + } + + @Override + public Rotation3d getRotation() { + return getMainGyro().getRotation(); + } + + @Override + public void resetRotation(Rotation3d newRotation) { + for (IGyroscopeLike gyro : gyros) { + gyro.resetRotation(newRotation); + } + } +} diff --git a/src/main/java/frc/robot/hardware/WheelMoverSpark.java b/src/main/java/frc/robot/hardware/WheelMoverSpark.java index 17c2356..d7599e5 100644 --- a/src/main/java/frc/robot/hardware/WheelMoverSpark.java +++ b/src/main/java/frc/robot/hardware/WheelMoverSpark.java @@ -175,7 +175,7 @@ protected void setSpeed(LinearVelocity mpsSpeed) { final double wheelCircumference = Math.PI * c.kWheelDiameter.in(Units.Meters); final double wheelRps = wheelCircumference == 0.0 ? 0.0 : requestedMps / wheelCircumference; - double ffVolts = c.kDriveV.in(Units.Volts) * wheelRps; + double ffVolts = c.kDriveV * wheelRps; // Clamp to reasonable motor voltage. ffVolts = Math.max(-12.0, Math.min(12.0, ffVolts)); diff --git a/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java b/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java index 0694bd6..e8cb194 100644 --- a/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java +++ b/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java @@ -10,8 +10,9 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -31,8 +32,8 @@ public class WheelMoverTalonFX extends WheelMoverBase { private TalonFX m_driveMotor; private TalonFX m_turnMotor; - private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0).withSlot(0); - private final MotionMagicVoltage positionRequest = new MotionMagicVoltage(0).withSlot(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); + private final PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); private final int port; private CANcoder turnCANcoder; @@ -52,7 +53,7 @@ public WheelMoverTalonFX( turnCANcoder = new CANcoder(CANCoderEncoderChannel); CANcoderConfiguration config = new CANcoderConfiguration(); - config.MagnetSensor.MagnetOffset = -CANCoderMagnetOffset; + config.MagnetSensor.MagnetOffset = CANCoderMagnetOffset; config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; config.MagnetSensor.SensorDirection = CANCoderDirection; turnCANcoder.getConfigurator().apply(config); @@ -67,7 +68,7 @@ public WheelMoverTalonFX( .withStatorCurrentLimit( c.kDriveStatorLimit) .withSupplyCurrentLimit( - c.kDriveSupplyLimit.in(Units.Amps))) + c.kDriveSupplyLimit)) .withFeedback( new FeedbackConfigs() .withSensorToMechanismRatio( @@ -77,7 +78,7 @@ public WheelMoverTalonFX( .withKP(c.kDriveP) .withKI(c.kDriveI) .withKD(c.kDriveD) - .withKV(c.kDriveV.in(Units.Volts))) + .withKV(c.kDriveV)) .withMotionMagic( new MotionMagicConfigs() // Phoenix expects mechanism rotations/sec; we treat the mechanism as the @@ -103,7 +104,7 @@ public WheelMoverTalonFX( new FeedbackConfigs() // CTRE expects motor rotations per mechanism rotation (module rotation). // Our project constant is module rotations per motor rotation. - .withSensorToMechanismRatio(1.0 / c.kTurnConversionFactor)) + .withSensorToMechanismRatio(c.kTurnConversionFactor)) .withSlot0( new Slot0Configs() .withKP(c.kTurnP) @@ -114,11 +115,12 @@ public WheelMoverTalonFX( .withMotionMagic( new MotionMagicConfigs() // Phoenix expects mechanism rotations/sec (module rotations/sec). - .withMotionMagicCruiseVelocity(maxModuleRps(c)) - .withMotionMagicAcceleration(maxModuleRpsPerSec(c)) - .withMotionMagicJerk(maxModuleRpsPerSec2(c))); + .withMotionMagicCruiseVelocity(c.kMaxTurnSpeed) + .withMotionMagicAcceleration(c.kMaxTurnAcceleration) + .withMotionMagicJerk(c.kMaxTurnJerk)); m_turnMotor.getConfigurator().apply(turnConfig); + m_turnMotor.setPosition( turnCANcoder.getAbsolutePosition().getValueAsDouble()); } @@ -135,9 +137,15 @@ protected void setSpeed(LinearVelocity mpsSpeed) { @Override protected void turnWheel(Angle newRotation) { - // CTRE uses rotations for position; convert from radians for a consistent API. - double rotations = newRotation.in(Units.Radians) / (2.0 * Math.PI); - m_turnMotor.setControl(positionRequest.withPosition(rotations)); + m_turnMotor.setControl(positionRequest.withPosition(newRotation)); + } + + @Override + public void drive(Angle angle, LinearVelocity speed) { + setSpeed(speed); + turnWheel(angle); + + logEverything(speed, angle); } @Override @@ -149,27 +157,31 @@ public double getCurrentAngle() { @Override public Angle getAngle() { - // Turn sensor is configured to report module rotations; negate to match project - // convention. - return Angle.ofRelativeUnits(-m_turnMotor.getPosition().getValueAsDouble(), Units.Rotations); + return wrapAngle(m_turnMotor.getPosition().getValue()); } @Override public LinearVelocity getSpeed() { return LinearVelocity.ofRelativeUnits( - convertWheelRotationsToMeters(m_driveMotor.getVelocity().getValueAsDouble()), + convertWheelRotationsToMeters(-m_driveMotor.getVelocity().getValueAsDouble()), Units.MetersPerSecond); } @Override public Distance getDistance() { return Distance.ofRelativeUnits( - convertWheelRotationsToMeters(m_driveMotor.getPosition().getValueAsDouble()), + convertWheelRotationsToMeters(-m_driveMotor.getPosition().getValueAsDouble()), Units.Meters); } /***************************************************************************************************/ + private Angle wrapAngle(Angle angle) { + double radians = angle.in(Units.Radians); + double wrappedRadians = Math.atan2(Math.sin(radians), Math.cos(radians)); + return Units.Radians.of(wrappedRadians); + } + /** * Converts wheel rotations to distance/velocity in meters * Note: With SensorToMechanismRatio configured, motor values are already in @@ -185,7 +197,7 @@ public double getCANCoderAngle() { @Override public Rotation2d getRotation2d() { - return new Rotation2d(getAngle().in(Units.Radians)); + return new Rotation2d(-getAngle().in(Units.Radians)); } @Override @@ -211,13 +223,21 @@ public void reset() { private void logEverything(LinearVelocity requestedMps, Angle requestedAngle) { String base = "Wheels/" + port + "/"; - LinearVelocity mpsSpeed = getSpeed(); - Angle newRotationRad = getAngle(); - Distance distance = getDistance(); - Logger.recordOutput(base + "requestedMps", mpsSpeed.in(Units.MetersPerSecond)); + var currentAngle = getAngle(); + var currentSpeed = getSpeed(); + var currentDistance = getDistance(); + var rawAngle = getCurrentAngle(); + + Logger.recordOutput(base + "requestedMps", requestedMps.in(Units.MetersPerSecond)); Logger.recordOutput(base + "requestedAngle", requestedAngle.in(Units.Degrees)); - Logger.recordOutput(base + "requestedDistance", distance.in(Units.Meters)); + + Logger.recordOutput(base + "currentAngle", currentAngle.in(Units.Degrees)); + Logger.recordOutput(base + "currentSpeed", currentSpeed.in(Units.MetersPerSecond)); + Logger.recordOutput(base + "currentDistance", currentDistance.in(Units.Meters)); + + Logger.recordOutput(base + "rawCurrentAngle", rawAngle); + } // *********************************************************************************************** @@ -249,18 +269,4 @@ private static double maxWheelRpsPerSec2(SwerveConstants c) { // c.kMaxLinearJerk is in meters/sec^3. return c.kMaxLinearJerk / wheelCircumference; } - - private static double maxModuleRps(SwerveConstants c) { - return c.kMaxTurnSpeed.in(Units.RadiansPerSecond) / (2.0 * Math.PI); - } - - private static double maxModuleRpsPerSec(SwerveConstants c) { - return c.kMaxTurnAcceleration.in(Units.RadiansPerSecondPerSecond) / (2.0 * Math.PI); - } - - private static double maxModuleRpsPerSec2(SwerveConstants c) { - // c.kMaxTurnJerk is in radians/sec^3. - return c.kMaxTurnJerk / (2.0 * Math.PI); - } - } diff --git a/src/main/java/frc/robot/subsystem/CameraSubsystem.java b/src/main/java/frc/robot/subsystem/CameraSubsystem.java index ba865c9..0c9fc48 100644 --- a/src/main/java/frc/robot/subsystem/CameraSubsystem.java +++ b/src/main/java/frc/robot/subsystem/CameraSubsystem.java @@ -1,26 +1,24 @@ package frc.robot.subsystem; -import java.io.IOException; import java.util.ArrayList; +import java.util.HashSet; import java.util.List; import java.util.concurrent.ConcurrentLinkedQueue; import org.littletonrobotics.junction.Logger; import autobahn.client.NamedCallback; -import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.constant.PiConstants; -import frc.robot.constant.TopicConstants; +import frc.robot.constant.BotConstants; +import frc.robot.constant.CommunicationConstants; import frc.robot.util.CustomUtil; import lombok.AllArgsConstructor; import lombok.Getter; -import frc4765.proto.sensor.Apriltags.AprilTagData; +import frc4765.proto.sensor.Apriltags.ProcessedTag; import frc4765.proto.sensor.GeneralSensorDataOuterClass.GeneralSensorData; import frc4765.proto.sensor.GeneralSensorDataOuterClass.SensorName;; @@ -28,42 +26,39 @@ public class CameraSubsystem extends SubsystemBase { private static CameraSubsystem self; + private static final long kTagTimeoutMs = 200; + /** * Queue of timed tags. This is fully instant concurrently because you only need * to retrieve the head of the queue when reading. Therefore, the writer * (another thread) only adds to the end of the queue. Therefore, they don't * contradict each other. */ - private final ConcurrentLinkedQueue q = new ConcurrentLinkedQueue<>(); - - private static final String FIELD_LAYOUT_DEPLOY_FILE = "2026-rebuilt-welded.json"; - private static final AprilTagFieldLayout FIELD_LAYOUT = loadFieldLayout(); - - /** - * Load the field layout from the deploy file. - * - * @return The field layout. - * @throws IOException If the field layout cannot be loaded. - * TODO: figure out why the default way does not work. - * - * How it is meant to work: - * AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); - */ - private static AprilTagFieldLayout loadFieldLayout() { - var path = Filesystem.getDeployDirectory().toPath().resolve(FIELD_LAYOUT_DEPLOY_FILE); - try { - return AprilTagFieldLayout.loadFromResource(path.toString()); - } catch (IOException e) { - e.printStackTrace(); - return null; - } - } + private volatile ConcurrentLinkedQueue q = new ConcurrentLinkedQueue<>(); + private HashSet localQ = new HashSet<>(); @Getter @AllArgsConstructor - private static class TimedTags { - public AprilTagData tags; + private static class TimedTag { + public ProcessedTag tag; public long timestamp; + + @Override + public boolean equals(Object obj) { + if (this == obj) { + return true; + } + if (obj == null || getClass() != obj.getClass()) { + return false; + } + TimedTag other = (TimedTag) obj; + return tag.getId() == other.tag.getId(); + } + + @Override + public int hashCode() { + return 31 * tag.getId(); + } } public static CameraSubsystem GetInstance() { @@ -75,7 +70,7 @@ public static CameraSubsystem GetInstance() { } public CameraSubsystem() { - Robot.getCommunicationClient().subscribe(TopicConstants.kCameraTagsViewTopic, + Robot.getCommunicationClient().subscribe(CommunicationConstants.kCameraTagsViewTopic, NamedCallback.FromConsumer(this::subscription)); } @@ -83,8 +78,12 @@ public void subscription(byte[] payload) { GeneralSensorData data = CustomUtil.DeserializeSilent(payload, GeneralSensorData.class); if (data == null || data.getSensorName() != SensorName.APRIL_TAGS) return; - - q.add(new TimedTags(data.getApriltags(), System.currentTimeMillis())); + if (!data.getApriltags().hasWorldTags()) + return; + long now = System.currentTimeMillis(); + for (ProcessedTag tag : data.getApriltags().getWorldTags().getTagsList()) { + q.add(new TimedTag(tag, now)); + } } @Override @@ -92,25 +91,30 @@ public void periodic() { List positionsRobot = new ArrayList<>(); List positionsReal = new ArrayList<>(); - TimedTags timedTags; - while ((timedTags = q.poll()) != null) { - for (var tag : timedTags.getTags().getWorldTags().getTagsList()) { - int id = tag.getId(); - double confidence = tag.getConfidence(); - var posRaw = tag.getPositionWPILib(); - var rotRaw = tag.getRotationWPILib(); + localQ.removeIf(timedTag -> System.currentTimeMillis() - timedTag.timestamp > kTagTimeoutMs); - Pose2d positionRobot = new Pose2d( - (double) posRaw.getX(), (double) posRaw.getY(), - new Rotation2d((double) rotRaw.getDirectionX().getX(), (double) rotRaw.getDirectionX().getY())); + TimedTag timedTag; + while ((timedTag = q.poll()) != null) { + localQ.add(timedTag); + } - Pose3d positionField = FIELD_LAYOUT.getTagPose(id).orElse(new Pose3d()); + for (var t : localQ) { + var tag = t.getTag(); + int id = tag.getId(); + double confidence = tag.getConfidence(); + var posRaw = tag.getPositionWPILib(); + var rotRaw = tag.getRotationWPILib(); - positionsRobot.add(positionRobot); - positionsReal.add(positionField); + Pose2d positionRobot = new Pose2d( + (double) posRaw.getX(), (double) posRaw.getY(), + new Rotation2d((double) rotRaw.getDirectionX().getX(), (double) rotRaw.getDirectionX().getY())); - Logger.recordOutput("Camera/Tags/" + id + "/Confidence", confidence); - } + Pose3d positionField = BotConstants.kFieldLayout.getTagPose(id).orElse(new Pose3d()); + + positionsRobot.add(positionRobot); + positionsReal.add(positionField); + + Logger.recordOutput("Camera/Tags/Confidences/" + id, confidence); } Logger.recordOutput("Camera/Tags/PositionsRobot", positionsRobot.toArray(new Pose2d[0])); diff --git a/src/main/java/frc/robot/subsystem/ClimberSubsystem.java b/src/main/java/frc/robot/subsystem/ClimberSubsystem.java new file mode 100644 index 0000000..437b622 --- /dev/null +++ b/src/main/java/frc/robot/subsystem/ClimberSubsystem.java @@ -0,0 +1,146 @@ +package frc.robot.subsystem; + +import static edu.wpi.first.units.Units.Meters; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constant.ClimberConstants; +import frc.robot.util.LocalMath; + +public class ClimberSubsystem extends SubsystemBase { + + private static ClimberSubsystem self; + + // LEFT MOTOR IS THE LEADER + private SparkMax m_leftMotor; + private SparkMax m_rightMotor; + private PIDController m_pid; + private ElevatorFeedforward m_feedforward; + + private Distance m_setpoint = ClimberConstants.kStartingHeight; + private Distance m_currentPosition = ClimberConstants.kStartingHeight; + + public static ClimberSubsystem GetInstance() { + if (self == null) { + self = new ClimberSubsystem(); + } + + return self; + } + + public ClimberSubsystem() { + m_leftMotor = new SparkMax(ClimberConstants.kLeftMotorID, MotorType.kBrushless); + m_rightMotor = new SparkMax(ClimberConstants.kRightMotorID, MotorType.kBrushless); + + m_pid = new PIDController( + ClimberConstants.kP, + ClimberConstants.kI, + ClimberConstants.kD); + m_pid.setTolerance(ClimberConstants.kTolerance); + m_feedforward = new ElevatorFeedforward(ClimberConstants.kS, ClimberConstants.kG, ClimberConstants.kV, + ClimberConstants.kA); + + configureMotors(); + } + + private void configureMotors() { + SparkMaxConfig leftMotorConfig = new SparkMaxConfig(); + leftMotorConfig.inverted(ClimberConstants.kLeftMotorInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(30).encoder.positionConversionFactor(ClimberConstants.kGearHeightRatio); + + m_leftMotor.configure(leftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + m_leftMotor.getEncoder().setPosition(ClimberConstants.kStartingHeight.in(Meters)); + + SparkMaxConfig rightMotorConfig = new SparkMaxConfig(); + rightMotorConfig.inverted(ClimberConstants.kRightMotorInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(30).encoder.positionConversionFactor(ClimberConstants.kGearHeightRatio); + + m_rightMotor.configure(rightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + m_rightMotor.getEncoder().setPosition(ClimberConstants.kStartingHeight.in(Meters)); + + m_pid.setIZone(ClimberConstants.kIZone); + } + + public void setHeight(Distance height) { + if (height.gt(ClimberConstants.kMaxHeight)) { + System.out.println("WARNING: tried to exceed elevator max height: " + height.in(Meters)); + height = ClimberConstants.kMaxHeight; + } else if (height.lt(ClimberConstants.kMinHeight)) { + System.out.println("WARNING: tried to exceed elevator min height: " + height.in(Meters)); + height = ClimberConstants.kMinHeight; + } + m_setpoint = height; + } + + public Distance getAverageHeight() { + double height = (m_leftMotor.getEncoder().getPosition() + m_rightMotor.getEncoder().getPosition()) / 2.0; + return Distance.ofRelativeUnits(height, Meters); + } + + private double calculateSpeed(Distance setpoint) { + double motorPowerPid = m_pid.calculate(getAverageHeight().in(Meters), setpoint.in(Meters)); + double ff = calculateFeedForwardValue(m_feedforward); + return MathUtil.clamp(motorPowerPid + ff, -1, 1); + } + + public boolean atTarget() { + return Math.abs(getAverageHeight().minus(m_setpoint).in(Meters)) < ClimberConstants.kTolerance; + } + + public void stopMotors() { + m_leftMotor.stopMotor(); + m_rightMotor.stopMotor(); + } + + private double calculateFeedForwardValue(ElevatorFeedforward feedforward) { + double currentVelocity = m_leftMotor.getEncoder().getVelocity(); + return feedforward.calculate(currentVelocity); + } + + private Distance rampSetpoint(Distance set) { + return Distance.ofRelativeUnits( + LocalMath.rampSetpoint(set.in(Meters), m_currentPosition.in(Meters), ClimberConstants.kMaxSetpointRamp), + Meters); + } + + private Distance calculateTemporarySetpoint(Distance set) { + // set = smoothRestingHeight(set); + set = rampSetpoint(set); + return set; + } + + public void resetIAccum() { + m_leftMotor.getClosedLoopController().setIAccum(0); + m_rightMotor.getClosedLoopController().setIAccum(0); + } + + @Override + public void periodic() { + Logger.recordOutput("Climber/Setpoint", m_setpoint.in(Meters)); + Logger.recordOutput("Climber/CurrentPosition", m_currentPosition.in(Meters)); + Logger.recordOutput("Climber/AtTarget", atTarget()); + Logger.recordOutput("Climber/LeftMotor", m_leftMotor.getEncoder().getPosition()); + Logger.recordOutput("Climber/RightMotor", m_rightMotor.getEncoder().getPosition()); + + m_currentPosition = calculateTemporarySetpoint(m_setpoint); + + double speed = calculateSpeed(m_currentPosition); + + m_leftMotor.setVoltage(speed * 12); + m_rightMotor.setVoltage(speed * 12); + } +} diff --git a/src/main/java/frc/robot/subsystem/GlobalPosition.java b/src/main/java/frc/robot/subsystem/GlobalPosition.java index 56c1f07..7d33177 100644 --- a/src/main/java/frc/robot/subsystem/GlobalPosition.java +++ b/src/main/java/frc/robot/subsystem/GlobalPosition.java @@ -7,17 +7,30 @@ import autobahn.client.NamedCallback; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.constant.TopicConstants; +import frc.robot.constant.CommunicationConstants; +import frc.robot.util.AimPoint; import frc4765.proto.util.Position.RobotPosition; +import lombok.Getter; public class GlobalPosition extends SubsystemBase { private static volatile long lastUpdateTime; + private static volatile double positionUpdateHz; private static GlobalPosition self; - private static Pose2d position; - private static ChassisSpeeds positionVelocity; + private static Pose2d position = new Pose2d(12.94, 3.52, new Rotation2d(1, 0)); + private static ChassisSpeeds positionVelocity = new ChassisSpeeds(0, 0, 0); + + @Getter + private static boolean isValid = false; + private static final long kPositionUpdateTimeoutMs = 1000; + + public static enum GMFrame { + kFieldRelative, + kRobotRelative, + } public static GlobalPosition GetInstance() { if (self == null) { @@ -27,7 +40,8 @@ public static GlobalPosition GetInstance() { } public GlobalPosition() { - Robot.getCommunicationClient().subscribe(TopicConstants.kPoseSubscribeTopic, + lastUpdateTime = System.currentTimeMillis(); + Robot.getCommunicationClient().subscribe(CommunicationConstants.kPoseSubscribeTopic, NamedCallback.FromConsumer(this::subscription)); } @@ -39,6 +53,12 @@ public void subscription(byte[] payload) { var direction = position.getPosition2D().getDirection(); var rotationSpeed = position.getPosition2D().getRotationSpeedRadS(); + if (pose == null || (direction.getX() == 0 && direction.getY() == 0) || Double.isNaN(direction.getX()) + || Double.isNaN(direction.getY())) { + System.out.println("Invalid position or direction! + " + pose + " + " + direction); + return; + } + GlobalPosition.position = new Pose2d(pose.getX(), pose.getY(), new Rotation2d(direction.getX(), direction.getY())); @@ -46,7 +66,9 @@ public void subscription(byte[] payload) { positionVelocity = new ChassisSpeeds(velocity.getX(), velocity.getY(), rotationSpeed); - lastUpdateTime = (long) System.currentTimeMillis(); + long now = System.currentTimeMillis(); + positionUpdateHz = 1000.0 / ((double) (now - lastUpdateTime)); + lastUpdateTime = now; } catch (InvalidProtocolBufferException e) { e.printStackTrace(); return; @@ -57,14 +79,75 @@ public static Pose2d Get() { return position; } - public static ChassisSpeeds GetVelocity() { + public static Translation2d Velocity2d(GMFrame velocityType) { + var velocity = Velocity(velocityType); + return new Translation2d(velocity.vxMetersPerSecond, velocity.vyMetersPerSecond); + } + + public static ChassisSpeeds Velocity(GMFrame velocityType) { + if (velocityType == GMFrame.kFieldRelative) { + return positionVelocity; + } else if (velocityType == GMFrame.kRobotRelative) { + return VelocityInFrame(position.getRotation()); + } + return positionVelocity; } + /** + * Converts the velocity from the field frame to the robot frame. + * + * @param rotationOfRobot The rotation of the robot in the field frame. This is + * the angle of the robot in the field frame. + * @return The velocity in the robot frame. + */ + public static ChassisSpeeds VelocityInFrame(Rotation2d rotationOfRobot) { + if (positionVelocity == null) { + return null; + } + + return ChassisSpeeds.fromFieldRelativeSpeeds(positionVelocity, rotationOfRobot); + } + + public static Pose2d ToRobotRelative(Pose2d pose) { + return pose.relativeTo(position); + } + + public static Translation2d ToRobotRelative(Translation2d translation) { + return translation.rotateBy(position.getRotation()); + } + @Override public void periodic() { Logger.recordOutput("Global/pose", position); Logger.recordOutput("Global/velocity", positionVelocity); - Logger.recordOutput("Global/lastUpdateTime", lastUpdateTime); + if (positionUpdateHz < 100) { + Logger.recordOutput("Global/positionUpdateHz", positionUpdateHz); + } + + for (AimPoint.ZoneName zoneName : AimPoint.ZoneName.values()) { + AimPoint.logZoneForAdvantageScope(zoneName, "Global/Zones/All"); + } + + if (position != null) { + AimPoint.ZoneName activeZone = AimPoint.getZone(position); + AimPoint.logZoneForAdvantageScope(activeZone, "Global/Zones/Active"); + + var target = AimPoint.getTarget(activeZone); + Logger.recordOutput( + "Global/Zones/Active/LineToTarget", + new Pose2d[] { + new Pose2d(position.getTranslation(), new Rotation2d()), + new Pose2d(target, new Rotation2d()) + }); + } + + if (System.currentTimeMillis() - lastUpdateTime > kPositionUpdateTimeoutMs) { + isValid = false; + } else { + isValid = true; + } + + Logger.recordOutput("Global/Position/IsValid", isValid); } } diff --git a/src/main/java/frc/robot/subsystem/IndexSubsystem.java b/src/main/java/frc/robot/subsystem/IndexSubsystem.java new file mode 100644 index 0000000..1c1f531 --- /dev/null +++ b/src/main/java/frc/robot/subsystem/IndexSubsystem.java @@ -0,0 +1,54 @@ +package frc.robot.subsystem; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constant.IndexConstants; + +public class IndexSubsystem extends SubsystemBase { + private static IndexSubsystem instance; + + private final SparkFlex m_indexMotor; + + public static IndexSubsystem GetInstance() { + if (instance == null) { + instance = new IndexSubsystem(); + } + return instance; + } + + private IndexSubsystem() { + m_indexMotor = new SparkFlex(IndexConstants.kIndexMotorID, MotorType.kBrushless); + configureMotor(); + } + + private void configureMotor() { + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(IdleMode.kBrake); + config.inverted(false); + + m_indexMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void runMotor(double speed) { + m_indexMotor.set(MathUtil.clamp(speed, -1.0, 1.0)); + } + + public void runMotor() { + runMotor(IndexConstants.kIndexMotorSpeed); + } + + public void reverseRunMotor() { + runMotor(-IndexConstants.kIndexMotorSpeed); + } + + public void stopMotor() { + m_indexMotor.set(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystem/IntakeSubsystem.java b/src/main/java/frc/robot/subsystem/IntakeSubsystem.java new file mode 100644 index 0000000..ecf8f50 --- /dev/null +++ b/src/main/java/frc/robot/subsystem/IntakeSubsystem.java @@ -0,0 +1,110 @@ +package frc.robot.subsystem; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constant.IntakeConstants; +import frc.robot.constant.IntakeConstants.WristRaiseLocation; + +public class IntakeSubsystem extends SubsystemBase { + private static IntakeSubsystem instance; + + private final SparkMax m_intakeIntakerMotor; + private final SparkMax m_intakeWristMotor; + + private Rotation2d m_wristSetpoint = IntakeConstants.WristRaiseLocation.BOTTOM.position; + + public static IntakeSubsystem GetInstance() { + if (instance == null) { + instance = new IntakeSubsystem(IntakeConstants.intakeIntakerMotorID, IntakeConstants.intakeWristMotorID); + } + + return instance; + } + + private IntakeSubsystem(int intakeMotorID, int wristMotorID) { + m_intakeIntakerMotor = new SparkMax(intakeMotorID, MotorType.kBrushless); + m_intakeWristMotor = new SparkMax(wristMotorID, MotorType.kBrushless); + configureIntaker(); + configureWrist(); + } + + private void configureIntaker() { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(IdleMode.kBrake); + config.inverted(IntakeConstants.intakeIntakerInverted); + + m_intakeIntakerMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + private void configureWrist() { + SparkMaxConfig wristConfig = new SparkMaxConfig(); + wristConfig.smartCurrentLimit(IntakeConstants.intakeWristCurrentLimit); + wristConfig.inverted(IntakeConstants.intakeWristInverted); + wristConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + wristConfig.idleMode(IdleMode.kBrake); + wristConfig.closedLoop.pid( + IntakeConstants.intakeWristP, + IntakeConstants.intakeWristI, + IntakeConstants.intakeWristD) + .iZone(IntakeConstants.intakeWristIZone); + + wristConfig.absoluteEncoder.zeroOffset(IntakeConstants.intakeWristOffset.getRotations()); + + m_intakeWristMotor.configure(wristConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + } + + private double calculateFeedForward() { + return IntakeConstants.intakeWristFeedForwardK + * Math.cos(getWristPosition().getRadians()); + } + + public Rotation2d getWristPosition() { + return Rotation2d.fromRotations(m_intakeWristMotor.getAbsoluteEncoder().getPosition()); + } + + private void setWristPosition(Rotation2d position) { + m_wristSetpoint = position; + } + + public void setWristPosition(WristRaiseLocation location) { + setWristPosition(location.position); + } + + public void runIntakeMotor(double speed) { + m_intakeIntakerMotor.set(MathUtil.clamp(speed, -1.0, 1.0)); + } + + public void stopIntakeMotor() { + m_intakeIntakerMotor.set(0.0); + } + + @Override + public void periodic() { + if (m_wristSetpoint != null) { + m_intakeWristMotor.getClosedLoopController().setSetpoint( + m_wristSetpoint.getRotations(), + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + calculateFeedForward()); + } + + Logger.recordOutput("IntakeSubsystem/WristPosition", getWristPosition().getRotations()); + Logger.recordOutput("IntakeSubsystem/WristSetpoint", m_wristSetpoint.getRotations()); + Logger.recordOutput("IntakeSubsystem/FeedForward", calculateFeedForward()); + } +} diff --git a/src/main/java/frc/robot/subsystem/LightsSubsystem.java b/src/main/java/frc/robot/subsystem/LightsSubsystem.java new file mode 100644 index 0000000..747ceab --- /dev/null +++ b/src/main/java/frc/robot/subsystem/LightsSubsystem.java @@ -0,0 +1,241 @@ +package frc.robot.subsystem; + +import com.ctre.phoenix6.hardware.CANdle; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.command.util.PollingCommand; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constant.LEDConstants; +import frc.robot.util.lighting.effects.BlinkEffect; +import frc.robot.util.lighting.effects.BreatheEffect; +import frc.robot.util.lighting.effects.ChaseEffect; +import frc.robot.util.lighting.effects.ConvergingArrowsEffect; +import frc.robot.util.lighting.effects.LarsonScannerEffect; +import frc.robot.util.lighting.effects.MorseCodeEffect; +import frc.robot.util.lighting.effects.ProgressBarEffect; +import frc.robot.util.lighting.effects.RainbowEffect; +import frc.robot.util.lighting.effects.SolidEffect; +import frc.robot.util.lighting.internal.CandleLightsTransport; +import frc.robot.util.lighting.internal.LightsTransport; +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.EffectHandle; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightZone; +import frc.robot.util.lighting.LightsApi; +import frc.robot.util.lighting.LightsEngine; + +import java.util.ArrayList; +import java.util.List; + +import org.littletonrobotics.junction.Logger; + +public final class LightsSubsystem extends SubsystemBase implements LightsApi { + private static LightsSubsystem instance; + + public static LightsSubsystem GetInstance() { + if (instance == null) { + instance = new LightsSubsystem(); + } + return instance; + } + + private final CANdle candle; + private final LightsEngine engine; + private final LightsTransport transport; + private final List pollingCommands; + private final PollingCommand pollingCommand; + + public LightsSubsystem() { + + this( + new CANdle(LEDConstants.candleCANId), + new LightsEngine(LEDConstants.ledCount, LEDConstants.maxSolidWritesPerCycle)); + } + + LightsSubsystem(CANdle candle, LightsEngine engine) { + this(candle, engine, new CandleLightsTransport(candle)); + } + + LightsSubsystem(CANdle candle, LightsEngine engine, LightsTransport transport) { + this.candle = candle; + this.engine = engine; + this.transport = transport; + this.pollingCommands = new ArrayList<>(); + + this.pollingCommand = new PollingCommand(this, () -> pollingCommands); + super.setDefaultCommand(pollingCommand); + } + + public LedRange rangeOf(LightZone zone) { + return zone.range(); + } + + @Override + public EffectHandle addSolid(LedRange range, LedColor color, int priority, BlendMode blend) { + return engine.addEffect(new SolidEffect(range, color, priority, blend)); + } + + @Override + public EffectHandle addProgressBar( + LedRange range, + LedColor fill, + LedColor emptyOrNull, + int priority, + BlendMode blend) { + return engine.addEffect(new ProgressBarEffect(range, fill, emptyOrNull, priority, blend)); + } + + @Override + public EffectHandle addBlink( + LedRange range, + LedColor on, + LedColor off, + double hz, + int priority, + BlendMode blend) { + return engine.addEffect(new BlinkEffect(range, on, off, hz, priority, blend)); + } + + @Override + public EffectHandle addBreathe( + LedRange range, + LedColor color, + double hz, + double minScalar, + double maxScalar, + int priority, + BlendMode blend) { + return engine.addEffect(new BreatheEffect(range, color, hz, minScalar, maxScalar, priority, blend)); + } + + @Override + public EffectHandle addChase( + LedRange range, + LedColor color, + int width, + double hz, + boolean wrap, + int priority, + BlendMode blend) { + return engine.addEffect(new ChaseEffect(range, color, width, hz, wrap, priority, blend)); + } + + @Override + public EffectHandle addRainbow( + LedRange range, + double hz, + double brightness, + int priority, + BlendMode blend) { + return engine.addEffect(new RainbowEffect(range, hz, brightness, priority, blend)); + } + + @Override + public EffectHandle addLarsonScanner( + LedRange range, + LedColor color, + int eyeWidth, + int trailLength, + double hz, + int priority, + BlendMode blend) { + return engine.addEffect(new LarsonScannerEffect(range, color, eyeWidth, trailLength, hz, priority, blend)); + } + + @Override + public EffectHandle addMorseCode( + LedRange range, + String message, + LedColor onColor, + LedColor offColor, + double unitSeconds, + int priority, + BlendMode blend) { + return engine.addEffect( + new MorseCodeEffect(range, message, onColor, offColor, unitSeconds, priority, blend)); + } + + @Override + public EffectHandle addConvergingArrows( + LedRange range, + LedColor color, + boolean wedge, + int priority, + BlendMode blend) { + return engine.addEffect(new ConvergingArrowsEffect(range, color, wedge, priority, blend)); + } + + @Override + public boolean setInput(EffectHandle handle, T inputArg) { + return engine.setInput(handle, inputArg); + } + + @Override + public boolean setProgress(EffectHandle handle, double progress01) { + return engine.setProgress(handle, progress01); + } + + @Override + public boolean setEnabled(EffectHandle handle, boolean enabled) { + return engine.setEnabled(handle, enabled); + } + + @Override + public boolean setPriority(EffectHandle handle, int priority) { + return engine.setPriority(handle, priority); + } + + @Override + public boolean removeEffect(EffectHandle handle) { + return engine.removeEffect(handle); + } + + @Override + public void clearEffects() { + engine.clearEffects(); + } + + /** + * Adds commands to the polling list run by the default {@link PollingCommand}. + * Does not replace the default command; the single default is always the + * poller that runs these. + */ + public void addLightsCommand(Command... commands) { + for (Command command : commands) { + if (pollingCommand.isCommandAlreadyInserted(command)) { + continue; + } + + pollingCommands.add(command); + } + } + + /** + * Adds commands to the polling list run by the default {@link PollingCommand}. + * Does not replace the default command; the single default is always the + * poller that runs these. + */ + public void addLightsCommands(Command command) { + addLightsCommand(command); + } + + @Override + public void periodic() { + LightsEngine.RenderResult result = engine.render(Timer.getFPGATimestamp()); + + if (result.hasWrites()) { + transport.writeSegments(result.segments()); + } + + Logger.recordOutput("Lights/ChangedLedCount", result.changedLedCount()); + Logger.recordOutput("Lights/SegmentWriteCount", result.segments().size()); + Logger.recordOutput("Lights/AdaptiveCompressionActive", result.adaptiveCompressionActive()); + Logger.recordOutput("Lights/ActiveEffectCount", result.activeEffectCount()); + } + + public CANdle getCandle() { + return candle; + } + +} diff --git a/src/main/java/frc/robot/subsystem/OdometrySubsystem.java b/src/main/java/frc/robot/subsystem/OdometrySubsystem.java index 1dd267d..e188dfc 100644 --- a/src/main/java/frc/robot/subsystem/OdometrySubsystem.java +++ b/src/main/java/frc/robot/subsystem/OdometrySubsystem.java @@ -4,10 +4,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constant.TopicConstants; -import frc.robot.hardware.AHRSGyro; +import frc.robot.constant.CommunicationConstants; +import frc.robot.hardware.PigeonGyro; +import frc.robot.hardware.UnifiedGyro; import frc4765.proto.sensor.GeneralSensorDataOuterClass.GeneralSensorData; import frc4765.proto.sensor.Odometry.OdometryData; import frc4765.proto.util.Position.Position2d; @@ -24,60 +26,66 @@ public class OdometrySubsystem extends SubsystemBase implements IDataClass { public Pose2d[] timedPositions = new Pose2d[2]; public long[] timestamps = new long[2]; - public static OdometrySubsystem GetInstance() { + public static OdometrySubsystem GetInstance(IGyroscopeLike gyro, SwerveSubsystem swerve) { if (self == null) { - self = new OdometrySubsystem(); + self = new OdometrySubsystem(gyro, swerve); } return self; } - public OdometrySubsystem() { - this.swerve = SwerveSubsystem.GetInstance(); - this.gyro = AHRSGyro.GetInstance(); + public static OdometrySubsystem GetInstance(IGyroscopeLike gyro) { + return GetInstance(gyro, SwerveSubsystem.GetInstance()); + } + public static OdometrySubsystem GetInstance() { + return GetInstance(UnifiedGyro.GetInstance(), SwerveSubsystem.GetInstance()); + } + + public OdometrySubsystem(IGyroscopeLike gyro, SwerveSubsystem swerve) { + this.gyro = gyro; + this.swerve = swerve; this.odometry = new SwerveDriveOdometry( swerve.getKinematics(), - getGlobalGyroRotation(), + gyro.getRotation2d(), swerve.getSwerveModulePositions(), new Pose2d(5, 5, new Rotation2d())); } public void setOdometryPosition(Pose2d newPose) { odometry.resetPosition( - getGlobalGyroRotation(), + gyro.getRotation2d(), swerve.getSwerveModulePositions(), newPose); + timedPositions[0] = newPose; + timedPositions[1] = newPose; } - public Rotation2d getGlobalGyroRotation() { - return Rotation2d.fromDegrees(-this.gyro.getYaw()); + private Pose2d getLatestPosition() { + return timedPositions[1]; } - @Override - public void periodic() { - timedPositions[0] = timedPositions[1]; - timestamps[0] = timestamps[1]; - timestamps[1] = System.currentTimeMillis(); + private Transform2d getPoseDifference() { + return timedPositions[1].minus(timedPositions[0]); + } - var positions = swerve.getSwerveModulePositions(); - timedPositions[1] = odometry.update(getGlobalGyroRotation(), positions); - Logger.recordOutput("odometry/pos", timedPositions[1]); + private double getTimeDifference() { + return ((timestamps[1] - timestamps[0]) + (System.currentTimeMillis() - timestamps[1])) / 1000.0; } @Override public byte[] getRawConstructedProtoData() { var all = GeneralSensorData.newBuilder().setOdometry(OdometryData.newBuilder()); - all.setSensorId("odom"); - - var positionChange = timedPositions[1].minus(timedPositions[0]); - var timeChange = ((timestamps[1] - timestamps[0]) + (System.currentTimeMillis() - timestamps[1])) / 1000.0; + all.setSensorId(CommunicationConstants.kOdometrySensorId); - var latestPosition = timedPositions[1]; + var positionChange = getPoseDifference(); + var timeChange = getTimeDifference(); + var latestPosition = getLatestPosition(); var rotation = Vector2.newBuilder().setX((float) latestPosition.getRotation().getCos()) .setY((float) latestPosition.getRotation().getSin()) .build(); + var pose = Position2d.newBuilder() .setPosition( Vector2.newBuilder().setX((float) latestPosition.getX()).setY((float) latestPosition.getY()).build()) @@ -88,8 +96,6 @@ public byte[] getRawConstructedProtoData() { .setY((float) SwerveSubsystem.GetInstance().getChassisSpeeds().vyMetersPerSecond) .build(); - Logger.recordOutput("Swerve/Velocity", SwerveSubsystem.GetInstance().getChassisSpeeds()); - var positionChangeVec = Vector2.newBuilder().setX((float) positionChange.getX()).setY((float) positionChange.getY()) .build(); @@ -106,6 +112,18 @@ public byte[] getRawConstructedProtoData() { @Override public String getPublishTopic() { - return TopicConstants.kOdometryPublishTopic; + return CommunicationConstants.kOdometryPublishTopic; + } + + @Override + public void periodic() { + timedPositions[0] = timedPositions[1]; + timestamps[0] = timestamps[1]; + timestamps[1] = System.currentTimeMillis(); + + var positions = swerve.getSwerveModulePositions(); + timedPositions[1] = odometry.update(gyro.getRotation2d(), positions); + + Logger.recordOutput("Odometry/Position", timedPositions[1]); } } diff --git a/src/main/java/frc/robot/subsystem/PathPlannerSubsystem.java b/src/main/java/frc/robot/subsystem/PathPlannerSubsystem.java new file mode 100644 index 0000000..6248048 --- /dev/null +++ b/src/main/java/frc/robot/subsystem/PathPlannerSubsystem.java @@ -0,0 +1,110 @@ +package frc.robot.subsystem; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinding; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constant.PathPlannerConstants; +import frc.robot.constant.PathPlannerConstants.SelectedAuto; +import frc.robot.subsystem.GlobalPosition.GMFrame; +import org.littletonrobotics.junction.Logger; + +public final class PathPlannerSubsystem extends SubsystemBase { + private final RobotConfig robotConfig; + private volatile SelectedAuto selectedAuto; + + private static PathPlannerSubsystem self; + + public Command currentAutoCommand; + + private String[] allAutos; + + public static PathPlannerSubsystem GetInstance() { + if (self == null) { + self = new PathPlannerSubsystem(); + } + + return self; + } + + public PathPlannerSubsystem() { + Pathfinding.setPathfinder(new LocalADStar()); + CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); + + this.robotConfig = loadRobotConfig(); + configureAutoBuilder(); + + this.selectedAuto = new SelectedAuto(PathPlannerSubsystem::shouldFlipForAlliance); + this.allAutos = AutoBuilder.getAllAutoNames().toArray(new String[0]); + } + + private RobotConfig loadRobotConfig() { + try { + return RobotConfig.fromGUISettings(); + } catch (Exception e) { + throw new RuntimeException("Failed to load RobotConfig from GUI settings", e); + } + } + + private void configureAutoBuilder() { + AutoBuilder.configure( + () -> GlobalPosition.Get(), + OdometrySubsystem.GetInstance()::setOdometryPosition, + () -> GlobalPosition.Velocity(GMFrame.kRobotRelative), + (speeds, feedforwards) -> SwerveSubsystem.GetInstance().drive(speeds, SwerveSubsystem.DriveType.RAW), + PathPlannerConstants.defaultPathfindingController, + robotConfig, + PathPlannerSubsystem::shouldFlipForAlliance, + SwerveSubsystem.GetInstance()); + } + + public Command getAutoCommand() { + if (!isSelectedAutoValid()) { + return Commands.none(); + } + + return selectedAuto.getCurrentAuto().get(); + } + + public boolean isSelectedAutoValid() { + return selectedAuto.getCurrentAuto().isPresent(); + } + + public Command getAndInitAutoCommand(boolean pathfindIfNotAtStart) { + if (!isSelectedAutoValid()) { + return Commands.none(); + } + + currentAutoCommand = selectedAuto.getCurrentAuto().get(); + Pose2d[] pathPoses = selectedAuto.getPathPoses(0); + if (pathfindIfNotAtStart && pathPoses.length > 0 && pathPoses[0].getTranslation() + .getDistance(GlobalPosition.Get().getTranslation()) > PathPlannerConstants.distanceConsideredOffTarget + .in(Units.Meters)) { + currentAutoCommand = AutoBuilder.pathfindToPose(pathPoses[0], + PathPlannerConstants.defaultPathfindingConstraints); + } + + return currentAutoCommand; + } + + private static boolean shouldFlipForAlliance() { + return DriverStation.getAlliance().orElse(Alliance.Blue) != Alliance.Red; + } + + @Override + public void periodic() { + Logger.recordOutput("PathPlanner/CurrentPath", selectedAuto.getAllPathPoses()); + Logger.recordOutput("PathPlanner/CurrentSelectedAuto", selectedAuto.getName()); + Logger.recordOutput("PathPlanner/SelectedAutoValid", selectedAuto.getCurrentAuto().isPresent()); + Logger.recordOutput("PathPlanner/ValidNames/Autos", allAutos); + } +} diff --git a/src/main/java/frc/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/robot/subsystem/ShooterSubsystem.java new file mode 100644 index 0000000..88271af --- /dev/null +++ b/src/main/java/frc/robot/subsystem/ShooterSubsystem.java @@ -0,0 +1,189 @@ +package frc.robot.subsystem; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constant.ShooterConstants; +import lombok.Setter; + +public class ShooterSubsystem extends SubsystemBase { + private static final double kStopVelocityThresholdRpm = 1e-3; + private static ShooterSubsystem instance; + + private final SparkFlex leaderMotor, followerMotor; + private final SparkClosedLoopController leaderClosedLoopController, followerClosedLoopController; + private final RelativeEncoder leaderEncoder, followerEncoder; + + private AngularVelocity lastShooterVelocitySetpoint; + + @Setter + private static boolean isGpsAssistEnabled = true; + + public static boolean getIsGpsAssistEnabled() { + return isGpsAssistEnabled; + } + + public static ShooterSubsystem GetInstance() { + if (instance == null) { + instance = new ShooterSubsystem(ShooterConstants.kShooterCanId, ShooterConstants.kShooterMotorType, + ShooterConstants.kShooterCanIdFollower, ShooterConstants.kShooterMotorTypeFollower); + } + + return instance; + } + + public ShooterSubsystem( + int leaderCanId, + MotorType leaderMotorType, + int followerCanId, + MotorType followerMotorType) { + this.leaderMotor = new SparkFlex(leaderCanId, leaderMotorType); + this.followerMotor = new SparkFlex(followerCanId, followerMotorType); + + this.leaderClosedLoopController = leaderMotor.getClosedLoopController(); + this.followerClosedLoopController = followerMotor.getClosedLoopController(); + this.leaderEncoder = leaderMotor.getEncoder(); + this.followerEncoder = followerMotor.getEncoder(); + + SparkMaxConfig leaderConfig = new SparkMaxConfig(); + leaderConfig + .inverted(ShooterConstants.kShooterLeaderReversed) + .smartCurrentLimit(ShooterConstants.kShooterCurrentLimit) + .idleMode(IdleMode.kCoast); + + SparkMaxConfig followerConfig = new SparkMaxConfig(); + followerConfig + .inverted(ShooterConstants.kShooterFollowerReversed) + .smartCurrentLimit(ShooterConstants.kShooterCurrentLimit) + .idleMode(IdleMode.kCoast); + + double mechanismRpmPerMotorRpm = 1.0 / ShooterConstants.kShooterMotorRotationsPerRotation; + + // Spark MAX native velocity is RPM; keep encoder values in mechanism RPM. + leaderConfig.encoder.velocityConversionFactor(mechanismRpmPerMotorRpm); + followerConfig.encoder.velocityConversionFactor(mechanismRpmPerMotorRpm); + + leaderConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(ShooterConstants.kShooterP, ShooterConstants.kShooterI, ShooterConstants.kShooterD) + .iZone(ShooterConstants.kShooterIZ); + followerConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(ShooterConstants.kShooterFollowerP, ShooterConstants.kShooterI, ShooterConstants.kShooterD) + .iZone(ShooterConstants.kShooterIZ); + + leaderMotor.configure( + leaderConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + + followerMotor.configure( + followerConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + } + + /** + * Set the shooter velocity in RPM. + * + * @param velocity The velocity to set the shooter to. + **/ + public void setShooterVelocity(AngularVelocity velocity) { + lastShooterVelocitySetpoint = velocity; + double targetRpm = velocity.in(Units.RPM); + + if (Math.abs(targetRpm) <= kStopVelocityThresholdRpm) { + stopShooter(); + return; + } + + double feedForward = ShooterConstants.kFF * targetRpm; + leaderClosedLoopController.setSetpoint(targetRpm, ControlType.kVelocity, + ClosedLoopSlot.kSlot0, feedForward); + followerClosedLoopController.setSetpoint(targetRpm, ControlType.kVelocity, + ClosedLoopSlot.kSlot0, feedForward); + } + + public void stopShooter() { + leaderMotor.stopMotor(); + followerMotor.stopMotor(); + } + + /** + * Re-issues the most recently commanded shooter velocity setpoint (if any). + */ + public void setShooterVelocity() { + if (lastShooterVelocitySetpoint == null) { + return; + } + + setShooterVelocity(lastShooterVelocitySetpoint); + } + + public void runMotorBaseSpeed() { + setShooterVelocity(ShooterConstants.kShooterBaseSpeed); + } + + public boolean isShooterSpunUp(AngularVelocity velocity) { + double targetVelocityRpm = velocity.in(Units.RPM); + if (Math.abs(targetVelocityRpm) <= kStopVelocityThresholdRpm) { + return Math.abs(leaderEncoder.getVelocity()) <= kStopVelocityThresholdRpm + && Math.abs(followerEncoder.getVelocity()) <= kStopVelocityThresholdRpm; + } + + double velocityToleranceRpm = ShooterConstants.kShooterVelocityTolerance.in(Units.RPM); + double leaderVelocityError = Math.abs(targetVelocityRpm - leaderEncoder.getVelocity()); + double followerVelocityError = Math.abs(targetVelocityRpm - followerEncoder.getVelocity()); + + return leaderVelocityError <= velocityToleranceRpm + && followerVelocityError <= velocityToleranceRpm; + } + + public boolean isShooterSpunUp() { + if (lastShooterVelocitySetpoint == null) { + return false; + } + + return isShooterSpunUp(lastShooterVelocitySetpoint); + } + + /** + * Get the current shooter velocity in RPM. + * + * @return the current shooter velocity + **/ + public AngularVelocity getCurrentShooterVelocity() { + return Units.RPM.of((leaderEncoder.getVelocity() + followerEncoder.getVelocity()) / 2.0); + } + + @Override + public void periodic() { + double currentLeaderVelocityRpm = leaderEncoder.getVelocity(); + double currentFollowerVelocityRpm = followerEncoder.getVelocity(); + + Logger.recordOutput("Shooter/VelocityRPM", getCurrentShooterVelocity().in(Units.RPM)); + Logger.recordOutput("Shooter/LeaderVelocityRPM", currentLeaderVelocityRpm); + Logger.recordOutput("Shooter/FollowerVelocityRPM", currentFollowerVelocityRpm); + Logger.recordOutput("Shooter/RequestedVelocityRPM", + lastShooterVelocitySetpoint == null ? 0.0 : lastShooterVelocitySetpoint.in(Units.RPM)); + Logger.recordOutput("Shooter/IsSpunUp", isShooterSpunUp()); + Logger.recordOutput("Shooter/LeaderAppliedOutput", leaderMotor.getAppliedOutput()); + Logger.recordOutput("Shooter/FollowerAppliedOutput", followerMotor.getAppliedOutput()); + Logger.recordOutput("Shooter/LeaderPositionRotations", Units.Rotations.of(leaderEncoder.getPosition())); + Logger.recordOutput("Shooter/FollowerPositionRotations", Units.Rotations.of(followerEncoder.getPosition())); + } +} diff --git a/src/main/java/frc/robot/subsystem/SwerveSubsystem.java b/src/main/java/frc/robot/subsystem/SwerveSubsystem.java index f5c4e61..296558f 100644 --- a/src/main/java/frc/robot/subsystem/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystem/SwerveSubsystem.java @@ -18,11 +18,14 @@ import frc.robot.constant.BotConstants; import frc.robot.constant.BotConstants.RobotVariant; import frc.robot.constant.swerve.SwerveConstants; -import frc.robot.hardware.AHRSGyro; +import frc.robot.hardware.PigeonGyro; +import frc.robot.hardware.UnifiedGyro; import frc.robot.hardware.WheelMoverBase; import frc.robot.hardware.WheelMoverSpark; import frc.robot.hardware.WheelMoverTalonFX; -import frc.robot.util.CustomMath; +import frc.robot.util.LocalMath; +import lombok.Getter; +import lombok.Setter; import pwrup.frc.core.hardware.sensor.IGyroscopeLike; /** @@ -38,14 +41,28 @@ public class SwerveSubsystem extends SubsystemBase { private final SwerveDrive swerve; private final IGyroscopeLike m_gyro; - private double gyroOffset = 0; + private Rotation2d swerveRotationOffset; private boolean shouldWork = true; private final SwerveDriveKinematics kinematics; + private boolean isGpsAssist = false; + + public boolean getIsGpsAssist() { + return isGpsAssist; + } + + public void setGpsAssist(boolean isGpsAssist) { + this.isGpsAssist = isGpsAssist; + } + public static SwerveSubsystem GetInstance() { + return GetInstance(UnifiedGyro.GetInstance()); + } + + public static SwerveSubsystem GetInstance(IGyroscopeLike gyro) { if (self == null) { - self = new SwerveSubsystem(AHRSGyro.GetInstance()); + self = new SwerveSubsystem(gyro); } return self; @@ -53,7 +70,9 @@ public static SwerveSubsystem GetInstance() { public SwerveSubsystem(IGyroscopeLike gyro) { this.m_gyro = gyro; + this.swerveRotationOffset = new Rotation2d(); final var c = SwerveConstants.INSTANCE; + this.isGpsAssist = true; if (BotConstants.robotType == RobotVariant.BBOT) { this.m_frontLeftSwerveModule = new WheelMoverSpark( @@ -153,8 +172,9 @@ public void stop() { } public enum DriveType { - GYRO_RELATIVE, + FIELD_RELATIVE, RAW, + DRIVER_RELATIVE, } public void drive(ChassisSpeeds speeds, DriveType driveType) { @@ -164,12 +184,15 @@ public void drive(ChassisSpeeds speeds, DriveType driveType) { } switch (driveType) { - case GYRO_RELATIVE: + case FIELD_RELATIVE: driveFieldRelative(speeds); break; case RAW: driveRaw(speeds); break; + case DRIVER_RELATIVE: + driveDriverRelative(speeds); + break; default: driveRaw(speeds); break; @@ -181,16 +204,35 @@ public void driveRaw(ChassisSpeeds speeds) { swerve.driveNonRelative(actualSpeeds); } + /** + * because the custom library for swerve has an orientation of +y => forward, +x + * => right (i think) this fixes the angles being fucked up + * + * @return + */ + private Rotation2d getSwerveRotation() { + var rotation = m_gyro.getRotation(); + return toSwerveOrientation(rotation.toRotation2d()); + } + + private Rotation2d getSwerveRotationWithOffset() { + return swerveRotationOffset.minus(getSwerveRotation()); + } + public void driveFieldRelative(ChassisSpeeds speeds) { var actualSpeeds = toSwerveOrientation(speeds); - swerve.driveWithGyro(actualSpeeds, new Rotation2d(getSwerveGyroAngle())); + swerve.driveWithGyro(actualSpeeds, getSwerveRotation()); + } + + public void driveDriverRelative(ChassisSpeeds speeds) { + var actualSpeeds = toSwerveOrientation(speeds); + swerve.driveWithGyro(actualSpeeds, getSwerveRotationWithOffset()); } public static ChassisSpeeds fromPercentToVelocity(Vec2 percentXY, double rotationPercent) { - final var c = SwerveConstants.INSTANCE; - double vx = clamp(percentXY.getX(), -1, 1) * c.kMaxSpeed.in(Units.MetersPerSecond); - double vy = clamp(percentXY.getY(), -1, 1) * c.kMaxSpeed.in(Units.MetersPerSecond); - double omega = clamp(rotationPercent, -1, 1) * c.kMaxTurnSpeed.in(Units.RadiansPerSecond); + double vx = clamp(percentXY.getX(), -1, 1) * SwerveConstants.kRobotMaxSpeed.in(Units.MetersPerSecond); + double vy = clamp(percentXY.getY(), -1, 1) * SwerveConstants.kRobotMaxSpeed.in(Units.MetersPerSecond); + double omega = clamp(rotationPercent, -1, 1) * SwerveConstants.kRobotMaxTurnSpeed.in(Units.RadiansPerSecond); return new ChassisSpeeds(vx, vy, omega); } @@ -224,16 +266,12 @@ public SwerveModuleState[] getSwerveModuleStates() { }; } - public void resetGyro() { - resetGyro(0); - } - - public void resetGyro(double offset) { - gyroOffset = -m_gyro.getYaw() + offset; + public void resetDriverRelative() { + swerveRotationOffset = getSwerveRotation(); } - public double getSwerveGyroAngle() { - return Math.toRadians(CustomMath.wrapTo180(m_gyro.getYaw() + gyroOffset)); + public void resetDriverRelative(Rotation2d newCur) { + swerveRotationOffset = toSwerveOrientation(newCur); } public void setShouldWork(boolean value) { @@ -254,8 +292,14 @@ private static ChassisSpeeds toSwerveOrientation(ChassisSpeeds target) { target.omegaRadiansPerSecond); } + private static Rotation2d toSwerveOrientation(Rotation2d target) { + return new Rotation2d(-target.getCos(), target.getSin()); + } + @Override public void periodic() { Logger.recordOutput("SwerveSubsystem/swerve/states", getSwerveModuleStates()); + Logger.recordOutput("SwerveSubsystem/swerve/velocity", getKinematics().toChassisSpeeds(getSwerveModuleStates())); + Logger.recordOutput("SwerveSubsystem/AdjustingVelocity", isGpsAssist); } } diff --git a/src/main/java/frc/robot/subsystem/TurretSubsystem.java b/src/main/java/frc/robot/subsystem/TurretSubsystem.java new file mode 100644 index 0000000..39892ef --- /dev/null +++ b/src/main/java/frc/robot/subsystem/TurretSubsystem.java @@ -0,0 +1,154 @@ +package frc.robot.subsystem; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + +import frc.robot.constant.TurretConstants; +import lombok.Setter; + +public class TurretSubsystem extends SubsystemBase { + private static TurretSubsystem instance; + + private SparkFlex m_turretMotor; + private SparkClosedLoopController closedLoopController; + private final AbsoluteEncoder absoluteEncoder; + + /** Last commanded turret goal angle (for logging / time estimate). */ + private Angle lastAimTarget; + + @Setter + private static boolean isGpsAssistEnabled = true; + + public static boolean getIsGpsAssistEnabled() { + return isGpsAssistEnabled; + } + + public static TurretSubsystem GetInstance() { + if (instance == null) { + instance = new TurretSubsystem(TurretConstants.kTurretCanId, TurretConstants.kTurretMotorType); + } + + return instance; + } + + public TurretSubsystem(int canId, MotorType motorType) { + configureSparkMax(canId, motorType); + absoluteEncoder = m_turretMotor.getAbsoluteEncoder(); + reset(); + } + + private void configureSparkMax(int canId, MotorType motorType) { + this.m_turretMotor = new SparkFlex(canId, motorType); + this.closedLoopController = m_turretMotor.getClosedLoopController(); + + SparkFlexConfig config = new SparkFlexConfig(); + + config.idleMode(IdleMode.kCoast); + config.inverted(TurretConstants.kMotorInverted); + config.openLoopRampRate(0.0); + config.closedLoopRampRate(0.0); + + config + .smartCurrentLimit(TurretConstants.kTurretCurrentLimit); + + config.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(TurretConstants.kTurretP, TurretConstants.kTurretI, TurretConstants.kTurretD) + .iZone(TurretConstants.kTurretIZ) + .outputRange(-1.0, 1.0) + .positionWrappingEnabled(true) + .positionWrappingMinInput(0) + .positionWrappingMaxInput(1); + + config.absoluteEncoder.zeroOffset(TurretConstants.kTurretOffset.in(Units.Rotations)).inverted(true); + + m_turretMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void reset() { + // Absolute encoder provides the turret angle reference; nothing to zero here. + lastAimTarget = getTurretPosition(); + } + + /** + * Simple position PID (no MAXMotion). + */ + public void setTurretPosition(Angle position, Voltage feedForward) { + lastAimTarget = position; + closedLoopController.setSetpoint( + lastAimTarget.in(Units.Rotations), + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedForward.in(Units.Volts), + ArbFFUnits.kVoltage); + } + + public int getAimTimeLeftMs() { + if (lastAimTarget == null) { + return 0; + } + + double maxVelRotPerSec = TurretConstants.kTurretMaxVelocity.in(Units.RotationsPerSecond); + if (maxVelRotPerSec <= 0.0) { + return 0; + } + + double currentRot = getTurretPosition().in(Units.Rotations); + double targetRot = lastAimTarget.in(Units.Rotations); + + // shortest-path wrapped error (-0.5 .. 0.5 rotations) + double errorRot = targetRot - currentRot; + errorRot = errorRot - Math.floor(errorRot + 0.5); + + double distanceRot = Math.abs(errorRot); + + double timeSec = distanceRot / maxVelRotPerSec; + + return (int) Math.ceil(timeSec * 1000.0); + } + + public Angle getTurretPosition() { + return Units.Rotations.of(absoluteEncoder.getPosition()); + } + + private double wrapToUnitRotations(double rotations) { + double wrapped = rotations % 1.0; + if (wrapped < 0.0) { + wrapped += 1.0; + } + return wrapped; + } + + @Override + public void periodic() { + Logger.recordOutput("Turret/PositionRot", getTurretPosition().in(Units.Rotations)); + Logger.recordOutput("Turret/PositionDeg", getTurretPosition().in(Units.Degrees)); + Logger.recordOutput("Turret/AbsolutePositionRawRot", absoluteEncoder.getPosition()); + Logger.recordOutput("Turret/Velocity", m_turretMotor.getEncoder().getVelocity()); + Logger.recordOutput("Turret/DesiredOutputRot", lastAimTarget != null ? lastAimTarget.in(Units.Rotations) : 0); + Logger.recordOutput("Turret/DesiredWrappedOutputRot", + lastAimTarget != null ? wrapToUnitRotations(lastAimTarget.in(Units.Rotations)) : 0); + Logger.recordOutput("Turret/AppliedOutput", m_turretMotor.getAppliedOutput()); + Logger.recordOutput("Turret/AppliedOutputWant", m_turretMotor.getOutputCurrent()); + Logger.recordOutput("Turret/BusVoltage", m_turretMotor.getBusVoltage()); + Logger.recordOutput("Turret/TimeTillGoal", getAimTimeLeftMs()); + } +} diff --git a/src/main/java/frc/robot/util/AimPoint.java b/src/main/java/frc/robot/util/AimPoint.java new file mode 100644 index 0000000..1a44441 --- /dev/null +++ b/src/main/java/frc/robot/util/AimPoint.java @@ -0,0 +1,188 @@ +package frc.robot.util; + +import java.util.List; +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constant.BotConstants; +import frc.robot.subsystem.GlobalPosition; + +public final class AimPoint { + + public enum ZoneName { + LEFT_CENTER, + MIDDLE_CENTER, + RIGHT_CENTER, + FRONT_OF_HUB, + } + + private static final double FIELD_LENGTH_METERS = BotConstants.kFieldLayout.getFieldLength(); + private static final double FIELD_WIDTH_METERS = BotConstants.kFieldLayout.getFieldWidth(); + + private static final List ZONES = List.of( + new Zone(ZoneName.FRONT_OF_HUB, + atFieldPercent(0.0, 0.0), + atFieldPercent(0.25, 1.00), + atFieldPercent(0.30, 0.50)), + new Zone( + ZoneName.LEFT_CENTER, + atFieldPercent(0.25, 0.00), + atFieldPercent(0.50, 0.45), + atFieldPercent(0.05, 0.25)), + new Zone( + ZoneName.MIDDLE_CENTER, + atFieldPercent(0.25, 0.45), + atFieldPercent(0.50, 0.55), + atFieldPercent(0.50, 0.75)), + new Zone( + ZoneName.RIGHT_CENTER, + atFieldPercent(0.25, 0.55), + atFieldPercent(0.50, 1.00), + atFieldPercent(0.05, 0.75))); + + private AimPoint() { + } + + public static ZoneName getZone(Pose2d pose) { + return getZone(pose, DriverStation.getAlliance()); + } + + public static ZoneName getZone(Pose2d pose, Optional alliance) { + Pose2d bluePose = toBluePerspective(pose, alliance); + + for (Zone zone : ZONES) { + if (zone.contains(bluePose)) { + return zone.name(); + } + } + + Translation2d robotPosition = bluePose.getTranslation(); + Zone nearest = ZONES.get(0); + double nearestDistance = Double.POSITIVE_INFINITY; + for (Zone zone : ZONES) { + double distance = robotPosition.getDistance(zone.center()); + if (distance < nearestDistance) { + nearestDistance = distance; + nearest = zone; + } + } + return nearest.name(); + } + + public static Translation2d getTarget(Pose2d pose) { + return getTarget(pose, DriverStation.getAlliance()); + } + + public static Translation2d getTarget() { + return getTarget(GlobalPosition.Get()); + } + + public static Translation2d getTarget(Pose2d pose, Optional alliance) { + return getTarget(getZone(pose, alliance), alliance); + } + + public static Translation2d getTarget(ZoneName zoneName) { + return getTarget(zoneName, DriverStation.getAlliance()); + } + + public static Translation2d getTarget(ZoneName zoneName, Optional alliance) { + return fromBluePerspective(getBlueTarget(zoneName), alliance); + } + + private static Translation2d getBlueTarget(ZoneName zoneName) { + for (Zone zone : ZONES) { + if (zone.name() == zoneName) { + return zone.target(); + } + } + return ZONES.get(0).target(); + } + + public static void logZoneForAdvantageScope(ZoneName zoneName) { + logZoneForAdvantageScope(zoneName, "AimPoint/Zones/" + zoneName); + } + + public static void logZoneForAdvantageScope(ZoneName zoneName, String keyPrefix) { + Optional alliance = DriverStation.getAlliance(); + Zone zone = getBlueZone(zoneName); + Translation2d min = zone.minCorner(); + Translation2d max = zone.maxCorner(); + + Translation2d c0 = fromBluePerspective(new Translation2d(min.getX(), min.getY()), alliance); + Translation2d c1 = fromBluePerspective(new Translation2d(max.getX(), min.getY()), alliance); + Translation2d c2 = fromBluePerspective(new Translation2d(max.getX(), max.getY()), alliance); + Translation2d c3 = fromBluePerspective(new Translation2d(min.getX(), max.getY()), alliance); + Translation2d target = fromBluePerspective(zone.target(), alliance); + + Pose2d[] outline = new Pose2d[] { + new Pose2d(c0, new Rotation2d()), + new Pose2d(c1, new Rotation2d()), + new Pose2d(c2, new Rotation2d()), + new Pose2d(c3, new Rotation2d()), + new Pose2d(c0, new Rotation2d()) + }; + + Logger.recordOutput(keyPrefix + "/Outline", outline); + Logger.recordOutput(keyPrefix + "/Target", new Pose2d(target, new Rotation2d())); + Logger.recordOutput(keyPrefix + "/Name", zoneName.toString()); + } + + private static Translation2d atFieldPercent(double xPercent, double yPercent) { + return new Translation2d(FIELD_LENGTH_METERS * xPercent, FIELD_WIDTH_METERS * yPercent); + } + + private static Pose2d toBluePerspective(Pose2d fieldPose, Optional alliance) { + if (!isRed(alliance)) { + return fieldPose; + } + return new Pose2d( + flipX(fieldPose.getX()), + fieldPose.getY(), + fieldPose.getRotation()); + } + + private static Translation2d fromBluePerspective(Translation2d bluePoint, Optional alliance) { + if (!isRed(alliance)) { + return bluePoint; + } + return new Translation2d(flipX(bluePoint.getX()), bluePoint.getY()); + } + + private static boolean isRed(Optional alliance) { + return alliance.isPresent() && alliance.get() == Alliance.Red; + } + + private static double flipX(double xMeters) { + return FIELD_LENGTH_METERS - xMeters; + } + + private static Zone getBlueZone(ZoneName zoneName) { + for (Zone zone : ZONES) { + if (zone.name() == zoneName) { + return zone; + } + } + return ZONES.get(0); + } + + private record Zone(ZoneName name, Translation2d minCorner, Translation2d maxCorner, Translation2d target) { + + boolean contains(Pose2d pose) { + Translation2d translation = pose.getTranslation(); + return translation.getX() >= minCorner.getX() + && translation.getX() < maxCorner.getX() + && translation.getY() >= minCorner.getY() + && translation.getY() < maxCorner.getY(); + } + + Translation2d center() { + return minCorner.interpolate(maxCorner, 0.5); + } + } +} diff --git a/src/main/java/frc/robot/util/AlignmentPoints.java b/src/main/java/frc/robot/util/AlignmentPoints.java deleted file mode 100644 index 0eaf238..0000000 --- a/src/main/java/frc/robot/util/AlignmentPoints.java +++ /dev/null @@ -1,182 +0,0 @@ -package frc.robot.util; - -import java.util.Comparator; -import java.util.HashMap; -import java.util.Map; -import java.util.Optional; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import lombok.Getter; -import lombok.Setter; - -public class AlignmentPoints { - @Setter - public static double kFieldWidth = 6.0; - @Setter - public static double kFieldLength = 6.0; - - private static AlignmentMap POINTS; - - /** - * Functional interface for providing the current robot position. - * Implement this to integrate with your odometry/position tracking system. - */ - @FunctionalInterface - public interface PositionSupplier { - Pose2d get(); - } - - private static PositionSupplier positionSupplier = () -> new Pose2d(); - - /** - * Set the position supplier for closest point calculations. - * @param supplier A supplier that returns the current robot Pose2d - */ - public static void setPositionSupplier(PositionSupplier supplier) { - positionSupplier = supplier; - } - - public static void setPoints(AlignmentMap points) { - POINTS = points; - } - - @Getter - public static class AlignmentMap { - private final Map>> points = new HashMap<>(); - - public AlignmentMap category(String name, CategoryBuilder builder) { - points.put(name, builder.build()); - return this; - } - - public Optional get(String category, String subCategory, String pointName) { - return Optional.ofNullable(points.get(category)) - .map(cat -> cat.get(subCategory)) - .map(sub -> sub.get(pointName)); - } - } - - public static class CategoryBuilder { - private final Map> subCategories = new HashMap<>(); - - public CategoryBuilder subCategory(String name, SubCategoryBuilder builder) { - subCategories.put(name, builder.build()); - return this; - } - - Map> build() { - return subCategories; - } - } - - public static class SubCategoryBuilder { - private final Map points = new HashMap<>(); - @Getter - private Pose2d center; - - public SubCategoryBuilder point(String name, double x, double y, double rotationRadians) { - points.put(name, new Pose2d(x, y, new Rotation2d(rotationRadians))); - return this; - } - - public SubCategoryBuilder point(String name, Pose2d pose) { - points.put(name, pose); - return this; - } - - Map build() { - double avgX = 0, avgY = 0, avgTheta = 0; - int count = points.size(); - if (count > 0) { - avgX = points.values().stream().mapToDouble(Pose2d::getX).sum() / count; - avgY = points.values().stream().mapToDouble(Pose2d::getY).sum() / count; - double sumSin = points.values().stream().mapToDouble(p -> Math.sin(p.getRotation().getRadians())).sum(); - double sumCos = points.values().stream().mapToDouble(p -> Math.cos(p.getRotation().getRadians())).sum(); - avgTheta = Math.atan2(sumSin / count, sumCos / count); - } - center = new Pose2d(avgX, avgY, new Rotation2d(avgTheta)); - - return points; - } - } - - private static Alliance alliance = Alliance.Blue; - - public static void setAlliance(Alliance fieldSide) { - alliance = fieldSide; - } - - public static Optional getPoint(String category, String subCategory, String pointName) { - return POINTS.get(category, subCategory, pointName) - .map(pose -> alliance == Alliance.Red ? mirrorPose(pose) : pose); - } - - public static Optional getPoint(String category) { - var subString = category.split(":"); - if (subString.length != 3) { - return Optional.empty(); - } - - String categoryName = subString[1]; - if (categoryName.toLowerCase().equals("closest")) { - categoryName = getClosestCategory(subString[0], positionSupplier.get()).orElse(null); - if (categoryName == null) { - return Optional.empty(); - } - } - - String pointName = subString[2]; - if (pointName.toLowerCase().equals("closest")) { - pointName = getClosestPoint(subString[0], categoryName, positionSupplier.get()).orElse(null); - if (pointName == null) { - return Optional.empty(); - } - } - - return getPoint(subString[0], categoryName, pointName); - } - - private static Optional getClosestPoint(String category, String subCategory, Pose2d position) { - var points = POINTS.getPoints().get(category).get(subCategory); - if (points == null) { - return Optional.empty(); - } - return points.entrySet().stream() - .min(Comparator.comparingDouble( - entry -> entry.getValue().getTranslation().getDistance(position.getTranslation()))) - .map(Map.Entry::getKey); - } - - private static Optional getClosestCategory(String category, Pose2d position) { - var cat = POINTS.getPoints().get(category); - if (cat == null) { - return Optional.empty(); - } - return cat.entrySet().stream() - .min(Comparator.comparingDouble( - entry -> calculateCenter(entry.getValue()).getTranslation().getDistance(position.getTranslation()))) - .map(Map.Entry::getKey); - } - - private static Pose2d calculateCenter(Map points) { - double avgX = 0, avgY = 0, avgTheta = 0; - int count = points.size(); - if (count > 0) { - avgX = points.values().stream().mapToDouble(Pose2d::getX).sum() / count; - avgY = points.values().stream().mapToDouble(Pose2d::getY).sum() / count; - double sumSin = points.values().stream().mapToDouble(p -> Math.sin(p.getRotation().getRadians())).sum(); - double sumCos = points.values().stream().mapToDouble(p -> Math.cos(p.getRotation().getRadians())).sum(); - avgTheta = Math.atan2(sumSin / count, sumCos / count); - } - return new Pose2d(avgX, avgY, new Rotation2d(avgTheta)); - } - - private static Pose2d mirrorPose(Pose2d pose) { - return new Pose2d( - kFieldLength - pose.getX(), - kFieldWidth - pose.getY(), - pose.getRotation().rotateBy(new Rotation2d(Math.PI))); - } -} diff --git a/src/main/java/frc/robot/util/Dashboard.java b/src/main/java/frc/robot/util/Dashboard.java new file mode 100644 index 0000000..9299893 --- /dev/null +++ b/src/main/java/frc/robot/util/Dashboard.java @@ -0,0 +1,5 @@ +package frc.robot.util; + +public class Dashboard { + +} diff --git a/src/main/java/frc/robot/util/CustomMath.java b/src/main/java/frc/robot/util/LocalMath.java similarity index 97% rename from src/main/java/frc/robot/util/CustomMath.java rename to src/main/java/frc/robot/util/LocalMath.java index d73a700..a5a2ca4 100644 --- a/src/main/java/frc/robot/util/CustomMath.java +++ b/src/main/java/frc/robot/util/LocalMath.java @@ -23,7 +23,7 @@ * @note MathFun = Math Functions * @apiNote this is the file where all of the math functions go */ -public class CustomMath { +public class LocalMath { public static List fromPathfindResultToTranslation2dList(PathfindResult pathfindResult) { return pathfindResult.getPathList().stream() @@ -31,6 +31,14 @@ public static List fromPathfindResultToTranslation2dList(Pathfind .collect(Collectors.toList()); } + public static int randomInt(int min, int max) { + return (int) (Math.random() * (max - min + 1)) + min; + } + + public static Translation2d fromGlobalToRelative(Translation2d global, Translation2d relative) { + return global.minus(relative); + } + public static Trajectory generatePathfindingTrajectory(List path, double maxSpeed, double maxAcceleration) { diff --git a/src/main/java/frc/robot/util/PathPlannerSetup.java b/src/main/java/frc/robot/util/PathPlannerSetup.java deleted file mode 100644 index abeaec1..0000000 --- a/src/main/java/frc/robot/util/PathPlannerSetup.java +++ /dev/null @@ -1,85 +0,0 @@ -package frc.robot.util; - -import java.util.Optional; -import java.util.function.Supplier; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.subsystem.GlobalPosition; -import frc.robot.subsystem.OdometrySubsystem; -import frc.robot.subsystem.SwerveSubsystem; - -public final class PathPlannerSetup { - private static boolean configured = false; - - private PathPlannerSetup() { - } - - public static void configure() { - if (configured) { - return; - } - - RobotConfig config; - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - System.out.println("ERROR: PathPlanner RobotConfig load failed (GUI settings)."); - e.printStackTrace(); - return; - } - - AutoBuilder.configure( - new Supplier() { - - @Override - public Pose2d get() { - return GlobalPosition.Get(); - } - - }, - OdometrySubsystem.GetInstance()::setOdometryPosition, - PathPlannerSetup::getRobotRelativeSpeeds, - (speeds, feedforwards) -> SwerveSubsystem.GetInstance().drive(speeds, SwerveSubsystem.DriveType.RAW), - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.15), // translation PID (initial: match ExecuteTrajectory) - new PIDConstants(1.0, 0.0, 0.7) // rotation PID (initial: match ExecuteTrajectory theta P) - ), - config, - PathPlannerSetup::shouldFlipForAlliance, - SwerveSubsystem.GetInstance()); - - configured = true; - } - - private static boolean shouldFlipForAlliance() { - // Optional alliance = DriverStation.getAlliance(); - // in the test field, we always start as red - return false; // alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; - } - - /** - * PathPlanner expects robot-relative speeds. Your - * `GlobalPosition.GetVelocity()` - * is field-relative, so convert using the current pose heading. - */ - private static ChassisSpeeds getRobotRelativeSpeeds() { - ChassisSpeeds field = GlobalPosition.GetVelocity(); - if (field == null) { - return new ChassisSpeeds(); - } - - var heading = GlobalPosition.Get().getRotation(); - return ChassisSpeeds.fromFieldRelativeSpeeds( - field.vxMetersPerSecond, - field.vyMetersPerSecond, - field.omegaRadiansPerSecond, - heading); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/PathedAuto.java b/src/main/java/frc/robot/util/PathedAuto.java new file mode 100644 index 0000000..d2169fd --- /dev/null +++ b/src/main/java/frc/robot/util/PathedAuto.java @@ -0,0 +1,104 @@ +package frc.robot.util; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.util.ArrayList; +import java.util.List; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; +import org.json.simple.parser.ParseException; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.wpilibj.Filesystem; + +public class PathedAuto extends PathPlannerAuto { + private final List paths = new ArrayList<>(); + + public PathedAuto(String name) { + super(name); + + File autoFile = getAutoFile(name); + + JSONObject autoJson; + try { + autoJson = getAutoJson(autoFile); + } catch (IOException | ParseException e) { + e.printStackTrace(); + return; + } + + JSONObject commandObj = asObject(autoJson.get("command")); + if (commandObj == null) + return; + + collectPathsFromCommand(commandObj); + } + + public List getPaths() { + return paths; + } + + private File getAutoFile(String autoName) { + return new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); + } + + private JSONObject getAutoJson(File autoFile) throws IOException, ParseException { + String rawJson = Files.readString(autoFile.toPath()); + return (JSONObject) new JSONParser().parse(rawJson); + } + + private void collectPathsFromCommand(JSONObject commandObj) { + String type = asString(commandObj.get("type")); + if (type == null) + return; + + if (type.equals("path")) { + JSONObject data = asObject(commandObj.get("data")); + if (data == null) + return; + + String pathName = asString(data.get("pathName")); + if (pathName == null) + return; + + try { + paths.add(PathPlannerPath.fromPathFile(pathName)); + } catch (Exception e) { + e.printStackTrace(); + } + return; + } + + JSONObject data = asObject(commandObj.get("data")); + if (data == null) + return; + + JSONArray commands = asArray(data.get("commands")); + if (commands == null) + return; + + for (Object child : commands) { + JSONObject childObj = asObject(child); + if (childObj != null) { + collectPathsFromCommand(childObj); + } + } + } + + private static JSONObject asObject(Object o) { + return (o instanceof JSONObject) ? (JSONObject) o : null; + } + + private static JSONArray asArray(Object o) { + return (o instanceof JSONArray) ? (JSONArray) o : null; + } + + private static String asString(Object o) { + return (o instanceof String) ? (String) o : null; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/SharedStringTopic.java b/src/main/java/frc/robot/util/SharedStringTopic.java new file mode 100644 index 0000000..8f97341 --- /dev/null +++ b/src/main/java/frc/robot/util/SharedStringTopic.java @@ -0,0 +1,53 @@ +package frc.robot.util; + +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringSubscriber; +import frc.robot.Robot; +import lombok.Getter; + +/** + * Wraps a pair of NetworkTables string topics for request/state communication. + * Subscribes to + * {@code topicBase/Request} and publishes to {@code topicBase/State} on the + * dashboard NetworkTables + * instance. + */ +public class SharedStringTopic { + @Getter + private final StringSubscriber subscriber; + private final StringPublisher publisher; + + private static final String kRequestSuffix = "/Request"; + private static final String kStateSuffix = "/State"; + + /** + * Creates a shared string topic using the given base name. + * + * @param topicBase base name for the topic pair; "/Request" and "/State" are + * appended for the + * subscriber and publisher respectively + */ + public SharedStringTopic(String topicBase) { + this.subscriber = Robot.getDashboard().getStringTopic(topicBase + kRequestSuffix).subscribe(""); + this.publisher = Robot.getDashboard().getStringTopic(topicBase + kStateSuffix).publish(); + } + + /** + * Returns the latest value from the Request topic. + * + * @return the current request string, or empty string if none has been + * published + */ + public String getState() { + return subscriber.get(); + } + + /** + * Publishes the given value to the State topic. + * + * @param state the string to publish + */ + public void setState(String state) { + publisher.set(state); + } +} diff --git a/src/main/java/frc/robot/util/lighting/BlendMode.java b/src/main/java/frc/robot/util/lighting/BlendMode.java new file mode 100644 index 0000000..a8e4419 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/BlendMode.java @@ -0,0 +1,6 @@ +package frc.robot.util.lighting; + +public enum BlendMode { + OVERWRITE, + ADD +} diff --git a/src/main/java/frc/robot/util/lighting/EffectHandle.java b/src/main/java/frc/robot/util/lighting/EffectHandle.java new file mode 100644 index 0000000..f9e0f49 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/EffectHandle.java @@ -0,0 +1,34 @@ +package frc.robot.util.lighting; + +public final class EffectHandle { + private final int id; + + EffectHandle(int id) { + this.id = id; + } + + int id() { + return id; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) { + return true; + } + if (!(obj instanceof EffectHandle other)) { + return false; + } + return id == other.id; + } + + @Override + public int hashCode() { + return Integer.hashCode(id); + } + + @Override + public String toString() { + return "EffectHandle(" + id + ")"; + } +} diff --git a/src/main/java/frc/robot/util/lighting/LedColor.java b/src/main/java/frc/robot/util/lighting/LedColor.java new file mode 100644 index 0000000..a82ecf1 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/LedColor.java @@ -0,0 +1,41 @@ +package frc.robot.util.lighting; + +public record LedColor(int red, int green, int blue, int white) { + public static final LedColor BLACK = new LedColor(0, 0, 0, 0); + + public LedColor { + red = clamp(red); + green = clamp(green); + blue = clamp(blue); + white = clamp(white); + } + + public LedColor(int red, int green, int blue) { + this(red, green, blue, 0); + } + + public LedColor add(LedColor other) { + return new LedColor( + clamp(red + other.red), + clamp(green + other.green), + clamp(blue + other.blue), + clamp(white + other.white)); + } + + public LedColor scale(double scalar) { + double clamped = clamp01(scalar); + return new LedColor( + (int) Math.round(red * clamped), + (int) Math.round(green * clamped), + (int) Math.round(blue * clamped), + (int) Math.round(white * clamped)); + } + + public static int clamp(int value) { + return Math.max(0, Math.min(255, value)); + } + + public static double clamp01(double value) { + return Math.max(0.0, Math.min(1.0, value)); + } +} diff --git a/src/main/java/frc/robot/util/lighting/LedRange.java b/src/main/java/frc/robot/util/lighting/LedRange.java new file mode 100644 index 0000000..4bbb89e --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/LedRange.java @@ -0,0 +1,29 @@ +package frc.robot.util.lighting; + +import frc.robot.constant.LEDConstants; + +public record LedRange(int startInclusive, int endInclusive) { + public LedRange { + if (startInclusive > endInclusive) { + throw new IllegalArgumentException( + "LED range start must be <= end. Got start=" + startInclusive + ", end=" + endInclusive); + } + if (startInclusive < LEDConstants.ledStartIndex || endInclusive > LEDConstants.ledEndIndex) { + throw new IllegalArgumentException( + "LED range must be within [" + LEDConstants.ledStartIndex + ", " + LEDConstants.ledEndIndex + + "]. Got [" + startInclusive + ", " + endInclusive + "]"); + } + } + + public static LedRange single(int index) { + return new LedRange(index, index); + } + + public int length() { + return endInclusive - startInclusive + 1; + } + + public boolean contains(int index) { + return index >= startInclusive && index <= endInclusive; + } +} diff --git a/src/main/java/frc/robot/util/lighting/LightEffect.java b/src/main/java/frc/robot/util/lighting/LightEffect.java new file mode 100644 index 0000000..563fdb5 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/LightEffect.java @@ -0,0 +1,53 @@ +package frc.robot.util.lighting; + +public abstract class LightEffect { + private final LedRange range; + private BlendMode blendMode; + private int priority; + private boolean enabled; + + protected LightEffect(LedRange range, int priority, BlendMode blendMode) { + this.range = range; + this.priority = priority; + this.blendMode = blendMode; + this.enabled = true; + } + + public LedRange getRange() { + return range; + } + + public BlendMode getBlendMode() { + return blendMode; + } + + public void setBlendMode(BlendMode blendMode) { + this.blendMode = blendMode; + } + + public int getPriority() { + return priority; + } + + public void setPriority(int priority) { + this.priority = priority; + } + + public boolean isEnabled() { + return enabled; + } + + public void setEnabled(boolean enabled) { + this.enabled = enabled; + } + + public boolean setProgress(double progress01) { + return false; + } + + public boolean updateInput(T inputArg) { + return false; + } + + public abstract LedColor sample(int ledIndex, double nowSeconds); +} diff --git a/src/main/java/frc/robot/util/lighting/LightZone.java b/src/main/java/frc/robot/util/lighting/LightZone.java new file mode 100644 index 0000000..61e80a0 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/LightZone.java @@ -0,0 +1,23 @@ +package frc.robot.util.lighting; + +import frc.robot.constant.LEDConstants; + +public enum LightZone { + ONBOARD(LEDConstants.onboardStartIndex, LEDConstants.onboardEndIndex), + FULL_STRIP(LEDConstants.ledStartIndex, LEDConstants.ledEndIndex), + EXTERNAL_STRIP(LEDConstants.externalStripStartIndex, LEDConstants.externalStripEndIndex), + LEFT_HALF(LEDConstants.leftHalfStartIndex, LEDConstants.leftHalfEndIndex), + RIGHT_HALF(LEDConstants.rightHalfStartIndex, LEDConstants.rightHalfEndIndex); + + private final int startInclusive; + private final int endInclusive; + + LightZone(int startInclusive, int endInclusive) { + this.startInclusive = startInclusive; + this.endInclusive = endInclusive; + } + + public LedRange range() { + return new LedRange(startInclusive, endInclusive); + } +} diff --git a/src/main/java/frc/robot/util/lighting/LightsApi.java b/src/main/java/frc/robot/util/lighting/LightsApi.java new file mode 100644 index 0000000..a7d4cc5 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/LightsApi.java @@ -0,0 +1,71 @@ +package frc.robot.util.lighting; + +public interface LightsApi { + EffectHandle addProgressBar( + LedRange range, + LedColor fill, + LedColor emptyOrNull, + int priority, + BlendMode blend); + + EffectHandle addSolid(LedRange range, LedColor color, int priority, BlendMode blend); + + EffectHandle addBlink(LedRange range, LedColor on, LedColor off, double hz, int priority, BlendMode blend); + + EffectHandle addBreathe( + LedRange range, + LedColor color, + double hz, + double minScalar, + double maxScalar, + int priority, + BlendMode blend); + + EffectHandle addChase( + LedRange range, + LedColor color, + int width, + double hz, + boolean wrap, + int priority, + BlendMode blend); + + EffectHandle addRainbow(LedRange range, double hz, double brightness, int priority, BlendMode blend); + + EffectHandle addLarsonScanner( + LedRange range, + LedColor color, + int eyeWidth, + int trailLength, + double hz, + int priority, + BlendMode blend); + + EffectHandle addMorseCode( + LedRange range, + String message, + LedColor onColor, + LedColor offColor, + double unitSeconds, + int priority, + BlendMode blend); + + EffectHandle addConvergingArrows( + LedRange range, + LedColor color, + boolean wedge, + int priority, + BlendMode blend); + + boolean setEnabled(EffectHandle handle, boolean enabled); + + boolean setPriority(EffectHandle handle, int priority); + + boolean removeEffect(EffectHandle handle); + + void clearEffects(); + + boolean setProgress(EffectHandle handle, double progress01); + + boolean setInput(EffectHandle handle, T inputArg); +} diff --git a/src/main/java/frc/robot/util/lighting/LightsEngine.java b/src/main/java/frc/robot/util/lighting/LightsEngine.java new file mode 100644 index 0000000..0b90fbe --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/LightsEngine.java @@ -0,0 +1,319 @@ +package frc.robot.util.lighting; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +public class LightsEngine { + public record LedSegment(int startInclusive, int endInclusive, LedColor color) { + } + + public record RenderResult( + List segments, + boolean adaptiveCompressionActive, + int changedLedCount, + int activeEffectCount) { + public boolean hasWrites() { + return !segments.isEmpty(); + } + } + + private final int ledCount; + private final int maxSolidWritesPerCycle; + + private final int[] currentRed; + private final int[] currentGreen; + private final int[] currentBlue; + private final int[] currentWhite; + + private final int[] previousRed; + private final int[] previousGreen; + private final int[] previousBlue; + private final int[] previousWhite; + + private final int[] layerRed; + private final int[] layerGreen; + private final int[] layerBlue; + private final int[] layerWhite; + private final boolean[] layerTouched; + + private final Map> effectsById = new HashMap<>(); + private int nextEffectId = 1; + + public LightsEngine(int ledCount, int maxSolidWritesPerCycle) { + if (ledCount <= 0) { + throw new IllegalArgumentException("ledCount must be > 0"); + } + + this.ledCount = ledCount; + this.maxSolidWritesPerCycle = Math.max(1, maxSolidWritesPerCycle); + + currentRed = new int[ledCount]; + currentGreen = new int[ledCount]; + currentBlue = new int[ledCount]; + currentWhite = new int[ledCount]; + + previousRed = new int[ledCount]; + previousGreen = new int[ledCount]; + previousBlue = new int[ledCount]; + previousWhite = new int[ledCount]; + + layerRed = new int[ledCount]; + layerGreen = new int[ledCount]; + layerBlue = new int[ledCount]; + layerWhite = new int[ledCount]; + layerTouched = new boolean[ledCount]; + } + + public EffectHandle addEffect(LightEffect effect) { + int id = nextEffectId++; + effectsById.put(id, effect); + return new EffectHandle<>(id); + } + + public boolean removeEffect(EffectHandle handle) { + return effectsById.remove(handle.id()) != null; + } + + public void clearEffects() { + effectsById.clear(); + } + + public boolean setEnabled(EffectHandle handle, boolean enabled) { + LightEffect effect = effectsById.get(handle.id()); + if (effect == null) { + return false; + } + effect.setEnabled(enabled); + return true; + } + + public boolean setPriority(EffectHandle handle, int priority) { + LightEffect effect = effectsById.get(handle.id()); + if (effect == null) { + return false; + } + effect.setPriority(priority); + return true; + } + + public boolean setProgress(EffectHandle handle, double progress01) { + LightEffect effect = effectsById.get(handle.id()); + if (effect == null) { + return false; + } + return effect.setProgress(progress01); + } + + public boolean setInput(EffectHandle handle, T inputArg) { + LightEffect effect = effectsById.get(handle.id()); + if (effect == null) { + return false; + } + return castAndUpdate(effect, inputArg); + } + + @SuppressWarnings("unchecked") + private static boolean castAndUpdate(LightEffect effect, T inputArg) { + try { + return ((LightEffect) effect).updateInput(inputArg); + } catch (ClassCastException ex) { + return false; + } + } + + public RenderResult render(double nowSeconds) { + clearCurrentFrame(); + + List>> activeEffects = new ArrayList<>(); + for (Map.Entry> entry : effectsById.entrySet()) { + if (entry.getValue().isEnabled()) { + activeEffects.add(entry); + } + } + + activeEffects.sort( + Comparator + .comparingInt((Map.Entry> entry) -> entry.getValue().getPriority()) + .thenComparingInt(Map.Entry::getKey)); + + for (Map.Entry> entry : activeEffects) { + applyEffect(entry.getValue(), nowSeconds); + } + + DiffResult diff = computeDiffSegments(); + boolean adaptiveCompressionActive = false; + List segmentsToWrite = diff.segments; + + if (segmentsToWrite.size() > maxSolidWritesPerCycle) { + adaptiveCompressionActive = true; + segmentsToWrite = buildCompressedSegments(maxSolidWritesPerCycle); + } + + copyCurrentToPrevious(); + + return new RenderResult( + List.copyOf(segmentsToWrite), + adaptiveCompressionActive, + diff.changedLedCount, + activeEffects.size()); + } + + public int getActiveEffectCount() { + int count = 0; + for (LightEffect effect : effectsById.values()) { + if (effect.isEnabled()) { + count++; + } + } + return count; + } + + private void applyEffect(LightEffect effect, double nowSeconds) { + LedRange range = effect.getRange(); + int start = range.startInclusive(); + int end = range.endInclusive(); + + for (int i = start; i <= end; i++) { + layerTouched[i] = false; + } + + for (int i = start; i <= end; i++) { + LedColor sample = effect.sample(i, nowSeconds); + if (sample == null) { + continue; + } + + layerTouched[i] = true; + layerRed[i] = sample.red(); + layerGreen[i] = sample.green(); + layerBlue[i] = sample.blue(); + layerWhite[i] = sample.white(); + } + + if (effect.getBlendMode() == BlendMode.OVERWRITE) { + for (int i = start; i <= end; i++) { + if (!layerTouched[i]) { + continue; + } + currentRed[i] = layerRed[i]; + currentGreen[i] = layerGreen[i]; + currentBlue[i] = layerBlue[i]; + currentWhite[i] = layerWhite[i]; + } + return; + } + + for (int i = start; i <= end; i++) { + if (!layerTouched[i]) { + continue; + } + currentRed[i] = LedColor.clamp(currentRed[i] + layerRed[i]); + currentGreen[i] = LedColor.clamp(currentGreen[i] + layerGreen[i]); + currentBlue[i] = LedColor.clamp(currentBlue[i] + layerBlue[i]); + currentWhite[i] = LedColor.clamp(currentWhite[i] + layerWhite[i]); + } + } + + private DiffResult computeDiffSegments() { + List segments = new ArrayList<>(); + int changedLedCount = 0; + + int i = 0; + while (i < ledCount) { + if (!isChanged(i)) { + i++; + continue; + } + + int start = i; + int red = currentRed[i]; + int green = currentGreen[i]; + int blue = currentBlue[i]; + int white = currentWhite[i]; + + changedLedCount++; + i++; + + while (i < ledCount + && isChanged(i) + && currentRed[i] == red + && currentGreen[i] == green + && currentBlue[i] == blue + && currentWhite[i] == white) { + changedLedCount++; + i++; + } + + segments.add(new LedSegment(start, i - 1, new LedColor(red, green, blue, white))); + } + + return new DiffResult(segments, changedLedCount); + } + + private List buildCompressedSegments(int maxSegmentCount) { + int segmentCount = Math.max(1, Math.min(maxSegmentCount, ledCount)); + List compressed = new ArrayList<>(segmentCount); + + for (int bucket = 0; bucket < segmentCount; bucket++) { + int start = bucket * ledCount / segmentCount; + int end = ((bucket + 1) * ledCount / segmentCount) - 1; + if (end < start) { + continue; + } + + long red = 0; + long green = 0; + long blue = 0; + long white = 0; + + int length = end - start + 1; + for (int i = start; i <= end; i++) { + red += currentRed[i]; + green += currentGreen[i]; + blue += currentBlue[i]; + white += currentWhite[i]; + } + + LedColor average = new LedColor( + (int) Math.round(red / (double) length), + (int) Math.round(green / (double) length), + (int) Math.round(blue / (double) length), + (int) Math.round(white / (double) length)); + + compressed.add(new LedSegment(start, end, average)); + } + + return compressed; + } + + private boolean isChanged(int ledIndex) { + return currentRed[ledIndex] != previousRed[ledIndex] + || currentGreen[ledIndex] != previousGreen[ledIndex] + || currentBlue[ledIndex] != previousBlue[ledIndex] + || currentWhite[ledIndex] != previousWhite[ledIndex]; + } + + private void clearCurrentFrame() { + for (int i = 0; i < ledCount; i++) { + currentRed[i] = 0; + currentGreen[i] = 0; + currentBlue[i] = 0; + currentWhite[i] = 0; + } + } + + private void copyCurrentToPrevious() { + for (int i = 0; i < ledCount; i++) { + previousRed[i] = currentRed[i]; + previousGreen[i] = currentGreen[i]; + previousBlue[i] = currentBlue[i]; + previousWhite[i] = currentWhite[i]; + } + } + + private record DiffResult(List segments, int changedLedCount) { + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/BlinkEffect.java b/src/main/java/frc/robot/util/lighting/effects/BlinkEffect.java new file mode 100644 index 0000000..c1f7b01 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/BlinkEffect.java @@ -0,0 +1,35 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +public class BlinkEffect extends LightEffect { + private final LedColor onColor; + private final LedColor offColor; + private final double hz; + + public BlinkEffect( + LedRange range, + LedColor onColor, + LedColor offColor, + double hz, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.onColor = onColor; + this.offColor = offColor; + this.hz = hz; + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + if (hz <= 0) { + return onColor; + } + double cycle = nowSeconds * hz; + boolean on = (cycle - Math.floor(cycle)) < 0.5; + return on ? onColor : offColor; + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/BreatheEffect.java b/src/main/java/frc/robot/util/lighting/effects/BreatheEffect.java new file mode 100644 index 0000000..805c69e --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/BreatheEffect.java @@ -0,0 +1,38 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +public class BreatheEffect extends LightEffect { + private final LedColor color; + private final double hz; + private final double minScalar; + private final double maxScalar; + + public BreatheEffect( + LedRange range, + LedColor color, + double hz, + double minScalar, + double maxScalar, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.color = color; + this.hz = hz; + this.minScalar = LedColor.clamp01(Math.min(minScalar, maxScalar)); + this.maxScalar = LedColor.clamp01(Math.max(minScalar, maxScalar)); + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + if (hz <= 0) { + return color.scale(maxScalar); + } + double wave01 = 0.5 + 0.5 * Math.sin(2.0 * Math.PI * hz * nowSeconds); + double scalar = minScalar + (maxScalar - minScalar) * wave01; + return color.scale(scalar); + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/ChaseEffect.java b/src/main/java/frc/robot/util/lighting/effects/ChaseEffect.java new file mode 100644 index 0000000..ae11a9e --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/ChaseEffect.java @@ -0,0 +1,60 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +public class ChaseEffect extends LightEffect { + private final LedColor color; + private final int width; + private final double hz; + private final boolean wrap; + + public ChaseEffect( + LedRange range, + LedColor color, + int width, + double hz, + boolean wrap, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.color = color; + this.width = Math.max(1, width); + this.hz = hz; + this.wrap = wrap; + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + int length = getRange().length(); + int localIndex = ledIndex - getRange().startInclusive(); + + if (length <= 0) { + return null; + } + + if (hz <= 0) { + return localIndex < width ? color : null; + } + + int step = (int) Math.floor(nowSeconds * hz); + + if (wrap) { + int head = floorMod(step, length); + int delta = floorMod(localIndex - head, length); + return delta >= 0 && delta < width ? color : null; + } + + int cycleLength = length + width; + int start = -width + 1 + floorMod(step, cycleLength); + boolean lit = localIndex >= start && localIndex < start + width; + return lit ? color : null; + } + + private static int floorMod(int x, int y) { + int mod = x % y; + return mod < 0 ? mod + y : mod; + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/ConvergingArrowsEffect.java b/src/main/java/frc/robot/util/lighting/effects/ConvergingArrowsEffect.java new file mode 100644 index 0000000..9bc5db3 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/ConvergingArrowsEffect.java @@ -0,0 +1,88 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +/** + * Aiming reticle: two arrows that converge toward the center as "exactness" goes from 0 to 1. + * Left arrow "---->" grows from the left toward center; right arrow "<----" grows from the right. + * Drive via {@link #setProgress(double)} with 0 = spread apart, 1 = meeting at center (on target). + */ +public class ConvergingArrowsEffect extends LightEffect { + private final LedColor color; + private final boolean wedge; // true = bright at tip, dim at base (arrow shape) + private double exactness01; + + public ConvergingArrowsEffect( + LedRange range, + LedColor color, + boolean wedge, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.color = color; + this.wedge = wedge; + this.exactness01 = 0.0; + } + + @Override + public boolean setProgress(double progress01) { + this.exactness01 = LedColor.clamp01(progress01); + return true; + } + + @Override + public boolean updateInput(Double inputArg) { + if (inputArg == null) { + return false; + } + return setProgress(inputArg); + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + int length = getRange().length(); + int localIndex = ledIndex - getRange().startInclusive(); + + if (length <= 0) { + return null; + } + + double center = (length - 1) * 0.5; + // Left arrow tip: at 0 when exactness=0, at center when exactness=1 + double tipLeft = exactness01 * center; + // Right arrow tip: at length-1 when exactness=0, at center when exactness=1 + double tipRight = (length - 1) - exactness01 * ((length - 1) - center); + + // Left arrow: indices 0..tipLeft (inclusive in continuous sense) + if (localIndex <= tipLeft + 0.5) { + if (tipLeft <= 0) { + return null; + } + if (wedge) { + double t = tipLeft > 0 ? localIndex / tipLeft : 1.0; + t = LedColor.clamp01(t); + return color.scale(t); + } + return color; + } + + // Right arrow: indices tipRight..length-1 + if (localIndex >= tipRight - 0.5) { + double rightSpan = (length - 1) - tipRight; + if (rightSpan <= 0) { + return null; + } + if (wedge) { + double t = rightSpan > 0 ? ((length - 1) - localIndex) / rightSpan : 1.0; + t = LedColor.clamp01(t); + return color.scale(t); + } + return color; + } + + return null; + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/LarsonScannerEffect.java b/src/main/java/frc/robot/util/lighting/effects/LarsonScannerEffect.java new file mode 100644 index 0000000..a29a79d --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/LarsonScannerEffect.java @@ -0,0 +1,75 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +/** + * Larson Scanner (Knight Rider / Cylon): a single "eye" that moves back and forth + * along the strip, with an optional trailing fade. + */ +public class LarsonScannerEffect extends LightEffect { + private final LedColor color; + private final int eyeWidth; + private final int trailLength; + private final double hz; + + public LarsonScannerEffect( + LedRange range, + LedColor color, + int eyeWidth, + int trailLength, + double hz, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.color = color; + this.eyeWidth = Math.max(1, eyeWidth); + this.trailLength = Math.max(0, trailLength); + this.hz = hz; + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + int length = getRange().length(); + int localIndex = ledIndex - getRange().startInclusive(); + + if (length <= 0) { + return null; + } + + if (hz <= 0) { + // Static: center at start, eye + trail only there + int halfEye = eyeWidth / 2; + int dist = Math.abs(localIndex - halfEye); + return sampleBrightness(dist); + } + + // Triangle wave: 0 -> 1 -> 0 over one period + double phase = (nowSeconds * hz) % 1.0; + if (phase < 0) { + phase += 1.0; + } + double pos01 = phase < 0.5 ? 2.0 * phase : 2.0 * (1.0 - phase); + double center = pos01 * (length - 1); + + double dist = Math.abs(localIndex - center); + return sampleBrightness(dist); + } + + private LedColor sampleBrightness(double distanceFromCenter) { + int halfEye = eyeWidth / 2; + if (distanceFromCenter <= halfEye) { + return color; + } + int fadeStart = halfEye; + int fadeEnd = halfEye + trailLength; + if (trailLength <= 0 || distanceFromCenter >= fadeEnd) { + return null; + } + double fade01 = 1.0 - (distanceFromCenter - fadeStart) / trailLength; + fade01 = LedColor.clamp01(fade01); + return color.scale(fade01); + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/MorseCodeEffect.java b/src/main/java/frc/robot/util/lighting/effects/MorseCodeEffect.java new file mode 100644 index 0000000..b977e36 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/MorseCodeEffect.java @@ -0,0 +1,148 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; + +public class MorseCodeEffect extends LightEffect { + private static final Map MORSE_TABLE = Map.ofEntries( + Map.entry('A', ".-"), + Map.entry('B', "-..."), + Map.entry('C', "-.-."), + Map.entry('D', "-.."), + Map.entry('E', "."), + Map.entry('F', "..-."), + Map.entry('G', "--."), + Map.entry('H', "...."), + Map.entry('I', ".."), + Map.entry('J', ".---"), + Map.entry('K', "-.-"), + Map.entry('L', ".-.."), + Map.entry('M', "--"), + Map.entry('N', "-."), + Map.entry('O', "---"), + Map.entry('P', ".--."), + Map.entry('Q', "--.-"), + Map.entry('R', ".-."), + Map.entry('S', "..."), + Map.entry('T', "-"), + Map.entry('U', "..-"), + Map.entry('V', "...-"), + Map.entry('W', ".--"), + Map.entry('X', "-..-"), + Map.entry('Y', "-.--"), + Map.entry('Z', "--.."), + Map.entry('0', "-----"), + Map.entry('1', ".----"), + Map.entry('2', "..---"), + Map.entry('3', "...--"), + Map.entry('4', "....-"), + Map.entry('5', "....."), + Map.entry('6', "-...."), + Map.entry('7', "--..."), + Map.entry('8', "---.."), + Map.entry('9', "----.")); + + private final LedColor onColor; + private final LedColor offColor; + private final double unitSeconds; + private List segments; + private double cycleDurationSeconds; + + private record Segment(boolean on, double durationSeconds) { + } + + public MorseCodeEffect( + LedRange range, + String message, + LedColor onColor, + LedColor offColor, + double unitSeconds, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.onColor = onColor; + this.offColor = offColor; + this.unitSeconds = unitSeconds > 0.0 ? unitSeconds : 0.1; + rebuildSequence(message); + } + + @Override + public boolean updateInput(String message) { + rebuildSequence(message); + return true; + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + if (segments.isEmpty()) { + return offColor; + } + + double cycleTime = nowSeconds % cycleDurationSeconds; + for (Segment segment : segments) { + if (cycleTime < segment.durationSeconds()) { + return segment.on() ? onColor : offColor; + } + cycleTime -= segment.durationSeconds(); + } + + return offColor; + } + + private void rebuildSequence(String message) { + this.segments = buildSegments(message == null ? "" : message); + this.cycleDurationSeconds = 0.0; + for (Segment segment : segments) { + this.cycleDurationSeconds += segment.durationSeconds(); + } + } + + private List buildSegments(String message) { + List built = new ArrayList<>(); + String[] words = message.toUpperCase().trim().split("\\s+"); + + for (int wordIdx = 0; wordIdx < words.length; wordIdx++) { + String word = words[wordIdx]; + if (word.isEmpty()) { + continue; + } + + for (int charIdx = 0; charIdx < word.length(); charIdx++) { + char currentChar = word.charAt(charIdx); + String morse = MORSE_TABLE.get(currentChar); + if (morse == null) { + continue; + } + + for (int symbolIdx = 0; symbolIdx < morse.length(); symbolIdx++) { + char symbol = morse.charAt(symbolIdx); + double onDurationUnits = symbol == '-' ? 3.0 : 1.0; + built.add(new Segment(true, onDurationUnits * unitSeconds)); + + if (symbolIdx < morse.length() - 1) { + built.add(new Segment(false, 1.0 * unitSeconds)); + } + } + + if (charIdx < word.length() - 1) { + built.add(new Segment(false, 3.0 * unitSeconds)); + } + } + + if (wordIdx < words.length - 1) { + built.add(new Segment(false, 7.0 * unitSeconds)); + } + } + + if (built.isEmpty()) { + built.add(new Segment(false, unitSeconds)); + } + + return built; + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/ProgressBarEffect.java b/src/main/java/frc/robot/util/lighting/effects/ProgressBarEffect.java new file mode 100644 index 0000000..c5ea372 --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/ProgressBarEffect.java @@ -0,0 +1,51 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +public class ProgressBarEffect extends LightEffect { + private final LedColor fillColor; + private final LedColor emptyColorOrNull; + private double progress01; + + public ProgressBarEffect( + LedRange range, + LedColor fillColor, + LedColor emptyColorOrNull, + int priority, + BlendMode blendMode) { + super(range, priority, blendMode); + this.fillColor = fillColor; + this.emptyColorOrNull = emptyColorOrNull; + this.progress01 = 0.0; + } + + @Override + public boolean setProgress(double progress01) { + this.progress01 = LedColor.clamp01(progress01); + return true; + } + + @Override + public boolean updateInput(Double inputArg) { + if (inputArg == null) { + return false; + } + return setProgress(inputArg); + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + int length = getRange().length(); + int localIndex = ledIndex - getRange().startInclusive(); + int filledCount = (int) Math.round(progress01 * length); + filledCount = Math.max(0, Math.min(length, filledCount)); + boolean filled = localIndex < filledCount; + if (filled) { + return fillColor; + } + return emptyColorOrNull; + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/RainbowEffect.java b/src/main/java/frc/robot/util/lighting/effects/RainbowEffect.java new file mode 100644 index 0000000..a708c9d --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/RainbowEffect.java @@ -0,0 +1,70 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +public class RainbowEffect extends LightEffect { + private final double hz; + private final double brightness; + + public RainbowEffect(LedRange range, double hz, double brightness, int priority, BlendMode blendMode) { + super(range, priority, blendMode); + this.hz = hz; + this.brightness = LedColor.clamp01(brightness); + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + int length = getRange().length(); + int localIndex = ledIndex - getRange().startInclusive(); + + double hue = (localIndex / (double) Math.max(1, length)) + (nowSeconds * hz); + hue = hue - Math.floor(hue); + + return hsvToRgb(hue, 1.0, brightness); + } + + private static LedColor hsvToRgb(double h, double s, double v) { + double chroma = v * s; + double hPrime = h * 6.0; + double x = chroma * (1.0 - Math.abs((hPrime % 2.0) - 1.0)); + + double r1; + double g1; + double b1; + + if (hPrime < 1.0) { + r1 = chroma; + g1 = x; + b1 = 0; + } else if (hPrime < 2.0) { + r1 = x; + g1 = chroma; + b1 = 0; + } else if (hPrime < 3.0) { + r1 = 0; + g1 = chroma; + b1 = x; + } else if (hPrime < 4.0) { + r1 = 0; + g1 = x; + b1 = chroma; + } else if (hPrime < 5.0) { + r1 = x; + g1 = 0; + b1 = chroma; + } else { + r1 = chroma; + g1 = 0; + b1 = x; + } + + double m = v - chroma; + int r = (int) Math.round((r1 + m) * 255.0); + int g = (int) Math.round((g1 + m) * 255.0); + int b = (int) Math.round((b1 + m) * 255.0); + return new LedColor(r, g, b, 0); + } +} diff --git a/src/main/java/frc/robot/util/lighting/effects/SolidEffect.java b/src/main/java/frc/robot/util/lighting/effects/SolidEffect.java new file mode 100644 index 0000000..fc421df --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/effects/SolidEffect.java @@ -0,0 +1,20 @@ +package frc.robot.util.lighting.effects; + +import frc.robot.util.lighting.BlendMode; +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LedRange; +import frc.robot.util.lighting.LightEffect; + +public class SolidEffect extends LightEffect { + private final LedColor color; + + public SolidEffect(LedRange range, LedColor color, int priority, BlendMode blendMode) { + super(range, priority, blendMode); + this.color = color; + } + + @Override + public LedColor sample(int ledIndex, double nowSeconds) { + return color; + } +} diff --git a/src/main/java/frc/robot/util/lighting/internal/CandleLightsTransport.java b/src/main/java/frc/robot/util/lighting/internal/CandleLightsTransport.java new file mode 100644 index 0000000..026696f --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/internal/CandleLightsTransport.java @@ -0,0 +1,32 @@ +package frc.robot.util.lighting.internal; + +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.RGBWColor; + +import frc.robot.util.lighting.LedColor; +import frc.robot.util.lighting.LightsEngine; +import frc.robot.util.lighting.LightsEngine.LedSegment; + +import java.util.List; + +import org.littletonrobotics.junction.Logger; + +public class CandleLightsTransport implements LightsTransport { + private final CANdle candle; + + public CandleLightsTransport(CANdle candle) { + this.candle = candle; + } + + public void writeSegments(List segments) { + Logger.recordOutput("Lights/Transport/SegmentWriteCount", segments.size()); + + for (LedSegment segment : segments) { + LedColor color = segment.color(); + candle.setControl( + new SolidColor(segment.startInclusive(), segment.endInclusive()) + .withColor(new RGBWColor(color.red(), color.green(), color.blue(), color.white()))); + } + } +} diff --git a/src/main/java/frc/robot/util/lighting/internal/LightsTransport.java b/src/main/java/frc/robot/util/lighting/internal/LightsTransport.java new file mode 100644 index 0000000..c31225a --- /dev/null +++ b/src/main/java/frc/robot/util/lighting/internal/LightsTransport.java @@ -0,0 +1,9 @@ +package frc.robot.util.lighting.internal; + +import java.util.List; + +import frc.robot.util.lighting.LightsEngine.LedSegment; + +public interface LightsTransport { + void writeSegments(List segments); +} diff --git a/src/proto b/src/proto index 4720743..77b6dc5 160000 --- a/src/proto +++ b/src/proto @@ -1 +1 @@ -Subproject commit 47207432b8869a80b6369c1a8233b50935096fb9 +Subproject commit 77b6dc5f86d8ca1483c64b8b5271a9f7accd8fd3