diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 000000000..9d4ed3626 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,7 @@ +FROM mcr.microsoft.com/devcontainers/java:1-17-bookworm + +RUN apt update && \ + export DEBIAN_FRONTEND=noninteractive && \ + apt install -y --no-install-recommends \ + screen \ + && rm -rf /var/lib/apt/lists/* \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 343e12157..25562d471 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ // README at: https://github.com/devcontainers/templates/tree/main/src/java { "name": "Java", - "image": "mcr.microsoft.com/devcontainers/java:1-17-bookworm", + "build": { "dockerfile": "Dockerfile" }, "features": { "ghcr.io/devcontainers/features/java:1": { diff --git a/.gitignore b/.gitignore index 8385d4a5d..fd268dd3c 100644 --- a/.gitignore +++ b/.gitignore @@ -172,6 +172,7 @@ out/ # Simulation GUI and other tools window save file networktables.json simgui.json +simgui-ds.json *-window.json # Simulation data log directory @@ -188,4 +189,4 @@ compile_commands.json .factorypath # Don't commit the cache of downloaded sim files -/buildSim/ \ No newline at end of file +/build-sim/ \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index 0875db2b6..7e1a4215c 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -23,18 +23,11 @@ "desktop": false, }, { - "type": "java", - "name": "Start 3d simulation mode", - "request": "launch", - "mainClass": "com.team766.hal.simulator.RobotMain", - "args": ["-vr_connector"] - }, - { - "type": "java", - "name": "Start Maroon simulation", + "type": "wpilib", + "name": "Run simulation", "request": "launch", - "mainClass": "com.team766.hal.simulator.RobotMain", - "args": ["-maroon_sim"] + "desktop": true, + "preLaunchTask": "Run Simulator" } ] } diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 2dfdc5301..b005dfdf2 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -3,20 +3,79 @@ // for the documentation about the tasks.json format "version": "2.0.0", "tasks": [ + // The "Run Simulator" task starts the simulator to run in parallel with the robot code. + { + "label": "Run Simulator", + "type": "shell", + "command": "./deploy_sim.py", + "hide": true, + "showOutput": "always", + // This task is a dependency of the Deploy Sim task which runs the robot code, and so + // it needs to finish before the robot code will be allowed to start running. However, + // we want the simulator to run in parallel with the robot code, so we set it as a + // background task, which allows a task to signal when it has finished (with console + // output that matches the endsPattern of the problemMatcher) and then continue running + // in the background. We then reactivate the task after the robot code starts running + // (using the beginsPattern). + "isBackground": true, + "problemMatcher": { + "pattern": { + "regexp": "Simulation doesn't report errors so we don't need a pattern" + }, + "background": { + "activeOnStart": true, + "beginsPattern": ".*Robot code started.*", + "endsPattern": ".*Waiting for robot code to start.*" + } + }, + "presentation": { + "group": "robotSim", + "echo": true, + "reveal": "always", + "panel": "dedicated", + "showReuseMessage": false, + "clear": true, + } + }, + // The "Cleanup old robot code instances" task is used to terminate any existing instances + // of the robot code before starting a new one. + // Codespaces can orphan task processes if the user refreshes the browser tab or + // disconnects, so we need to manually clean them up. + { + "label": "Cleanup old robot code instances", + "type": "shell", + "command": "pkill --signal TERM --full simulateJava || true", + "hide": true, + "problemMatcher": [], + "presentation": { + "group": "robotSim", + "echo": true, + "showReuseMessage": false, + "clear": true, + "close": true + }, + }, { "label": "Deploy Sim", "type": "shell", - "command": "./deploy_sim.sh", + "command": "./gradlew simulateJava", "problemMatcher": [], "showOutput": "always", "presentation": { + "group": "robotSim", "echo": true, "reveal": "always", - "focus": true, "panel": "dedicated", "showReuseMessage": false, "clear": true - } + }, + "dependsOrder": "sequence", + "dependsOn": [ + // Before starting the robot code, make sure that any previous instances are + // stopped, and that the simulator is started. + "Cleanup old robot code instances", + "Run Simulator" + ] }, { "label": "Gradle Build", diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 000000000..0de33d22f --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,141 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project Overview + +This is Team 766's FRC (FIRST Robotics Competition) 2025 robot code, written in Java using WPILib. The codebase uses a custom framework built around a Mechanism/Procedure architecture with resource reservation, plus an integrated physics simulator for testing without hardware. + +## Build and Development Commands + +### Building and Testing +- `./gradlew build` - Compile code and run all tests +- `./gradlew test` - Run tests only +- `./gradlew spotlessApply` - Auto-format code (Java, Gradle, XML) using Spotless +- `./gradlew spotlessCheck` - Check code formatting without applying changes + +### Deployment +- `./gradlew deploy` - Deploy to the RoboRIO (robot must be connected) +- Config files are deployed from `src/main/deploy/configs/` to `/home/lvuser/deploy/configs` on the robot + +### Simulation +- Run the "Simulate Robot Code" task in VS Code to start robot code in simulation mode +- `./deploy_sim.py` - Start the external physics simulator (waits for robot code on port 5800, then launches in a `screen` session) +- Simulation mode is configured via `simConfig.txt` (JSON format) +- The simulator provides a web-based 3D visualization of robot physics + +## Architecture + +### Framework Layer (`com.team766.framework`) +The custom framework provides the core abstractions: + +**Mechanism**: Base class for all robot subsystems. Mechanisms must be reserved before use via Procedures. Each Mechanism has a `run()` method called periodically and an `onMechanismIdle()` method called when no Procedure is using it. Mechanisms integrate with WPILib2's Command system via internal proxy subsystems. + +**Procedure**: Asynchronous tasks that can reserve Mechanisms. Procedures receive a `Context` object that provides cooperative-multitasking primitives like `waitFor()`, `waitForSeconds()`, etc. Procedures run as WPILib2 Commands under the hood. + +**Context**: Execution context for Procedures providing wait/yield operations. Procedures can be interrupted if another Procedure with overlapping Mechanism reservations is started. + +**Rule/RuleEngine**: Event-driven behavior system. Rules trigger Procedures based on conditions (e.g., button presses in operator interface). Used for teleoperated control. + +### Hardware Abstraction Layer (`com.team766.hal`) +Abstraction layer over WPILib hardware APIs: +- Multiple implementations: `wpilib` (real robot), `simulator` (integrated sim), `mock` (unit tests) +- `RobotProvider` provides factory methods for motors, sensors, etc. +- `MotorController` interface for motor control with position/velocity PID, current limiting, and sensor scaling +- Configuration loaded from JSON files at runtime (see Configuration section) + +### Robot Implementations (`com.team766.robot`) +Multiple robot configurations in separate packages: +- `reva` - 2024 robot with swerve drive, shoulder, intake, shooter, climber +- `reva_2025` - Active development for 2025 season +- `common` - Shared code including swerve drive implementation +- `example`, `gatorade`, etc. - Other robot configurations + +Each robot package contains: +- `Robot.java` - Implements `RobotConfigurator` to initialize mechanisms, operator interface, and autonomous modes +- `mechanisms/` - Mechanism implementations +- `procedures/` - Procedure implementations for autonomous and complex actions +- `OI.java` - RuleEngine implementation that binds controls to Procedures + +### Swerve Drive +Swerve drive is in `com.team766.robot.common.mechanisms.SwerveDrive`: +- Field-oriented control with gyro-based rotation compensation +- Independent control of translation and rotation +- Cross-wheels mode for resisting movement +- Odometry integration for position tracking +- Configuration via `SwerveConfig` class (wheel locations, CAN bus, current limits) +- Controlled via `controlFieldOriented(x, y, rotation)` or `drive(chassisSpeeds)` + +See `docs/SwerveDrive.md` for detailed implementation notes and bringup instructions. + +### Operator Interface +Controls are organized by role: +- **Driver** (`DriverOI`) - Two Thrustmaster T.16000M joysticks for robot movement +- **Box Operator** (`BoxOpOI`) - Xbox controller for mechanism control +- **Debug** (`DebugOI`) - Megalodon macro pad for pit testing individual mechanisms + +See `docs/OperatorInterface.md` for complete control mappings. + +## Configuration System + +Robot hardware configuration is stored in JSON files: +- Main config: `simConfig.txt` (JSON despite extension) at project root +- Additional configs can be in `src/main/deploy/configs/` +- Config is read via `com.team766.config.ConfigFileReader` +- Access config values using `ConfigValue` types in code +- Supports motor controllers, sensors, PID constants, physical parameters +- Specify which robot configuration to load via `"robotConfigurator"` field pointing to a class implementing `RobotConfigurator` + +## Logging and Telemetry + +Uses AdvantageKit for logging: +- `@AutoLog` annotation for automatic logging of mechanism state +- Logs written to `logs/` directory +- Web dashboard on port 5800 during simulation +- Replay logs using `./gradlew replayWatch` + +## Testing + +- JUnit 5 for unit tests +- ErrorProne static analysis (custom check: `DontDiscardProcedures` ensures Procedures are scheduled) +- AspectJ used to enforce that Procedures are not discarded without being scheduled +- Mock HAL implementation for hardware-independent testing + +## Common Development Patterns + +### Creating a New Mechanism +1. Extend `Mechanism` (or `MechanismWithStatus` for status publishing) +2. Add config entries for hardware in config file +3. Initialize hardware in constructor using `RobotProvider.instance.getXXX(configValue)` +4. Implement `run()` for periodic updates +5. Implement `onMechanismIdle()` to stop/safe state when not reserved +6. Add to `Robot.initializeMechanisms()` + +### Creating a New Procedure +1. Extend `Procedure` or `InstantProcedure` +2. Call `reserve(mechanism)` in constructor for any Mechanisms you'll use +3. Implement `run(Context context)` with your logic +4. Use `context.waitFor()`, `context.waitForSeconds()`, etc. for timing +5. Trigger from OI using `Rule.when(condition).then(procedureName, mechanism1, mechanism2, ...)` + +### Creating a New Autonomous Mode +1. Create a Procedure for the autonomous routine in `procedures/auton_routines/` +2. Add to `Robot.getAutonomousModes()` array +3. Use WPILib `PathPlanner` for path following with swerve drive + +## Simulation Mode + +The codebase includes an integrated physics simulator: +- Set `"simulationMode": "VrConnector"` in config to enable +- Simulator runs separately and connects via network sockets +- Simulates motors, sensors, pneumatics, and robot physics +- Web UI shows 3D robot visualization and controls +- Implementation in `com.team766.simulator` package + +## Project Structure Notes + +- Generated code (protobuf, build constants) in `build/generated/` +- Vendor dependencies in `vendordeps/` (AdvantageKit, CTRE, REV, etc.) +- Uses Gradle with GradleRIO plugin for FRC-specific build tasks +- DevContainer support for consistent development environment +- CI via GitHub Actions: builds and tests on every push/PR diff --git a/deploy_sim.py b/deploy_sim.py new file mode 100755 index 000000000..596894144 --- /dev/null +++ b/deploy_sim.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 + +import os +import shutil +import socket +import subprocess +import time + +def main(argv=None): + # Handle the case where somebody tried to run this script from somewhere besides the root + # directory of the repo. + os.chdir(os.path.dirname(os.path.abspath(__file__))) + project_base = os.getcwd() + + # Create a directory in which to keep all of the simulator's files. + os.makedirs("build-sim", exist_ok=True) + os.chdir("build-sim") + + # Download the simulator package from github. + # The -N flag to wget skips the download if we already have the most recent version (as + # determined by comparing the modified timestamp of the file to the Last-Modified header + # returned by the server). + sim_package = "sim.tar.gz" + try: + t1 = os.path.getmtime(sim_package) + except OSError: + t1 = None + subprocess.check_call([ + "wget", + "-N", + f"https://github.com/Team766/2020Sim/releases/latest/download/{sim_package}", + ]) + t2 = os.path.getmtime(sim_package) + + # Extract the simulator package (unless we've already extracted the most recent version). + extracted_dir = "files" + if t1 != t2 or not os.path.isdir(extracted_dir): + shutil.rmtree(extracted_dir, ignore_errors=True) + os.makedirs(extracted_dir, exist_ok=True) + subprocess.check_call([ + "tar", + f"--directory={extracted_dir}", + "-xf", + sim_package, + ]) + + os.chdir(extracted_dir) + + # If you change this print, also update the regex in .vscode/tasks.json + print("Waiting for robot code to start") + + # Wait until the robot code has started before starting the simulator. + # It determines whether the robot code is running by polling whether it can connect to the HTTP + # server on port 5800 that's built into the robot code. + # This is because the simulator's run script prints the link to the viewer webpage, but we don't + # want the user opening that until the robot code has actually started running (otherwise, the + # right-side pane that tries to display data from the HTTP server built into the robot code will + # fail to load). + iter = 0 + while True: + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect(("localhost", 5800)) + break + except ConnectionRefusedError: + # Display a simple animation to show signs of life while we're waiting for the + # robot code to start. + iter += 1 + print("." * (iter % 6) + " ", end="\r") + time.sleep(1) + + # Invoke the simulator package's entrypoint. + # NOTE(2025-09-19): There appears to be some issue in Codespaces right now that causes + # the Run Simulator task to randomly restart (sometimes accompanied by the message + # "Extension Host Process exited with code: null, signal: SIGTERM"). Thus, we run the simulator + # in a GNU Screen session so that if the task restarts, we can just reconnect to the existing + # instance of the simulator without any disruption to the user. + os.execlp( + "screen", "screen", "-D", "-R", "-S", "simulator", + "./run.sh", project_base) + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass \ No newline at end of file diff --git a/deploy_sim.sh b/deploy_sim.sh deleted file mode 100755 index 2a7240bab..000000000 --- a/deploy_sim.sh +++ /dev/null @@ -1,29 +0,0 @@ -#!/bin/bash - -set -euo pipefail - -builtin cd "$(dirname -- "${BASH_SOURCE[0]}")" -project_base="$(pwd)" - -./gradlew jar - -jar_file=( $(readlink -f build/libs/*.jar) ) -[ "${#jar_file[@]}" -eq 1 ] || (echo "Output jar file could not be determined"; exit 1) - -mkdir -p buildSim -cd buildSim - -sim_package="sim.tar.gz" -t1="$(stat -c %y "$sim_package" || true)" -wget -N "https://github.com/Team766/2020Sim/releases/latest/download/$sim_package" || [ -f "$sim_package" ] || exit 1 -t2="$(stat -c %y "$sim_package")" - -extracted_dir="files" -if [ "$t1" != "$t2" -o ! -d "$extracted_dir" ]; then - rm -rf "$extracted_dir" - mkdir -p "$extracted_dir" - tar --directory="$extracted_dir" -xf "$sim_package" -fi -cd "$extracted_dir" - -exec ./run.sh "$project_base" "$jar_file" \ No newline at end of file diff --git a/docs/AprilTagSimulation.md b/docs/AprilTagSimulation.md new file mode 100644 index 000000000..6aa9e207b --- /dev/null +++ b/docs/AprilTagSimulation.md @@ -0,0 +1,273 @@ +# AprilTag Simulation + +This document explains how to simulate AprilTag detections during robot simulation, mimicking the behavior of the Orin coprocessor that normally provides vision data. + +## Overview + +In the real robot: +- **Orin coprocessor** runs vision processing +- Detects AprilTags using cameras +- Publishes detections to **NetworkTables** as double arrays +- **RoboRIO** reads these via `GetOrinRawValue` class +- Data format: `[timestamp, tagId, x, y, z, timestamp, tagId, x, y, z, ...]` + +In simulation: +- **AprilTagSimulator** replaces the Orin +- Publishes fake AprilTag detections based on simulated robot position +- Uses the same NetworkTables interface +- Robot code works identically - it just reads from NetworkTables + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Robot Code (simulateJava) │ +│ │ +│ ┌─────────────────┐ ┌──────────────────────┐ │ +│ │ RobotMain │ │ Orin Mechanism │ │ +│ │ simulationInit()│ │ │ │ +│ │ │ │ GetOrinRawValue │ │ +│ │ Creates │ │ .getRawPoseData() │ │ +│ │ AprilTagSim ────┼──────┐ │ reads from NT │ │ +│ └─────────────────┘ │ └──────────────────────┘ │ +│ │ │ +│ ┌─────────────────┐ │ │ +│ │ simulationPeriodic() │ │ │ +│ │ calls: │ │ │ +│ │ aprilTagSim │ │ │ +│ │ .simulateAprilTags() │ │ +│ └─────────────────┘ │ │ +│ │ │ +│ ┌─────────────────────▼───────────────────────┐ │ +│ │ AprilTagSimulator │ │ +│ │ │ │ +│ │ 1. Read robot position from │ │ +│ │ ProgramInterface.robotPosition │ │ +│ │ │ │ +│ │ 2. Calculate which field tags are visible │ │ +│ │ (within detection range) │ │ +│ │ │ │ +│ │ 3. Transform tag positions from field │ │ +│ │ frame to robot-relative frame │ │ +│ │ │ │ +│ │ 4. Publish to NetworkTables: │ │ +│ │ /SmartDashboard/ │ │ +│ └──────────────────┬──────────────────────────┘ │ +│ │ │ +└─────────────────────┼────────────────────────────────────────┘ + │ + │ NetworkTables + │ (UDP port 5810) + │ + ↓ + [Robot code reads it back via GetOrinRawValue] +``` + +## Usage + +### Option 1: Integrated Mode (Recommended) + +The AprilTag simulator runs **inside** the robot code process. This is simpler and already integrated. + +1. **Start simulation**: + ```bash + ./gradlew simulateJava + ``` + +2. **Configuration** (in `RobotMain.simulationInit()`): + ```java + aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", // NetworkTables topic name + 5.0, // Detection range in meters + 20.0 // Publish rate in Hz + ); + ``` + +3. **Configure field tags** (in `AprilTagSimulator.initializeFieldTags()`): + ```java + // Add tags at known field positions + fieldTags.add(new FieldAprilTag(1, 0.0, 0.0, 0.5)); + fieldTags.add(new FieldAprilTag(2, 5.0, 0.0, 0.5)); + // ... add more based on your field layout + ``` + +4. **Update your Orin mechanism** to use the correct topic: + ```java + // In your robot's Orin mechanism or wherever you create GetOrinRawValue: + GetOrinRawValue orinReader = new GetOrinRawValue("apriltag_detections", 0.1); + ``` + +### Option 2: Standalone Process + +Run the AprilTag simulator as a **separate Java process**. This better mimics the real Orin architecture. + +1. **Add to `build.gradle`**: + ```gradle + task runAprilTagSim(type: JavaExec) { + mainClass = "com.team766.simulator.StandaloneAprilTagSimulator" + classpath = sourceSets.main.runtimeClasspath + args = ["localhost", "apriltag_detections", "5.0", "20.0"] + // args: [ntServer, topicName, rangeMeters, rateHz] + } + ``` + +2. **Start robot code** (in one terminal): + ```bash + ./gradlew simulateJava + ``` + +3. **Start AprilTag simulator** (in another terminal): + ```bash + ./gradlew runAprilTagSim + ``` + + Or with custom arguments: + ```bash + ./gradlew runAprilTagSim --args="localhost my_topic 8.0 30.0" + ``` + +## Configuration + +### NetworkTables Topic Name + +The topic name must match what your Orin mechanism expects: + +**In simulation** (`RobotMain.java`): +```java +aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", // ← This topic name + 5.0, 20.0 +); +``` + +**In robot code** (wherever you create `GetOrinRawValue`): +```java +GetOrinRawValue orinReader = new GetOrinRawValue( + "apriltag_detections", // ← Must match! + 0.1 // covariance +); +``` + +### Field Tag Positions + +Update the field tag positions in `AprilTagSimulator.initializeFieldTags()`: + +```java +private void initializeFieldTags() { + // Tag ID, X (meters), Y (meters), Z (meters - height) + fieldTags.add(new FieldAprilTag(1, 0.0, 0.0, 0.5)); + fieldTags.add(new FieldAprilTag(2, 5.0, 0.0, 0.5)); + // ... add all field tags here +} +``` + +**Important**: These positions should match the actual AprilTag layout for your game's field. You can usually find these in the FRC game manual or WPILib's `AprilTagFieldLayout`. + +### Detection Parameters + +**Detection Range**: Maximum distance (in meters) at which tags can be detected +```java +aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", + 5.0, // ← Detection range in meters + 20.0 +); +``` + +**Publish Rate**: How often (Hz) to publish detections +```java +aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", + 5.0, + 20.0 // ← 20 Hz = every 50ms +); +``` + +## How It Works + +1. **Every simulation tick** (`simulationPeriodic`): + - `aprilTagSimulator.simulateAprilTags(currentTime)` is called + +2. **Inside `simulateAprilTags()`**: + - Reads robot position from `ProgramInterface.robotPosition` (x, y, heading) + - For each field tag: + - Calculates distance from robot to tag + - If within detection range: + - Transforms tag position from field frame to robot frame + - Adds to detection array: `[timestamp, tagId, x, y, z]` + - Publishes array to NetworkTables + +3. **Your robot code**: + - `Orin` mechanism calls `GetApriltagPoseData.getAllTags()` + - Which calls `GetOrinRawValue.getRawPoseData()` + - Which reads from NetworkTables + - Returns list of `TimestampedApriltag` objects + - Your code uses these for localization/targeting/etc. + +## Testing + +### Verify NetworkTables Communication + +1. Start simulation with AprilTag simulator +2. Open **OutlineViewer** (WPILib tool): + ```bash + ~/wpilib/2025/tools/OutlineViewer.jar + ``` +3. Connect to `localhost` +4. Look for `/SmartDashboard/apriltag_detections` +5. You should see arrays of doubles being published + +### Debug Output + +The simulator prints debug info when tags are detected: +``` +[AprilTagSim] Published 2 tags at t=5.43 +``` + +### Test with Specific Tags + +For testing, you can bypass the automatic detection and inject specific tags: +```java +// In your test code: +aprilTagSimulator.simulateSpecificTag( + currentTime, // timestamp + 1, // tag ID + 2.0, 0.5, 0.3 // x, y, z in robot frame +); +``` + +## Troubleshooting + +### "No tags detected" +- Check that field tags are configured in `initializeFieldTags()` +- Verify detection range is large enough +- Check that robot is moving in simulation (look at `ProgramInterface.robotPosition`) + +### "GetOrinRawValue throws ValueNotFoundOnTableError" +- Verify topic names match between simulator and `GetOrinRawValue` +- Check NetworkTables connection (use OutlineViewer) +- Make sure simulator is running and publishing + +### "Wrong tag positions" +- Verify field tag coordinates are correct (check field layout) +- Check that coordinate frame is correct (WPILib uses blue alliance origin) +- Make sure robot position is being set correctly in simulation + +## Future Enhancements + +Potential improvements to make simulation more realistic: + +1. **Camera FOV**: Only detect tags within camera field of view +2. **Occlusion**: Don't detect tags behind obstacles +3. **Noise**: Add realistic sensor noise to detections +4. **Latency**: Add network latency to simulate real Orin→RoboRIO delay +5. **Multiple cameras**: Simulate multiple cameras with different poses +6. **Tag orientation**: Include full 6DOF pose (currently only translation) + +## See Also + +- `src/main/java/com/team766/simulator/AprilTagSimulator.java` - Main simulator +- `src/main/java/com/team766/simulator/StandaloneAprilTagSimulator.java` - Standalone version +- `src/main/java/com/team766/orin/GetOrinRawValue.java` - NetworkTables reader +- `src/main/java/com/team766/orin/GetApriltagPoseData.java` - Data parser +- `src/main/java/com/team766/robot/reva/mechanisms/Orin.java` - Orin mechanism diff --git a/simConfig.txt b/simConfig.txt index a3afa2071..932efbe02 100644 --- a/simConfig.txt +++ b/simConfig.txt @@ -1,4 +1,6 @@ { + "robotConfigurator": "com.team766.robot.reva_2025.Robot", + "simulationMode": "VrConnector", "drive": { "leftMotor": { "deviceId": 6, diff --git a/src/main/java/com/team766/hal/mock/MockJoystick.java b/src/main/java/com/team766/hal/mock/MockJoystick.java index 03bb1aff2..31e968651 100755 --- a/src/main/java/com/team766/hal/mock/MockJoystick.java +++ b/src/main/java/com/team766/hal/mock/MockJoystick.java @@ -27,7 +27,8 @@ public double getAxis(final int axis) { @Override public boolean isAxisMoved(int axis) { - return getAxis(axis) >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); + return Math.abs(axisValues[axis]) + >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); } @Override diff --git a/src/main/java/com/team766/hal/simulator/RobotMain.java b/src/main/java/com/team766/hal/simulator/RobotMain.java deleted file mode 100755 index 5102c0cf0..000000000 --- a/src/main/java/com/team766/hal/simulator/RobotMain.java +++ /dev/null @@ -1,130 +0,0 @@ -package com.team766.hal.simulator; - -import com.team766.config.ConfigFileReader; -import com.team766.hal.GenericRobotMain; -import com.team766.hal.RobotProvider; -import com.team766.logging.LoggerExceptionUtils; -import com.team766.simulator.Program; -import com.team766.simulator.ProgramInterface; -import com.team766.simulator.Simulator; -import java.io.IOException; - -public class RobotMain { - enum Mode { - MaroonSim, - VrConnector, - } - - private GenericRobotMain robot; - private Runnable simulator; - - @SuppressWarnings("StaticAssignmentInConstructor") - public RobotMain(final Mode mode) { - try { - // TODO: update this to come from deploy directory? - ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); - RobotProvider.instance = new SimulationRobotProvider(); - - robot = new GenericRobotMain(); - - robot.robotInit(); - - ProgramInterface.program = - new Program() { - ProgramInterface.RobotMode prevRobotMode = null; - - @Override - public void step(double dt) { - switch (ProgramInterface.robotMode) { - case DISABLED: - if (prevRobotMode != ProgramInterface.RobotMode.DISABLED) { - robot.disabledInit(); - prevRobotMode = ProgramInterface.RobotMode.DISABLED; - } - robot.disabledPeriodic(); - break; - case AUTON: - if (prevRobotMode != ProgramInterface.RobotMode.AUTON) { - robot.autonomousInit(); - prevRobotMode = ProgramInterface.RobotMode.AUTON; - } - robot.autonomousPeriodic(); - break; - case TELEOP: - if (prevRobotMode != ProgramInterface.RobotMode.TELEOP) { - robot.teleopInit(); - prevRobotMode = ProgramInterface.RobotMode.TELEOP; - } - robot.teleopPeriodic(); - break; - default: - LoggerExceptionUtils.logException( - new IllegalArgumentException( - "Value of ProgramInterface.robotMode invalid. Provided value: " - + ProgramInterface.robotMode)); - break; - } - } - - @Override - public void reset() { - robot.resetAutonomousMode("simulation reset"); - } - }; - } catch (Exception exc) { - exc.printStackTrace(); - LoggerExceptionUtils.logException(exc); - } - - switch (mode) { - case MaroonSim: - simulator = new Simulator(); - break; - case VrConnector: - ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; - try { - simulator = new VrConnector(); - } catch (IOException ex) { - throw new RuntimeException( - "Error initializing communication with 3d Simulator", ex); - } - break; - default: - LoggerExceptionUtils.logException( - new IllegalArgumentException( - "Unknown simulator mode. ProgramInterface.robotMode: " - + ProgramInterface.robotMode)); - break; - } - } - - public void run() { - try { - simulator.run(); - } catch (Exception exc) { - exc.printStackTrace(); - LoggerExceptionUtils.logException(exc); - } - } - - public static void main(final String[] args) { - if (args.length != 1) { - System.err.println("Needs -maroon_sim or -vr_connector"); - System.exit(1); - } - Mode mode; - switch (args[0]) { - case "-maroon_sim": - mode = Mode.MaroonSim; - break; - case "-vr_connector": - mode = Mode.VrConnector; - break; - default: - System.err.println("Needs -maroon_sim or -vr_connector"); - System.exit(1); - return; - } - new RobotMain(mode).run(); - } -} diff --git a/src/main/java/com/team766/hal/simulator/SimulatorInterface.java b/src/main/java/com/team766/hal/simulator/SimulatorInterface.java new file mode 100644 index 000000000..4b1969d9c --- /dev/null +++ b/src/main/java/com/team766/hal/simulator/SimulatorInterface.java @@ -0,0 +1,10 @@ +package com.team766.hal.simulator; + +public interface SimulatorInterface { + /** + * @return deltaTime for the pending program step + */ + double prepareStep(); + + void setResetHandler(Runnable handler); +} diff --git a/src/main/java/com/team766/hal/simulator/VrConnector.java b/src/main/java/com/team766/hal/simulator/VrConnector.java index 2b60c1f2a..7296fb61c 100644 --- a/src/main/java/com/team766/hal/simulator/VrConnector.java +++ b/src/main/java/com/team766/hal/simulator/VrConnector.java @@ -16,7 +16,7 @@ import java.util.List; import java.util.Map; -public class VrConnector implements Runnable { +public class VrConnector implements SimulatorInterface { private static class PortMapping { public final int messageDataIndex; public final int robotPortIndex; @@ -149,8 +149,11 @@ private static class CANPortMapping { private static final int feedbackPort = 7662; private static final int BUF_SZ = 1024; + private Runnable resetHandler; + private long startTime; private boolean started = false; + private double prevStepSimTime = 0; private Selector selector; private InetSocketAddress sendAddr; @@ -226,7 +229,7 @@ private boolean process() throws IOException { } selector.selectedKeys().clear(); - selector.select(); + selector.select(3000); boolean newData = false; for (SelectionKey key : selector.selectedKeys()) { if (!key.isValid()) { @@ -353,8 +356,8 @@ private boolean process() throws IOException { return newData; } - public void run() { - double prevSimTime = 0; + @Override + public double prepareStep() { while (true) { boolean newData = false; try { @@ -370,11 +373,14 @@ public void run() { if (ProgramInterface.simulationTime == 0) { // Wait for a connection to the simulator before starting to run the robot code. startTime = System.currentTimeMillis(); + System.out.println("Waiting for 3d simulator to connect"); continue; } if (resetCounter != lastResetCounter) { lastResetCounter = resetCounter; - ProgramInterface.program.reset(); + if (resetHandler != null) { + resetHandler.run(); + } } if (!newData) { continue; @@ -390,13 +396,16 @@ public void run() { } else { continue; } - prevSimTime = ProgramInterface.simulationTime; - } - if (ProgramInterface.program != null) { - final double time = ProgramInterface.simulationTime; - ProgramInterface.program.step(time - prevSimTime); - prevSimTime = time; + prevStepSimTime = ProgramInterface.simulationTime; } + final double time = ProgramInterface.simulationTime; + prevStepSimTime = time; + return time - prevStepSimTime; } } + + @Override + public void setResetHandler(Runnable handler) { + resetHandler = handler; + } } diff --git a/src/main/java/com/team766/hal/wpilib/Joystick.java b/src/main/java/com/team766/hal/wpilib/Joystick.java index b0e57dad6..95deb5bd6 100755 --- a/src/main/java/com/team766/hal/wpilib/Joystick.java +++ b/src/main/java/com/team766/hal/wpilib/Joystick.java @@ -21,7 +21,7 @@ public double getAxis(final int axis) { @Override public boolean isAxisMoved(int axis) { - return java.lang.Math.abs(getAxis(axis)) + return Math.abs(getRawAxis(axis)) >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); } diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index b3ae9c349..4c149e59b 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -5,15 +5,23 @@ import com.team766.hal.CanivPoller; import com.team766.hal.GenericRobotMain; import com.team766.hal.RobotProvider; +import com.team766.hal.simulator.SimulationRobotProvider; +import com.team766.hal.simulator.SimulatorInterface; +import com.team766.hal.simulator.VrConnector; +import com.team766.logging.Category; import com.team766.logging.LoggerExceptionUtils; +import com.team766.logging.Severity; +import com.team766.simulator.ProgramInterface; +import com.team766.simulator.Simulator; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import java.io.File; -// import java.nio.file.Files; +import java.io.IOException; import java.nio.file.Path; import java.util.function.Supplier; import org.littletonrobotics.junction.LoggedRobot; @@ -26,6 +34,8 @@ public class RobotMain extends LoggedRobot { private static final String INTERNAL_CONFIG_FILE = "/home/lvuser/robotConfig.txt"; private GenericRobotMain robot; + private SimulatorInterface simulator; + private com.team766.simulator.AprilTagSimulator aprilTagSimulator; public static void main(final String... args) { Supplier supplier = @@ -86,36 +96,41 @@ private static String checkForAndReturnPathToConfigFile(final String file) { @Override public void robotInit() { try { - boolean configFromUSB = true; - String filename = checkForAndReturnPathToConfigFile(USB_CONFIG_FILE); - - if (filename == null) { - filename = INTERNAL_CONFIG_FILE; - configFromUSB = false; + if (isSimulation()) { + ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); + // TODO: Use WPILib's simulation interfaces and switch this to WPIRobotProvider + RobotProvider.instance = new SimulationRobotProvider(); + } else { + boolean configFromUSB = true; + String filename = checkForAndReturnPathToConfigFile(USB_CONFIG_FILE); + if (filename == null) { + filename = INTERNAL_CONFIG_FILE; + configFromUSB = false; + } + ConfigFileReader.instance = + new ConfigFileReader(filename, configFromUSB ? INTERNAL_CONFIG_FILE : null); + RobotProvider.instance = new WPIRobotProvider(); } - ConfigFileReader.instance = - new ConfigFileReader(filename, configFromUSB ? INTERNAL_CONFIG_FILE : null); - RobotProvider.instance = new WPIRobotProvider(); - robot = new GenericRobotMain(); + var configLogDir = com.team766.logging.Logger.getLogDirFromConfig(); + if (configLogDir.hasValue()) { + new File(configLogDir.get()).mkdirs(); + } + DataLogManager.start(configLogDir.valueOr("" /* use DataLogManager's default dir */)); + com.team766.logging.Logger.init(DataLogManager.getLogDir()); DriverStation.startDataLog(DataLogManager.getLog()); - if (isReal()) { - // enable dual-logging - com.team766.logging.Logger.enableLoggingToDataLog(true); + // enable dual-logging + com.team766.logging.Logger.enableLoggingToDataLog(true); - // set up AdvantageKit logging - DataLogManager.log("Initializing logging."); - Logger.addDataReceiver(new WPILOGWriter("/U/logs")); // Log to sdcard - if (!DriverStation.isFMSAttached()) { - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - } - new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging - - } else { - // TODO: add support for simulation logging/replay + // set up AdvantageKit logging + DataLogManager.log("Initializing logging."); + Logger.addDataReceiver(new WPILOGWriter(DataLogManager.getLogDir())); // Log to sdcard + if (!DriverStation.isFMSAttached()) { + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables } + new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDirty", BuildConstants.DIRTY != 0 ? "Yes" : "No"); @@ -123,6 +138,8 @@ public void robotInit() { Logger.start(); + robot = new GenericRobotMain(); + robot.robotInit(); } catch (Exception e) { e.printStackTrace(); @@ -150,6 +167,91 @@ public void autonomousInit() { } } + @Override + public void simulationInit() { + try { + enum SimulationMode { + None, + MaroonSim, + VrConnector, + } + final var simulationMode = + ConfigFileReader.getInstance().getEnum(SimulationMode.class, "simulationMode"); + switch (simulationMode.get()) { + case None -> { + simulator = null; + return; + } + case MaroonSim -> { + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw(Severity.INFO, "Running Maroon simulator"); + simulator = new Simulator(); + } + case VrConnector -> { + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw(Severity.INFO, "Running VR simulator"); + ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; + try { + simulator = new VrConnector(); + } catch (IOException ex) { + throw new RuntimeException( + "Error initializing communication with 3d Simulator", ex); + } + } + } + simulator.setResetHandler(() -> robot.resetAutonomousMode("simulation reset")); + // Do an initial step here to flush any needed state initialization. + simulator.prepareStep(); + + // Initialize AprilTag simulator + // Topic names must match what Vision.java expects + String[] cameraTopics = {"left_back", "left_front", "right_back", "right_front"}; + aprilTagSimulator = + new com.team766.simulator.AprilTagSimulator( + cameraTopics, // NetworkTables topic names (matches Vision.java) + 5.0, // Detection range in meters + 20.0 // Publish rate in Hz + ); + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw(Severity.INFO, "AprilTag simulator initialized"); + } catch (Exception exc) { + exc.printStackTrace(); + LoggerExceptionUtils.logException(exc); + } + } + + @Override + public void simulationPeriodic() { + if (simulator == null) { + return; + } + try { + final double dt = simulator.prepareStep(); + switch (ProgramInterface.robotMode) { + case AUTON -> { + DriverStationSim.setAutonomous(true); + DriverStationSim.setEnabled(true); + } + case DISABLED -> { + DriverStationSim.setEnabled(false); + } + case TELEOP -> { + DriverStationSim.setAutonomous(false); + DriverStationSim.setEnabled(true); + } + } + DriverStationSim.notifyNewData(); + + // Update AprilTag simulator with current simulation time + if (aprilTagSimulator != null) { + aprilTagSimulator.simulateAprilTags(ProgramInterface.simulationTime); + } + } catch (Exception exc) { + exc.printStackTrace(); + LoggerExceptionUtils.logException(exc); + } + } + @Override public void teleopInit() { try { diff --git a/src/main/java/com/team766/logging/Logger.java b/src/main/java/com/team766/logging/Logger.java index b0b52875e..d130fa0dc 100644 --- a/src/main/java/com/team766/logging/Logger.java +++ b/src/main/java/com/team766/logging/Logger.java @@ -3,6 +3,7 @@ import com.google.errorprone.annotations.FormatMethod; import com.team766.config.ConfigFileReader; import com.team766.library.CircularBuffer; +import com.team766.library.ValueProvider; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import java.io.File; @@ -54,30 +55,25 @@ public void uncaughtException(final Thread t, final Throwable e) { for (Category category : Category.values()) { m_loggers.put(category, new Logger(category)); } + + Thread.setDefaultUncaughtExceptionHandler(new LogUncaughtException()); + } + + public static ValueProvider getLogDirFromConfig() { + return ConfigFileReader.getInstance().getString(LOG_FILE_PATH_KEY); + } + + public static void init(String logDir) { try { - ConfigFileReader config_file = ConfigFileReader.getInstance(); - if (config_file != null && config_file.containsKey(LOG_FILE_PATH_KEY)) { - logFilePathBase = config_file.getString(LOG_FILE_PATH_KEY).get(); - new File(logFilePathBase).mkdirs(); - final String timestamp = - new SimpleDateFormat("yyyyMMdd'T'HHmmss").format(new Date()); - final String logFilePath = new File(logFilePathBase, timestamp).getAbsolutePath(); - m_logWriter = new LogWriter(logFilePath); - get(Category.CONFIGURATION).logRaw(Severity.INFO, "Logging to " + logFilePath); - } else { - get(Category.CONFIGURATION) - .logRaw( - Severity.ERROR, - "Config file does not specify " - + LOG_FILE_PATH_KEY - + ". Logs will only be in-memory and will be lost when the robot is turned off."); - } + logFilePathBase = logDir; + final String timestamp = new SimpleDateFormat("yyyyMMdd'T'HHmmss").format(new Date()); + final String logFilePath = new File(logFilePathBase, timestamp).getAbsolutePath(); + m_logWriter = new LogWriter(logFilePath); + get(Category.CONFIGURATION).logRaw(Severity.INFO, "Logging to " + logFilePath); } catch (Exception e) { e.printStackTrace(); LoggerExceptionUtils.logException(e); } - - Thread.setDefaultUncaughtExceptionHandler(new LogUncaughtException()); } public static void enableLoggingToDataLog(boolean enabled) { diff --git a/src/main/java/com/team766/simulator/AprilTagSimulator.java b/src/main/java/com/team766/simulator/AprilTagSimulator.java new file mode 100644 index 000000000..84b2171f9 --- /dev/null +++ b/src/main/java/com/team766/simulator/AprilTagSimulator.java @@ -0,0 +1,177 @@ +package com.team766.simulator; + +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Simulates an Orin coprocessor publishing AprilTag detections to NetworkTables. + * Simplified version - publishes same data to multiple camera topics without worrying + * about camera extrinsics or field of view. + * + * The Orin publishes AprilTag data as double arrays with the format: + * [timestamp, tagId, x, y, z, timestamp, tagId, x, y, z, ...] + * + * Usage: + * - Create simulator with camera topic names + * - Call simulateAprilTags() periodically during simulation + */ +public class AprilTagSimulator { + + private final NetworkTableInstance ntInstance; + private final NetworkTable table; + private final Map publishers = new HashMap<>(); + + // Configuration + private final double detectionRange; // meters + private final double publishRateHz; + + // State + private double lastPublishTime = 0.0; + + /** + * Represents an AprilTag on the field with a known position. + */ + public static class FieldAprilTag { + public final int id; + public final double x; // meters + public final double y; // meters + public final double z; // meters (height) + + public FieldAprilTag(int id, double x, double y, double z) { + this.id = id; + this.x = x; + this.y = y; + this.z = z; + } + } + + // Known AprilTag locations on the field + private final List fieldTags = new ArrayList<>(); + + /** + * Create AprilTag simulator. + * + * @param cameraTopics List of NetworkTables topic names (e.g., ["left_back", "right_front"]) + * @param detectionRangeMeters Maximum detection range in meters + * @param publishRateHz How often to publish detections + */ + public AprilTagSimulator( + String[] cameraTopics, double detectionRangeMeters, double publishRateHz) { + this.detectionRange = detectionRangeMeters; + this.publishRateHz = publishRateHz; + + ntInstance = NetworkTableInstance.getDefault(); + table = ntInstance.getTable("/SmartDashboard"); + + // Create publishers for each camera topic + for (String topic : cameraTopics) { + DoubleArrayPublisher publisher = table.getDoubleArrayTopic(topic).publish(); + publishers.put(topic, publisher); + System.out.printf("[AprilTagSim] Publishing to topic: %s%n", topic); + } + + // Initialize with some example tags (update these for your field) + initializeFieldTags(); + } + + /** + * Configure the known AprilTag positions on the field. + * You should update this based on your actual field layout. + */ + private void initializeFieldTags() { + // Example: Add some test tags + // Format: (tagId, x, y, z) + fieldTags.add(new FieldAprilTag(1, 0.0, 0.0, 0.5)); // Tag 1 at origin + fieldTags.add(new FieldAprilTag(2, 5.0, 0.0, 0.5)); // Tag 2 at 5m + fieldTags.add(new FieldAprilTag(3, 5.0, 5.0, 0.5)); // Tag 3 at corner + fieldTags.add(new FieldAprilTag(6, 0.0, 5.0, 0.5)); // Tag 6 at corner + // Add more tags based on your field layout + } + + /** + * Add a field tag dynamically (useful for testing). + */ + public void addFieldTag(int id, double x, double y, double z) { + fieldTags.add(new FieldAprilTag(id, x, y, z)); + } + + /** + * Simulates AprilTag detections based on robot position. + * Publishes the same data to all camera topics. + * Call this periodically during simulation (e.g., every 20ms in simulationPeriodic). + * + * @param currentTime Current simulation time in seconds + */ + public void simulateAprilTags(double currentTime) { + // Rate limiting - only publish at the configured rate + double timeSinceLastPublish = currentTime - lastPublishTime; + if (timeSinceLastPublish < (1.0 / publishRateHz)) { + return; + } + lastPublishTime = currentTime; + + // Get robot position from simulation + double robotX = ProgramInterface.robotPosition.x; + double robotY = ProgramInterface.robotPosition.y; + double robotHeading = ProgramInterface.robotPosition.heading; // degrees + + // Find all tags within detection range + List detectionData = new ArrayList<>(); + + for (FieldAprilTag tag : fieldTags) { + double dx = tag.x - robotX; + double dy = tag.y - robotY; + double distance = Math.sqrt(dx * dx + dy * dy); + + // Only detect tags within range + if (distance <= detectionRange) { + // Convert tag position from field frame to robot frame + // This simulates what the camera would see + double angleToTag = Math.atan2(dy, dx) - Math.toRadians(robotHeading); + double relativeX = distance * Math.cos(angleToTag); + double relativeY = distance * Math.sin(angleToTag); + double relativeZ = tag.z; // Simplified - doesn't account for robot tilt + + // Add to detection array: [timestamp, tagId, x, y, z] + detectionData.add(currentTime); + detectionData.add((double) tag.id); + detectionData.add(relativeX); + detectionData.add(relativeY); + detectionData.add(relativeZ); + } + } + + // Publish to all camera topics + if (!detectionData.isEmpty()) { + double[] array = detectionData.stream().mapToDouble(Double::doubleValue).toArray(); + for (Map.Entry entry : publishers.entrySet()) { + entry.getValue().set(array); + } + + // Debug output + System.out.printf( + "[AprilTagSim] Published %d tags at t=%.2f (robot: %.2f, %.2f, %.1f°)%n", + detectionData.size() / 5, currentTime, robotX, robotY, robotHeading); + } else { + // Publish empty array when no tags detected + double[] emptyArray = new double[0]; + for (DoubleArrayPublisher publisher : publishers.values()) { + publisher.set(emptyArray); + } + } + } + + /** + * Stop publishing and clean up resources. + */ + public void close() { + for (DoubleArrayPublisher publisher : publishers.values()) { + publisher.close(); + } + } +} diff --git a/src/main/java/com/team766/simulator/Parameters.java b/src/main/java/com/team766/simulator/Parameters.java index 6d3814cd9..e2d49cc1b 100644 --- a/src/main/java/com/team766/simulator/Parameters.java +++ b/src/main/java/com/team766/simulator/Parameters.java @@ -2,9 +2,9 @@ public class Parameters { public static final double TIME_STEP = 0.0001; // seconds - public static final double DURATION = 10.0; // seconds public static final double LOGGING_PERIOD = 0.005; // seconds + public static final double DISPLAY_PERIOD = 10.0; // seconds // Robot mode to run in the simulator public static final ProgramInterface.RobotMode INITIAL_ROBOT_MODE = diff --git a/src/main/java/com/team766/simulator/Program.java b/src/main/java/com/team766/simulator/Program.java deleted file mode 100644 index c6753fdef..000000000 --- a/src/main/java/com/team766/simulator/Program.java +++ /dev/null @@ -1,7 +0,0 @@ -package com.team766.simulator; - -public interface Program { - public void step(double dt); - - void reset(); -} diff --git a/src/main/java/com/team766/simulator/ProgramInterface.java b/src/main/java/com/team766/simulator/ProgramInterface.java index 15b293e32..6091e094f 100644 --- a/src/main/java/com/team766/simulator/ProgramInterface.java +++ b/src/main/java/com/team766/simulator/ProgramInterface.java @@ -5,8 +5,6 @@ import java.lang.reflect.Array; public class ProgramInterface { - public static Program program = null; - public static double simulationTime; public static int driverStationUpdateNumber = 0; diff --git a/src/main/java/com/team766/simulator/Simulator.java b/src/main/java/com/team766/simulator/Simulator.java index 05ab17bd1..cf46edb23 100644 --- a/src/main/java/com/team766/simulator/Simulator.java +++ b/src/main/java/com/team766/simulator/Simulator.java @@ -1,5 +1,6 @@ package com.team766.simulator; +import com.team766.hal.simulator.SimulatorInterface; import com.team766.simulator.elements.*; import com.team766.simulator.mechanisms.*; import com.team766.simulator.ui.*; @@ -8,15 +9,18 @@ import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; -public class Simulator implements Runnable { +public class Simulator implements SimulatorInterface { private ElectricalSystem electricalSystem = new ElectricalSystem(); private PneumaticsSystem pneumaticsSystem = new PneumaticsSystem(); private WestCoastDrive drive = new WestCoastDrive(electricalSystem); private AirCompressor compressor = new AirCompressor(); private DoubleJointedArm arm = new DoubleJointedArm(electricalSystem); + private Runnable resetHandler; + private double time; private double nextLogTime; + private double nextDisplayTime = Parameters.DISPLAY_PERIOD; private final Metrics metrics = new Metrics(); private final Metrics.Series xPositionSeries = metrics.addSeries("X Position (m)", false); @@ -54,7 +58,8 @@ public Simulator() { } } - public void step() { + @Override + public double prepareStep() { double dt = Parameters.TIME_STEP; time += dt; ProgramInterface.simulationTime = time; @@ -64,8 +69,8 @@ public void step() { drive.step(dt); arm.step(dt); - if (ProgramInterface.program != null) { - ProgramInterface.program.step(dt); + if (Math.abs(time - 3.0) < Parameters.TIME_STEP) { + pneumaticsSystem.ventPressure(); } if (nextLogTime <= time) { @@ -116,19 +121,32 @@ public void step() { arm.getJ2Position().get(1, 0) }); } + + if (nextDisplayTime <= time) { + nextDisplayTime += Parameters.DISPLAY_PERIOD; + + displayResultsUi(); + } + + return dt; } - public void run() { + public void reset() { metrics.clear(); time = 0.0; nextLogTime = 0.0; - while (time <= Parameters.DURATION) { - step(); - if (Math.abs(time - 3.0) < Parameters.TIME_STEP) { - pneumaticsSystem.ventPressure(); - } + nextDisplayTime = Parameters.DISPLAY_PERIOD; + if (resetHandler != null) { + resetHandler.run(); } + } + + @Override + public void setResetHandler(Runnable handler) { + resetHandler = handler; + } + void displayResultsUi() { var playbackTimer = new PlaybackTimer(time); // var trajectoryPanel = new Trajectory(driveTrajectory, playbackTimer);