diff --git a/.gitignore b/.gitignore index d012c1d3..38a40cca 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,10 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. +### Python ### +__pycache__/ +*.pyc + ### C++ ### # Prerequisites *.d diff --git a/auto_generator/build_autos.py b/auto_generator/build_autos.py index c37a0b30..5d8554e8 100644 --- a/auto_generator/build_autos.py +++ b/auto_generator/build_autos.py @@ -2,51 +2,45 @@ """ build_autos.py -Discovers and imports all auto-definition modules in src/, runs them to -register autos with auto_lib, then writes a single Autos.json and/or -publishes to the robot's tuning server. +Orchestrates auto generation and publishing. The autos themselves are authored +in Java (see src/main/java/frc/robot/autogen) and serialized to Autos.json by a fast Java +generator that reuses the robot's own coppercore Gson configuration. This script +invokes that generator, then writes the local Autos.json and/or publishes to the +robot's tuning server (verifying a structural round-trip on PUT). -Helper modules (auto_lib, shorthands, constants, field_locations, auto_action, units) -are excluded from auto-discovery. - -Usage (from auto_generator/): +Usage (from the repo root or auto_generator/): python build_autos.py # write Autos.json to deploy dir python build_autos.py --sim # also publish to sim (localhost:8088) python build_autos.py --robot # also publish to robot (10.4.1.2:8088) python build_autos.py --url HOST # also publish to a custom address python build_autos.py --no-file # skip writing the local file + python build_autos.py --bootstrap # recompile robot classes + classpath first """ from __future__ import annotations import argparse -import importlib +import importlib.util import json +import subprocess import sys import urllib.request import urllib.error from pathlib import Path from typing import Any -# Ensure the auto_generator directory is on the path so `src` is importable _SCRIPT_DIR = Path(__file__).resolve().parent -sys.path.insert(0, str(_SCRIPT_DIR)) - -from src import auto_lib - -# Helper modules that should not be treated as auto files -EXCLUDED_MODULES = { - "__init__", - "auto_lib", - "auto_action", - "units", - "shorthands", - "constants", - "field_locations", - "routines", -} - -OUTPUT_DIR = _SCRIPT_DIR.parent / "src" / "main" / "deploy" / "constants" +_REPO_ROOT = _SCRIPT_DIR.parent + +# The autos are authored in Java (src/main/java/frc/robot/autogen) and serialized to +# Autos.json by the cross-platform generator driver, which reuses the robot's own +# coppercore Gson configuration. Load it as a module (it sits next to this file). +_GENERATOR_PATH = _SCRIPT_DIR / "generate.py" +_spec = importlib.util.spec_from_file_location("autogen_generate", _GENERATOR_PATH) +_autogen = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(_autogen) + +OUTPUT_DIR = _REPO_ROOT / "src" / "main" / "deploy" / "constants" CONFIG_FILE = OUTPUT_DIR / "config.json" SIM_URL = "http://localhost:8088" @@ -70,14 +64,16 @@ def detect_environment() -> str: return "comp" -def discover_auto_modules() -> list[str]: - """Find all Python modules in src/ that are auto definitions (not helpers).""" - src_dir = _SCRIPT_DIR / "src" - modules = [] - for f in sorted(src_dir.iterdir()): - if f.suffix == ".py" and f.stem not in EXCLUDED_MODULES: - modules.append(f"src.{f.stem}") - return modules +def generate_autos_json(env: str, bootstrap: bool = False) -> str: + """Build the autos via the Java generator and return the Autos.json content for `env`. + + Generator progress and native (HAL) diagnostics stream to stderr; the JSON is + returned in-memory. + """ + try: + return _autogen.generate(env, bootstrap) + except subprocess.CalledProcessError as e: + raise SystemExit(f"Java auto generator failed with status {e.returncode}") def _is_json_number(value: Any) -> bool: @@ -173,10 +169,10 @@ def publish(base_url: str, env: str, content: str) -> None: req = urllib.request.Request(url, method="POST") try: with urllib.request.urlopen(req, timeout=5) as resp: - body = resp.read().decode("utf-8", errors="replace") + # Drain the response but don't echo it; on success the body is just the + # full autos JSON, which is only noise. Failures are reported below. + resp.read() print(f" POST {url} -> {resp.status}") - if body: - print(f" Response: {body}") except urllib.error.HTTPError as e: body = e.read().decode("utf-8", errors="replace") if e.fp else "" message = f"POST {url} failed: HTTP {e.code} {e.reason}" @@ -206,6 +202,12 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--env", type=str, default=None, help="Override environment (default: auto-detect from config.json)" ) + parser.add_argument( + "--bootstrap", + action="store_true", + help="Force the Java generator to recompile robot classes and re-dump the classpath " + "(needed after changing robot/builder code or dependencies)", + ) return parser.parse_args() @@ -213,51 +215,18 @@ def main() -> None: args = parse_args() env = args.env if args.env else detect_environment() - # Step 1: Import routines first so @routine decorators register before - # any auto modules try to reference them via routines.() - print(" Importing src.routines...") - importlib.import_module("src.routines") - - # Step 2: Discover and import auto modules - auto_modules = discover_auto_modules() - - if not auto_modules: - print("No auto modules found in src/") - return - - print(f"Found {len(auto_modules)} auto module(s): {[m.split('.')[-1] for m in auto_modules]}") - - for module_name in auto_modules: - print(f" Importing {module_name}...") - importlib.import_module(module_name) - - # Step 3: Serialize - autos = auto_lib.get_autos() - routine_map = auto_lib.get_routines() - - if not autos and not routine_map: - print("Warning: No autos or routines were registered.") - return - - autos_obj = {} - for name, action in autos.items(): - autos_obj[name] = auto_lib._serialize_value(action) - - routines_obj = {} - for name, action in routine_map.items(): - routines_obj[name] = auto_lib._serialize_value(action) - - output = {"autos": autos_obj, "routines": routines_obj} - content = json.dumps(output, indent=4) + # Generate Autos.json by running the Java auto generator (reuses the robot's + # own coppercore Gson config, so the output round-trips by construction). + content = generate_autos_json(env, bootstrap=args.bootstrap) - # Step 4: Write local file (unless --no-file) + # Write local file (unless --no-file) if not args.no_file: output_file = OUTPUT_DIR / env / "Autos.json" output_file.parent.mkdir(parents=True, exist_ok=True) output_file.write_text(content, encoding="utf-8") - print(f"Written {len(autos)} auto(s) and {len(routine_map)} routine(s) to: {output_file}") + print(f"Written autos to: {output_file}") - # Step 5: Publish to tuning server(s) + # Publish to tuning server(s) targets: list[str] = [] if args.sim: targets.append(SIM_URL) diff --git a/auto_generator/dump-classpath.gradle b/auto_generator/dump-classpath.gradle new file mode 100644 index 00000000..24c54f1a --- /dev/null +++ b/auto_generator/dump-classpath.gradle @@ -0,0 +1,15 @@ +// Init script: dump the main runtime classpath so the Java auto generator can +// compile and run without paying gradle's per-invocation configuration cost. +// Used by generate.sh during the one-time (or after-dependency-change) bootstrap. +gradle.projectsEvaluated { + rootProject.tasks.register("dumpAutoGenClasspath") { + doLast { + def cp = rootProject.sourceSets.main.runtimeClasspath.files + .collect { it.absolutePath } + .join(File.pathSeparator) + def out = new File(rootProject.projectDir, "build/autogen-classpath.txt") + out.text = cp + println "wrote ${out} (${cp.split(File.pathSeparator).length} entries)" + } + } +} diff --git a/auto_generator/generate.py b/auto_generator/generate.py new file mode 100755 index 00000000..06d9a141 --- /dev/null +++ b/auto_generator/generate.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +""" +Cross-platform driver for the Java auto generator. + +Compiles the auto-definition sources (src/main/java/frc/robot/autogen) against the +already-compiled robot classes and runs them as a (headless) robot JVM with the WPILib +native libraries on the path, so the auto code has full, unrestricted access to +the rest of the robot code (HAL, Filesystem, NetworkTables), exactly as on the +robot. Emits Autos.json to stdout. + +This deliberately bypasses gradle on the hot path. A warm gradle daemon still +spends ~4s configuring an up-to-date compileJava task on this project; a bare +javac + java cycle is well under that. Gradle is only invoked for the one-time +bootstrap (compile robot classes, extract native libs, dump the classpath), or +when --bootstrap is passed (e.g. after editing robot/builder code or changing +dependencies). + +Usage: + python generate.py [ENVIRONMENT] # print Autos.json (default env: comp) + python generate.py --bootstrap [ENVIRONMENT] # force recompile + native extraction + dump + +All diagnostics (and native HAL stdout chatter) go to stderr so stdout is clean JSON. +""" +from __future__ import annotations + +import os +import platform +import subprocess +import sys +from pathlib import Path + +_THIS_DIR = Path(__file__).resolve().parent # auto_generator/ +_ROOT = _THIS_DIR.parent + +CP_FILE = _ROOT / "build" / "autogen-classpath.txt" +CLASSES = _ROOT / "build" / "classes" / "java" / "main" +NATIVE_DIR = _ROOT / "build" / "jni" / "release" +# Only the autogen package is fast-compiled on the hot path (the rest of the robot +# is already compiled into CLASSES by the bootstrap's gradle compileJava). +GEN_SRC = _ROOT / "src" / "main" / "java" / "frc" / "robot" / "autogen" +GEN_OUT = _ROOT / "build" / "autogen-out" +OUT_JSON = GEN_OUT / "Autos.json" +MARKER = GEN_OUT / ".compiled" +INIT_SCRIPT = _THIS_DIR / "dump-classpath.gradle" +MAIN_CLASS = "frc.robot.autogen.GenerateAutos" + +_IS_WINDOWS = os.name == "nt" +JVM_FAST = ["-XX:+UseSerialGC", "-XX:TieredStopAtLevel=1", "-XX:-UsePerfData"] + + +def _log(message: str) -> None: + print(f"[generate] {message}", file=sys.stderr, flush=True) + + +def _gradlew() -> str: + return str(_ROOT / ("gradlew.bat" if _IS_WINDOWS else "gradlew")) + + +def _jdk_tool(name: str) -> str: + """Resolve a JDK tool (java/javac), preferring JAVA_HOME (set by the WPILib env).""" + java_home = os.environ.get("JAVA_HOME") + if java_home: + exe = name + (".exe" if _IS_WINDOWS else "") + candidate = Path(java_home) / "bin" / exe + if candidate.exists(): + return str(candidate) + return name # fall back to PATH + + +def _java_sources() -> list[str]: + return [str(p) for p in sorted(GEN_SRC.rglob("*.java"))] + + +def _sources_changed() -> bool: + if not MARKER.exists(): + return True + marker_mtime = MARKER.stat().st_mtime + return any(Path(src).stat().st_mtime > marker_mtime for src in _java_sources()) + + +def _bootstrap() -> None: + _log("bootstrapping: gradle compileJava + native extraction + classpath dump...") + subprocess.run( + [ + _gradlew(), + "--init-script", + str(INIT_SCRIPT), + "compileJava", + "extractReleaseNative", + "dumpAutoGenClasspath", + "-q", + ], + cwd=str(_ROOT), + check=True, + stdout=sys.stderr, # keep gradle chatter out of stdout (which carries the JSON) + ) + + +def _native_loader_env() -> dict[str, str]: + """Environment with the native lib dir prepended to the OS's dynamic-loader path.""" + env = dict(os.environ) + var = { + "Windows": "PATH", + "Darwin": "DYLD_LIBRARY_PATH", + }.get(platform.system(), "LD_LIBRARY_PATH") + existing = env.get(var, "") + env[var] = str(NATIVE_DIR) + (os.pathsep + existing if existing else "") + return env + + +def generate(env: str = "comp", bootstrap: bool = False) -> str: + """Build the autos and return the Autos.json content for `env`.""" + if bootstrap or not CP_FILE.exists() or not CLASSES.is_dir() or not NATIVE_DIR.is_dir(): + _bootstrap() + + classpath = CP_FILE.read_text(encoding="utf-8").strip() + os.pathsep + str(CLASSES) + GEN_OUT.mkdir(parents=True, exist_ok=True) + + if _sources_changed(): + _log("compiling generator sources...") + subprocess.run( + [ + _jdk_tool("javac"), + "-J-XX:+UseSerialGC", + "-J-XX:TieredStopAtLevel=1", + "-J-XX:-UsePerfData", + "-cp", + classpath, + "-d", + str(GEN_OUT), + *_java_sources(), + ], + cwd=str(_ROOT), + check=True, + ) + MARKER.touch() + else: + _log("generator sources unchanged; skipping compile") + + # The generator selects the environment from the deploy constants/config.json (the same way + # the robot does); `env` here is only what the caller intends to write to, for logging. + _log(f"running generator (requested env '{env}'; environment comes from config.json)...") + # GEN_OUT goes first so the freshly fast-compiled autogen classes take precedence + # over the (possibly stale) copies the gradle bootstrap compiled into CLASSES. + run_classpath = str(GEN_OUT) + os.pathsep + classpath + # The generator writes JSON to OUT_JSON. Initializing HAL prints native chatter to + # the JVM's stdout, so we route that to our stderr to keep it out of the JSON. + subprocess.run( + [ + _jdk_tool("java"), + *JVM_FAST, + f"-Djava.library.path={NATIVE_DIR}", + "-cp", + run_classpath, + MAIN_CLASS, + str(OUT_JSON), + ], + cwd=str(_ROOT), + check=True, + stdout=sys.stderr, + env=_native_loader_env(), + ) + return OUT_JSON.read_text(encoding="utf-8") + + +def main(argv: list[str]) -> int: + bootstrap = False + args = list(argv) + if args and args[0] == "--bootstrap": + bootstrap = True + args = args[1:] + env = args[0] if args else "comp" + sys.stdout.write(generate(env, bootstrap)) + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main(sys.argv[1:])) + except subprocess.CalledProcessError as exc: + print(f"[generate] command failed ({exc.returncode}): {exc.cmd}", file=sys.stderr) + raise SystemExit(exc.returncode) diff --git a/auto_generator/src/__init__.py b/auto_generator/src/__init__.py deleted file mode 100644 index 440e8854..00000000 --- a/auto_generator/src/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -""" -Python package for the auto generator source modules. -""" diff --git a/auto_generator/src/auto_action.py b/auto_generator/src/auto_action.py deleted file mode 100644 index 7e250938..00000000 --- a/auto_generator/src/auto_action.py +++ /dev/null @@ -1,751 +0,0 @@ -""" -Auto-generated by PythonGenerator. Do not edit manually. -""" - -from __future__ import annotations - -import json -import math as _math -from dataclasses import dataclass, field -from typing import Any, Callable, List, Optional - - -@dataclass -class AutoReference: - type: str = field(default="AutoReference", init=False) - name: str = "" - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["name"] = _to_dict(self.name) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class Deadline: - type: str = field(default="Deadline", init=False) - deadline: AutoAction = None - others: List[AutoAction] = field(default_factory=list) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["deadline"] = _to_dict(self.deadline) - d["others"] = _to_dict(self.others) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class Sequence: - type: str = field(default="Sequence", init=False) - actions: List[AutoAction] = field(default_factory=list) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["actions"] = _to_dict(self.actions) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class Parallel: - type: str = field(default="Parallel", init=False) - actions: List[AutoAction] = field(default_factory=list) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["actions"] = _to_dict(self.actions) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class Race: - type: str = field(default="Race", init=False) - actions: List[AutoAction] = field(default_factory=list) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["actions"] = _to_dict(self.actions) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - -from . import units - - -@dataclass -class Wait: - type: str = field(default="Wait", init=False) - delay: units.Measure = None - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["delay"] = _to_dict(self.delay) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class Print: - type: str = field(default="Print", init=False) - message: str = "" - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["message"] = _to_dict(self.message) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class NetworkConfigurableWait: - type: str = field(default="NetworkConfigurableWait", init=False) - name: str = "" - default_delay: units.Measure = None - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["name"] = _to_dict(self.name) - d["defaultDelay"] = _to_dict(self.default_delay) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class Rotation2d: - degrees: float = 0.0 - - def to_dict(self) -> dict: - d: dict = {} - d["degrees"] = _to_dict(self.degrees) - return d - - @property - def radians(self) -> float: - return _math.radians(self.degrees) - - @property - def cos(self) -> float: - return _math.cos(self.radians) - - @property - def sin(self) -> float: - return _math.sin(self.radians) - - @staticmethod - def from_radians(radians: float) -> Rotation2d: - return Rotation2d(degrees=_math.degrees(radians)) - - def plus(self, other: Rotation2d) -> Rotation2d: - return Rotation2d(degrees=self.degrees + other.degrees) - - def minus(self, other: Rotation2d) -> Rotation2d: - return Rotation2d(degrees=self.degrees - other.degrees) - - def unary_minus(self) -> Rotation2d: - return Rotation2d(degrees=-self.degrees) - - def rotate_by(self, other: Rotation2d) -> Rotation2d: - return self.plus(other) - - def __neg__(self) -> Rotation2d: - return self.unary_minus() - - def __add__(self, other: Rotation2d) -> Rotation2d: - return self.plus(other) - - def __sub__(self, other: Rotation2d) -> Rotation2d: - return self.minus(other) - - -@dataclass -class Translation2d: - x: float = 0.0 - y: float = 0.0 - - def to_dict(self) -> dict: - d: dict = {} - d["x"] = _to_dict(self.x) - d["y"] = _to_dict(self.y) - return d - - @property - def norm(self) -> float: - return _math.hypot(self.x, self.y) - - def plus(self, other: Translation2d) -> Translation2d: - return Translation2d(x=self.x + other.x, y=self.y + other.y) - - def minus(self, other: Translation2d) -> Translation2d: - return Translation2d(x=self.x - other.x, y=self.y - other.y) - - def unary_minus(self) -> Translation2d: - return Translation2d(x=-self.x, y=-self.y) - - def times(self, scalar: float) -> Translation2d: - return Translation2d(x=self.x * scalar, y=self.y * scalar) - - def div(self, scalar: float) -> Translation2d: - return Translation2d(x=self.x / scalar, y=self.y / scalar) - - def rotate_by(self, rotation: Rotation2d) -> Translation2d: - c = rotation.cos - s = rotation.sin - return Translation2d(x=self.x * c - self.y * s, y=self.x * s + self.y * c) - - def distance(self, other: Translation2d) -> float: - return self.minus(other).norm - - def __neg__(self) -> Translation2d: - return self.unary_minus() - - def __add__(self, other: Translation2d) -> Translation2d: - return self.plus(other) - - def __sub__(self, other: Translation2d) -> Translation2d: - return self.minus(other) - - def __mul__(self, scalar: float) -> Translation2d: - return self.times(scalar) - - def __truediv__(self, scalar: float) -> Translation2d: - return self.div(scalar) - - def to_pose2d(self, rotation: Optional[Rotation2d] = None) -> Pose2d: - """Convert to a Pose2d, optionally with a rotation (defaults to 0 deg).""" - return Pose2d( - translation=Translation2d(x=self.x, y=self.y), - rotation=rotation if rotation is not None else Rotation2d(), - ) - - -@dataclass -class Pose2d: - rotation: Rotation2d = field(default_factory=Rotation2d) - translation: Translation2d = field(default_factory=Translation2d) - - def to_dict(self) -> dict: - d: dict = {} - d["rotation"] = _to_dict(self.rotation) - d["translation"] = _to_dict(self.translation) - return d - - def plus(self, other: Transform2d) -> Pose2d: - """Apply a transform (equivalent to transform_by).""" - return self.transform_by(other) - - def transform_by(self, transform: Transform2d) -> Pose2d: - """Apply a Transform2d to this pose (WPILib transformBy).""" - t_trans = transform.translation if transform.translation else Translation2d() - t_rot = transform.rotation if transform.rotation else Rotation2d() - new_translation = self.translation.plus(t_trans.rotate_by(self.rotation)) - new_rotation = self.rotation.plus(t_rot) - return Pose2d(translation=new_translation, rotation=new_rotation) - - def translate_by(self, translation: Translation2d) -> Pose2d: - """Translate this pose by a Translation2d (no rotation change).""" - return Pose2d( - translation=self.translation.plus(translation), - rotation=Rotation2d(degrees=self.rotation.degrees), - ) - - def rotate_by(self, rotation: Rotation2d) -> Pose2d: - """Rotate this pose by a Rotation2d.""" - return Pose2d( - translation=self.translation.rotate_by(rotation), - rotation=self.rotation.plus(rotation), - ) - - def rotate_around(self, point: Translation2d, rotation: Rotation2d) -> Pose2d: - """Rotate this pose around a given point.""" - new_translation = self.translation.minus(point).rotate_by(rotation).plus(point) - new_rotation = self.rotation.plus(rotation) - return Pose2d(translation=new_translation, rotation=new_rotation) - - def relative_to(self, other: Pose2d) -> Pose2d: - """Express this pose relative to other's coordinate frame.""" - inv_rot = other.rotation.unary_minus() - delta = self.translation.minus(other.translation).rotate_by(inv_rot) - new_rot = self.rotation.minus(other.rotation) - return Pose2d(translation=delta, rotation=new_rot) - - def inverse(self) -> Pose2d: - inv_rot = self.rotation.unary_minus() - inv_trans = self.translation.unary_minus().rotate_by(inv_rot) - return Pose2d(translation=inv_trans, rotation=inv_rot) - - def to_pose3d(self, z: float = 0.0) -> Pose3d: - return Pose3d( - translation=Translation3d(x=self.translation.x, y=self.translation.y, z=z), - rotation=Rotation3d(yaw=self.rotation.degrees), - ) - - def to_translation2d(self) -> Translation2d: - return Translation2d(x=self.translation.x, y=self.translation.y) - - -@dataclass -class APTarget: - reference: Pose2d = field(default_factory=Pose2d) - entry_angle: Optional[Rotation2d] = None - velocity: float = 0.0 - rotation_radius: Optional[units.Measure] = None - - def to_dict(self) -> dict: - d: dict = {} - d["reference"] = _to_dict(self.reference) - if self.entry_angle is not None: - d["entryAngle"] = _to_dict(self.entry_angle) - d["velocity"] = _to_dict(self.velocity) - if self.rotation_radius is not None: - d["rotationRadius"] = _to_dict(self.rotation_radius) - return d - - -@dataclass -class APConstraints: - velocity: float = 0.0 - acceleration: float = 0.0 - jerk: float = 0.0 - - def to_dict(self) -> dict: - d: dict = {} - d["velocity"] = _to_dict(self.velocity) - d["acceleration"] = _to_dict(self.acceleration) - d["jerk"] = _to_dict(self.jerk) - return d - - -@dataclass -class APProfile: - constraints: APConstraints = field(default_factory=APConstraints) - error_x_y: units.Measure = None - error_theta: units.Measure = None - beeline_radius: units.Measure = None - - def to_dict(self) -> dict: - d: dict = {} - d["constraints"] = _to_dict(self.constraints) - d["errorXY"] = _to_dict(self.error_x_y) - d["errorTheta"] = _to_dict(self.error_theta) - d["beelineRadius"] = _to_dict(self.beeline_radius) - return d - - -@dataclass -class PIDGains: - k_p: float = 0.0 - k_i: float = 0.0 - k_d: float = 0.0 - k_s: float = 0.0 - k_g: float = 0.0 - k_v: float = 0.0 - k_a: float = 0.0 - - def to_dict(self) -> dict: - d: dict = {} - d["kP"] = _to_dict(self.k_p) - d["kI"] = _to_dict(self.k_i) - d["kD"] = _to_dict(self.k_d) - d["kS"] = _to_dict(self.k_s) - d["kG"] = _to_dict(self.k_g) - d["kV"] = _to_dict(self.k_v) - d["kA"] = _to_dict(self.k_a) - return d - - -@dataclass -class AutoPilotAction: - type: str = field(default="AutoPilotAction", init=False) - target: APTarget = field(default_factory=APTarget) - profile: Optional[APProfile] = None - constraints: Optional[APConstraints] = None - pid_gains: Optional[PIDGains] = None - can_mirror: bool = True - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["target"] = _to_dict(self.target) - if self.profile is not None: - d["profile"] = _to_dict(self.profile) - if self.constraints is not None: - d["constraints"] = _to_dict(self.constraints) - if self.pid_gains is not None: - d["pidGains"] = _to_dict(self.pid_gains) - d["canMirror"] = _to_dict(self.can_mirror) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class XBasedAutoPilotAction: - type: str = field(default="XBasedAutoPilotAction", init=False) - target: APTarget = field(default_factory=APTarget) - profile: Optional[APProfile] = None - constraints: Optional[APConstraints] = None - pid_gains: Optional[PIDGains] = None - can_mirror: bool = True - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["target"] = _to_dict(self.target) - if self.profile is not None: - d["profile"] = _to_dict(self.profile) - if self.constraints is not None: - d["constraints"] = _to_dict(self.constraints) - if self.pid_gains is not None: - d["pidGains"] = _to_dict(self.pid_gains) - d["canMirror"] = _to_dict(self.can_mirror) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class StopDriveAction: - type: str = field(default="StopDriveAction", init=False) - can_mirror: bool = True - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["canMirror"] = _to_dict(self.can_mirror) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class FollowPathPlannerPath: - type: str = field(default="FollowPathPlannerPath", init=False) - path_name: str = "" - mirror_path: bool = False - can_mirror: bool = True - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - d["pathName"] = _to_dict(self.path_name) - d["mirrorPath"] = _to_dict(self.mirror_path) - d["canMirror"] = _to_dict(self.can_mirror) - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class DeployIntakeAction: - type: str = field(default="DeployIntakeAction", init=False) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class StowIntakeAction: - type: str = field(default="StowIntakeAction", init=False) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class ClimbSearchAction: - type: str = field(default="ClimbSearchAction", init=False) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class ClimbHangAction: - type: str = field(default="ClimbHangAction", init=False) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class StartShooting: - type: str = field(default="StartShooting", init=False) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -@dataclass -class StopShooting: - type: str = field(default="StopShooting", init=False) - - def to_dict(self) -> dict: - d: dict = {"type": self.type} - return d - - def add(self): - """Adds this command to the current auto and returns itself for chaining.""" - _call_hook(self) - return self - - -AutoAction = Optional[AutoReference | Deadline | Sequence | Parallel | Race | Wait | Print | NetworkConfigurableWait | AutoPilotAction | XBasedAutoPilotAction | StopDriveAction | FollowPathPlannerPath | DeployIntakeAction | StowIntakeAction | ClimbSearchAction | ClimbHangAction | StartShooting | StopShooting] - - -_add_command_hook: Callable[[Any], None] | None = None - - -def set_add_command_hook(hook: Callable[[Any], None]) -> None: - global _add_command_hook - _add_command_hook = hook - - -def _call_hook(obj: Any) -> None: - if _add_command_hook is not None: - _add_command_hook(obj) - - -def _to_dict(obj: Any) -> Any: - """Recursively convert an object to a JSON-serializable dict.""" - if obj is None: - return None - if hasattr(obj, 'to_dict'): - return obj.to_dict() - if isinstance(obj, list): - return [_to_dict(item) for item in obj] - if isinstance(obj, dict): - return {k: _to_dict(v) for k, v in obj.items()} - return obj - - -@dataclass -class Auto: - root_action: AutoAction = None - can_be_mirrored: Optional[bool] = None - should_be_flipped: Optional[bool] = None - - def to_dict(self) -> dict: - d: dict = {} - d["rootAction"] = _to_dict(self.root_action) - if self.can_be_mirrored is not None: - d["canBeMirrored"] = _to_dict(self.can_be_mirrored) - if self.should_be_flipped is not None: - d["shouldBeFlipped"] = _to_dict(self.should_be_flipped) - return d - - -@dataclass -class Transform2d: - rotation: Rotation2d = field(default_factory=Rotation2d) - translation: Translation2d = field(default_factory=Translation2d) - - def to_dict(self) -> dict: - d: dict = {} - d["rotation"] = _to_dict(self.rotation) - d["translation"] = _to_dict(self.translation) - return d - - def plus(self, other: Transform2d) -> Transform2d: - """Compose two transforms.""" - return Transform2d( - translation=self.translation.plus(other.translation.rotate_by(self.rotation)), - rotation=self.rotation.plus(other.rotation), - ) - - def inverse(self) -> Transform2d: - inv_rot = self.rotation.unary_minus() - inv_trans = self.translation.unary_minus().rotate_by(inv_rot) - return Transform2d(translation=inv_trans, rotation=inv_rot) - - -@dataclass -class Rotation3d: - roll: float = 0.0 - pitch: float = 0.0 - yaw: float = 0.0 - - def to_dict(self) -> dict: - d: dict = {} - d["roll"] = _to_dict(self.roll) - d["pitch"] = _to_dict(self.pitch) - d["yaw"] = _to_dict(self.yaw) - return d - - def plus(self, other: Rotation3d) -> Rotation3d: - return Rotation3d(roll=self.roll + other.roll, pitch=self.pitch + other.pitch, yaw=self.yaw + other.yaw) - - def minus(self, other: Rotation3d) -> Rotation3d: - return Rotation3d(roll=self.roll - other.roll, pitch=self.pitch - other.pitch, yaw=self.yaw - other.yaw) - - def unary_minus(self) -> Rotation3d: - return Rotation3d(roll=-self.roll, pitch=-self.pitch, yaw=-self.yaw) - - def to_rotation2d(self) -> Rotation2d: - return Rotation2d(degrees=self.yaw) - - -@dataclass -class Translation3d: - x: float = 0.0 - y: float = 0.0 - z: float = 0.0 - - def to_dict(self) -> dict: - d: dict = {} - d["x"] = _to_dict(self.x) - d["y"] = _to_dict(self.y) - d["z"] = _to_dict(self.z) - return d - - def plus(self, other: Translation3d) -> Translation3d: - return Translation3d(x=self.x + other.x, y=self.y + other.y, z=self.z + other.z) - - def minus(self, other: Translation3d) -> Translation3d: - return Translation3d(x=self.x - other.x, y=self.y - other.y, z=self.z - other.z) - - def unary_minus(self) -> Translation3d: - return Translation3d(x=-self.x, y=-self.y, z=-self.z) - - def times(self, scalar: float) -> Translation3d: - return Translation3d(x=self.x * scalar, y=self.y * scalar, z=self.z * scalar) - - @property - def norm(self) -> float: - return _math.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2) - - def distance(self, other: Translation3d) -> float: - return self.minus(other).norm - - def to_translation2d(self) -> Translation2d: - return Translation2d(x=self.x, y=self.y) - - def to_pose2d(self, rotation: Optional[Rotation2d] = None) -> Pose2d: - """Convert to a Pose2d (dropping Z), optionally with a rotation.""" - return Pose2d( - translation=Translation2d(x=self.x, y=self.y), - rotation=rotation if rotation is not None else Rotation2d(), - ) - - -@dataclass -class Transform3d: - rotation: Rotation3d = field(default_factory=Rotation3d) - translation: Translation3d = field(default_factory=Translation3d) - - def to_dict(self) -> dict: - d: dict = {} - d["rotation"] = _to_dict(self.rotation) - d["translation"] = _to_dict(self.translation) - return d - - def plus(self, other: Transform3d) -> Transform3d: - return Transform3d( - translation=self.translation.plus(other.translation), - rotation=self.rotation.plus(other.rotation), - ) - - def inverse(self) -> Transform3d: - return Transform3d( - translation=self.translation.unary_minus(), - rotation=self.rotation.unary_minus(), - ) - - -@dataclass -class Pose3d: - rotation: Rotation3d = field(default_factory=Rotation3d) - translation: Translation3d = field(default_factory=Translation3d) - - def to_dict(self) -> dict: - d: dict = {} - d["rotation"] = _to_dict(self.rotation) - d["translation"] = _to_dict(self.translation) - return d - - def translate_by(self, translation: Translation3d) -> Pose3d: - return Pose3d( - translation=self.translation.plus(translation), - rotation=Rotation3d(roll=self.rotation.roll, pitch=self.rotation.pitch, yaw=self.rotation.yaw), - ) - - def to_pose2d(self) -> Pose2d: - return Pose2d( - translation=Translation2d(x=self.translation.x, y=self.translation.y), - rotation=Rotation2d(degrees=self.rotation.yaw), - ) - - def to_translation2d(self) -> Translation2d: - return Translation2d(x=self.translation.x, y=self.translation.y) - diff --git a/auto_generator/src/auto_lib.py b/auto_generator/src/auto_lib.py deleted file mode 100644 index 96684a5a..00000000 --- a/auto_generator/src/auto_lib.py +++ /dev/null @@ -1,255 +0,0 @@ -""" -Auto registration system with a context-manager DSL for building -nested command trees (Sequence, Parallel, Race). - -Usage:: - - @auto("My Auto") - def my_auto(): - with sequence(): - autopilot(target_pose=...) - with parallel(): - wait(1.0) - climb_search() -""" - -from __future__ import annotations - -import json -from contextlib import contextmanager -from typing import Any, Callable, Dict, List, Optional - -from . import auto_action as AutoAction -from .auto_action import set_add_command_hook - -# --------------------------------------------------------------------------- -# Internal pointer stack -# --------------------------------------------------------------------------- - -_command_pointers: List[List[Any]] = [] - - -def _current_pointer() -> Optional[List[Any]]: - return _command_pointers[-1] if _command_pointers else None - - -def _push_pointer(pointer: List[Any]) -> None: - _command_pointers.append(pointer) - - -def _pop_pointer() -> None: - if _command_pointers: - _command_pointers.pop() - - -def _add_command(command: Any) -> None: - ptr = _current_pointer() - if ptr is not None: - ptr.append(command) - - -# Wire up the hook so that .add() on any AutoAction instance calls _add_command. -set_add_command_hook(_add_command) - -# --------------------------------------------------------------------------- -# Context-manager containers -# --------------------------------------------------------------------------- - - -@contextmanager -def sequence(): - """Context manager that groups all commands inside into a Sequence.""" - container = AutoAction.Sequence() - _push_pointer(container.actions) - try: - yield container - finally: - _pop_pointer() - _add_command(container) - - -@contextmanager -def parallel(): - """Context manager that groups all commands inside into a Parallel.""" - container = AutoAction.Parallel() - _push_pointer(container.actions) - try: - yield container - finally: - _pop_pointer() - _add_command(container) - - -@contextmanager -def race(): - """Context manager that groups all commands inside into a Race.""" - container = AutoAction.Race() - _push_pointer(container.actions) - try: - yield container - finally: - _pop_pointer() - _add_command(container) - - -# --------------------------------------------------------------------------- -# Auto registration -# --------------------------------------------------------------------------- - -_autos: Dict[str, Any] = {} -_routines: Dict[str, Any] = {} - - -def auto(name: str, can_be_mirrored: bool = True, should_be_flipped: bool = True): - """ - Decorator that defines and registers a named autonomous routine. - - Usage:: - - @auto("My Auto") - def my_auto(): - autopilot(target_pose=...) - wait(1.0) - """ - def decorator(build: Callable[[], None]): - global _command_pointers - _command_pointers = [] - root = AutoAction.Sequence() - auto = AutoAction.Auto(root_action=root, can_be_mirrored=can_be_mirrored, should_be_flipped=should_be_flipped) - - _push_pointer(root.actions) - - try: - build() - finally: - _command_pointers = [] - - _autos[name] = auto - return build - return decorator - - -# --------------------------------------------------------------------------- -# Routine registration (reusable subroutines referenced by name) -# --------------------------------------------------------------------------- - -class _Routines: - """ - Registry of named routines. - - Accessing ``routines.()`` emits an AutoReference into the current - context (compact, no inlining). Calling the decorated function directly - inlines the body and prints a warning. - """ - - _registry: Dict[str, Callable[[], None]] = {} - - def add(self, name: str, func: Callable[[], None]) -> None: - self._registry[name] = func - - def __getattr__(self, name: str) -> Callable[[], None]: - if name.startswith("_"): - raise AttributeError(name) - if name in self._registry: - def _emit_reference() -> None: - AutoAction.AutoReference(name=name).add() - return _emit_reference - raise AttributeError(f"Unknown routine '{name}'. Registered: {list(self._registry.keys())}") - - -routines = _Routines() - - -def routine(func_or_name): - """ - Decorator that registers a function as a named routine. - - Can be used with or without an explicit name:: - - @routine - def climb_left(): - ... - - @routine("ClimbRight") - def _climb_right(): - ... - - The routine body is built eagerly and stored in ``_routines`` so it - appears under the "routines" key in Autos.json. Use ``routines.()`` - inside an auto to emit an AutoReference instead of inlining. - """ - - def _register(name: str, build: Callable[[], None]) -> Callable[[], None]: - # Build and register the routine body - global _command_pointers - _command_pointers = [] - root = AutoAction.Sequence() - _push_pointer(root.actions) - try: - build() - finally: - _command_pointers = [] - _routines[name] = root - - routines.add(name, build) - - # The returned wrapper inlines the body with a warning - def inner(ignore_warning=False) -> None: - if not ignore_warning: - print( - f"WARNING: routine method {name} was called directly. " - f"This results in the routine being inlined into the auto, " - f"which may result in large file sizes. To rectify the issue, " - f"replace the call with `routines.{name}()`" - ) - build() - - return inner - - if callable(func_or_name): - # @routine (no parentheses — use the function name) - return _register(func_or_name.__name__, func_or_name) - else: - # @routine("CustomName") - def decorator(func: Callable[[], None]) -> Callable[[], None]: - return _register(func_or_name, func) - return decorator - - -def get_auto(name: str) -> Optional[Any]: - return _autos.get(name) - - -def get_autos() -> Dict[str, Any]: - return _autos - - -def get_routine(name: str) -> Optional[Any]: - return _routines.get(name) - - -def get_routines() -> Dict[str, Any]: - return _routines - - -# --------------------------------------------------------------------------- -# Serialization -# --------------------------------------------------------------------------- - -def _serialize_value(obj: Any) -> Any: - """Recursively convert objects to JSON-serializable dicts.""" - if obj is None: - return None - if hasattr(obj, "to_dict"): - return _serialize_value(obj.to_dict()) - if isinstance(obj, list): - return [_serialize_value(item) for item in obj] - if isinstance(obj, dict): - return {k: _serialize_value(v) for k, v in obj.items()} - return obj - - -def serialize_autos() -> str: - """Serializes all registered autos to a JSON string.""" - result = {name: _serialize_value(action) for name, action in _autos.items()} - return json.dumps(result, indent=4) diff --git a/auto_generator/src/autos.py b/auto_generator/src/autos.py deleted file mode 100644 index c0b71abf..00000000 --- a/auto_generator/src/autos.py +++ /dev/null @@ -1,434 +0,0 @@ -""" -"Test Auto" autonomous routine. -""" - -from __future__ import annotations - -from .routines import go_to_alliance_under_left_trench, go_to_alliance_under_right_trench, go_to_center_under_left_trench_from_alliance, go_to_center_under_left_trench_from_alliance_intake_in, go_to_center_under_right_trench_from_alliance - -from .auto_action import APConstraints, PIDGains, Transform2d -from .auto_lib import auto, parallel, routines, sequence -from .field_locations import FieldConstants -from .shorthands import ( - autopilot, - deploy_intake, - networkConfigurableWait, - pose2d, - rotation2d, - transform2d, - startShooting, - stopShooting, - stow_intake, - translation2d, - wait, - x_based_autopilot, - followPath -) -from . import units -from . import constants -from . import auto_action - -# TODO: Add alliance-relative coordinate utilities. -# TODO: Replace placeholder coordinates with real field positions. -# TODO: Switch to AutoPilotAction with entry angle and exit velocity for trench segments. - - -@auto("Literally just shoot Auto", can_be_mirrored=False, should_be_flipped=False) -def _literally_just_shoot(): - startShooting() - -def command(func: callable): # type: ignore - - def _inner(*args, **kwargs): - with sequence(): - func(*args, **kwargs) - - return _inner - -@command -def cycle_intake(time, count): - delay_each = time/2/count - for i in range(count): - stow_intake() - wait(delay_each) - deploy_intake() - wait(delay_each) - -@command -def from_bump_prepare_for_trench(angle=-90): - autopilot( - target_pose=pose2d(3.5, 7.55, angle), - velocity=constants.default_trench_velocity, - entry_angle=rotation2d(0), - constraints=auto_action.APConstraints( - velocity=2.0, - acceleration=2.0, - jerk=2.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - -@command -def _aggressive(use_depot=False, from_bump=False, shoot_preload=False, do_second_sweep=True): - intake_cycle_time = 1 / 3 - intake_cycle_count = 1 - - - if shoot_preload: - startShooting() - wait(1.5) - - if from_bump: - from_bump_prepare_for_trench() - - if shoot_preload: - stopShooting() - - # Cycle 1 - - # Autopilot under the trench - # Gives us solid acceleration and makes us resilient to unpredictable starting - # location - x_based_autopilot( - target_pose=constants.left_trench_center_side_pose, - velocity=constants.aggressive_trench_velocity, - # Constraints here are unique to this very high speed, high - # acceleration movement, so almost infinite acceleration limit is - # hardcoded in here. - constraints=APConstraints(constants.aggressive_trench_velocity, 200.0), - # No entry angle, we just want to beeline to trench exit - entry_angle=None - ) - - #with parallel(): - # stow_intake() - # followPath(path_name="Starting Position Left Trench To Center Intake In") - - with parallel(): - deploy_intake() - followPath(path_name="Left Side Aggressive Sweep Intake In") - - with parallel(): - with sequence(): - wait(0.6) - startShooting() - followPath(path_name="Left Bump To Alliance") - - wait(0.1) - - autopilot( - target_pose=constants.left_alliance_zone_middle_pose - ) - - cycle_intake(2.5, 5) - - if do_second_sweep: - cycle_intake(intake_cycle_time, intake_cycle_count) - - # wait(1.0), - # Cycle 2 - - autopilot( - target_pose=pose2d(3.5, 7.55, -90), - velocity=constants.default_trench_velocity, - entry_angle=rotation2d(0), - constraints=auto_action.APConstraints( - velocity=2.0, - acceleration=2.0, - jerk=2.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - - with parallel(): - - stopShooting() - - go_to_center_under_left_trench_from_alliance_intake_in() - - with parallel(): - deploy_intake() - followPath(path_name="Left Side Close 2nd Sweep Intake In") - - with parallel(): - with sequence(): - wait(0.4) - startShooting() - followPath(path_name="Left Bump To Alliance") - - wait(0.1) - - autopilot( - target_pose=pose2d(x=2.700, y=5.75,angle_degrees=-90) - ) - - wait(1) - - if use_depot: - autopilot( - target_pose=pose2d(1.5, 5.9, -180), - velocity=0.0, - constraints=auto_action.APConstraints( - velocity=2.0, - acceleration=2.0, - jerk=2.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - - cycle_intake(intake_cycle_time, intake_cycle_count) - -@auto("Aggressive Depot") -def _aggressive_depot(): - _aggressive(True, False, False, True) - -@auto("Aggressive No Depot") -def _aggressive_no_depot(): - _aggressive(False, False, False, True) - -@auto("Aggressive Depot From Bump") -def _aggressive_depot_from_bump(): - _aggressive(True, True, True, False) - -@command -def _conservative(use_depot=False, from_bump=False, shoot_preload=False, do_second_sweep=True): - intake_cycle_time = 0.5 - intake_cycle_count = 1 - - if shoot_preload: - startShooting() - wait(1.5) - - if from_bump: - from_bump_prepare_for_trench(angle=0) - - if shoot_preload: - stopShooting() - - # Cycle 1 - - x_based_autopilot( - target_pose=pose2d(5.2, 7.4), - velocity=constants.default_trench_velocity, - entry_angle=None - ) - - with parallel(): - stow_intake() - followPath(path_name="Starting Position Left Trench To Center") - - with parallel(): - deploy_intake() - followPath(path_name="Left Side Conservative Sweep") - - with parallel(): - with sequence(): - wait(0.6) - startShooting() - followPath(path_name="Left Bump To Alliance") - - wait(0.1) - - autopilot( - target_pose=pose2d(x=2.700, y=5.75,angle_degrees=-90) - ) - - wait(2.5) - - # wait(1.0), - # Cycle 2 - if do_second_sweep: - cycle_intake(intake_cycle_time, intake_cycle_count) - - autopilot( - target_pose=pose2d(3.5, 7.55, -90), - velocity=constants.default_trench_velocity, - entry_angle=rotation2d(0), - constraints=auto_action.APConstraints( - velocity=2.0, - acceleration=2.0, - jerk=2.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - - with parallel(): - - stopShooting() - - go_to_center_under_left_trench_from_alliance_intake_in() - - with parallel(): - deploy_intake() - followPath(path_name="Left Side Close 2nd Sweep Intake In") - - with parallel(): - with sequence(): - wait(0.4) - startShooting() - followPath(path_name="Left Bump To Alliance") - - wait(0.1) - - autopilot( - target_pose=pose2d(x=2.700, y=5.75,angle_degrees=-90) - ) - - wait(1) - - if use_depot: - autopilot( - target_pose=pose2d(1.5, 5.9, -180), - velocity=0.0, - constraints=auto_action.APConstraints( - velocity=2.0, - acceleration=2.0, - jerk=2.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - - cycle_intake(intake_cycle_time, intake_cycle_count) - - -@auto("Conservative Depot") -def _conservative_depot(): - _conservative(True, False, False, True) - -@auto("Conservative No Depot") -def _conservative_no_depot(): - _conservative(False, False, False, True) - -@auto("Conservative Depot From Bump") -def _conservative_depot_from_bump(): - _conservative(True, True, True, False) - -@command -def go_to_depot_and_intake(): - # drive near the depot and slow down so that we don't go too fast while intaking - autopilot( - target_pose=pose2d(1.5, 5.1, 135), - profile=auto_action.APProfile( - constraints= - auto_action.APConstraints( - velocity=5.1, - acceleration=10.0, - jerk=3.0 - ), - error_x_y=units.Meter.of(0.1), - error_theta=units.Degree.of(4.0), - beeline_radius=units.Meter.of(0.2) - ), - velocity=1.0, - ) - autopilot( - target_pose=pose2d(0.715, 5.1, 135), - constraints=auto_action.APConstraints( - velocity=1.0, - acceleration=3.0, - jerk=3.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - wait(0.1) # Wait to stop autopilot from seeing initial velocity and panicking - # Autopilot is probably more robust and simpler - # Also removes a path planner path and enables us to use hot reload to tune it - # followPath("Intake Depot") - autopilot( - target_pose=pose2d(0.715, 6.4, 135), - constraints=auto_action.APConstraints( - velocity=1.0, - acceleration=3.0, - jerk=2.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - wait(0.1) # Wait to stop autopilot from seeing initial velocity and panicking - - autopilot( - target_pose=pose2d(1.0, 6.8, 135), - constraints=auto_action.APConstraints( - velocity=5.1, - acceleration=5.0, - jerk=3.0 - ), - pid_gains=PIDGains( - k_p=1.5, - ) - ) - -@auto("Follower") -def _follower(): - # Don't deploy the intake to avoid smashing it into the trench - #deploy_intake() - #startShooting() - networkConfigurableWait("Follower - Preload", units.Second.of(2.5)) - #stopShooting() - x_based_autopilot( - target_pose=constants.left_trench_center_side_pose, - velocity=2.0, - # Constraints here are unique to this very high speed, high - # acceleration movement, so almost infinite acceleration limit is - # hardcoded in here. - constraints=APConstraints(constants.aggressive_trench_velocity, 200.0), - # No entry angle, we just want to beeline to trench exit - entry_angle=None - ) - deploy_intake() - followPath("Left Side Follower Sweep Intake In") - networkConfigurableWait("Follower - Before Bump Return", units.Second.of(0.0)) - wait(0.1) - autopilot( - target_pose=pose2d(x=2.700, y=5.75,angle_degrees=-90) - ) - autopilot( - target_pose=pose2d(1.5, 5.1, 135), - profile=auto_action.APProfile( - constraints= - auto_action.APConstraints( - velocity=5.1, - acceleration=10.0, - jerk=3.0 - ), - error_x_y=units.Meter.of(0.1), - error_theta=units.Degree.of(4.0), - beeline_radius=units.Meter.of(0.2) - ), - ) - startShooting() - networkConfigurableWait("Follower - Before Depot", units.Second.of(0.0)) - go_to_depot_and_intake() - wait(2.0) - cycle_intake(6/3, 6) - -@auto("Single Swipe Then Depot", can_be_mirrored=False) -def _single_swipe(): - _aggressive(do_second_sweep=False) - go_to_depot_and_intake() - wait(1.0) - cycle_intake(6/3, 6) - - -@auto("Center Depot", can_be_mirrored=False) -def _center_depot(): - deploy_intake() - startShooting() - networkConfigurableWait("Center Depot - Shoot Preload", units.Second.of(4.0)) - go_to_depot_and_intake() - # startShooting() - wait(2.0) - cycle_intake(6/3, 6) diff --git a/auto_generator/src/constants.py b/auto_generator/src/constants.py deleted file mode 100644 index bc13a981..00000000 --- a/auto_generator/src/constants.py +++ /dev/null @@ -1,60 +0,0 @@ -""" -Python equivalent of Constants.ts. - -Robot-specific constants used by the auto routines. -""" - -from __future__ import annotations - -from . import auto_action as AutoAction -from .auto_action import ( - APConstraints, - Transform2d -) -from .field_locations import FieldConstants -from .shorthands import ( - pose2d, - rotation2d, - translation2d, -) - -# TODO: Maybe make these loaded from the constants files in the main robot code -# instead of hardcoded here, to avoid duplication and potential inconsistencies. - -climb_offset = Transform2d( - translation=translation2d(x=0.41, y=0.0225), - rotation=rotation2d(degrees=-90.0), -) - -climb_constraints = APConstraints( - velocity=2.0, - acceleration=2.0, - jerk=0.1, -) - -left_climb_location = FieldConstants.Tower.left_upright().to_pose2d().transform_by(climb_offset) - -right_climb_location = FieldConstants.Tower.right_upright().to_pose2d().transform_by(climb_offset) - -climb_left_lineup_entry_angle = rotation2d(degrees=0) -climb_right_lineup_entry_angle = climb_left_lineup_entry_angle - - -default_trench_velocity = 4.2 -aggressive_trench_velocity = 5.1 - -# Reduce tolerance because in most spots we dont need them to be so far from the trench. -# and if we need to be further from the trench we can just have the auto drive to a pose that it can use such as -# the a position that is 1 m from the trench and then drive to the trench from there. -center_side_trench_offset = translation2d(x=0.5, y=0.0) -alliance_side_trench_offset = translation2d(x=-0.5, y=0.0) - -left_trench_center_side_pose = pose2d(5.2, 7.4, -90) -right_trench_center_side_pose = FieldConstants.RightTrench.opening_floor_center().to_pose2d().translate_by(center_side_trench_offset) - -left_trench_alliance_side_pose = FieldConstants.LeftTrench.opening_floor_center().to_pose2d().translate_by(alliance_side_trench_offset) -right_trench_alliance_side_pose = FieldConstants.RightTrench.opening_floor_center().to_pose2d().translate_by(alliance_side_trench_offset) - -# Shooting position in the middle of the left side of the alliance zone -# This is where we end up after coming over the bump -left_alliance_zone_middle_pose = pose2d(x=2.700, y=5.75,angle_degrees=-90) diff --git a/auto_generator/src/field_locations.py b/auto_generator/src/field_locations.py deleted file mode 100644 index 5fe5fde2..00000000 --- a/auto_generator/src/field_locations.py +++ /dev/null @@ -1,675 +0,0 @@ -""" -Python equivalent of FieldLocations.ts. - -Contains information for location of field elements and other useful -reference points. All constants are defined relative to the field -coordinate system, and from the perspective of the blue alliance station. - -Original Java source: - https://github.com/Mechanical-Advantage/RobotCode2026Public - Copyright (c) 2025-2026 Littleton Robotics - License: MIT (see FieldConstants_LICENSE) - Modifications after 16:47 ET 2026-1-28 by FRC Team 401 Copperhead Robotics -""" - -from __future__ import annotations - -import json -import math -from pathlib import Path -from typing import Any, Dict, List - -from .auto_action import ( - Pose2d, - Pose3d, - Rotation2d, - Rotation3d, - Translation2d, - Translation3d, -) -from .shorthands import pose3d as _make_pose3d - - -# --------------------------------------------------------------------------- -# Geometry helpers (local to this module) -# --------------------------------------------------------------------------- - - -def _translation2d(x: float, y: float) -> Translation2d: - return Translation2d(x=x, y=y) - - -def _translation3d(x: float, y: float, z: float) -> Translation3d: - return Translation3d(x=x, y=y, z=z) - - -def _pose2d(x: float, y: float, degrees: float) -> Pose2d: - return Pose2d( - translation=Translation2d(x=x, y=y), - rotation=Rotation2d(degrees=degrees), - ) - - -# --------------------------------------------------------------------------- -# Unit conversions -# --------------------------------------------------------------------------- - - -def _inches_to_meters(inches: float) -> float: - return inches * 0.0254 - - -# --------------------------------------------------------------------------- -# AprilTag layout loading -# --------------------------------------------------------------------------- - -_LAYOUT_PATH = ( - Path(__file__).resolve().parent.parent.parent - / "src" - / "main" - / "deploy" - / "apriltags" - / "2026-rebuilt-andymark.json" -) - - -def _load_april_tag_layout() -> Dict[str, Any]: - with open(_LAYOUT_PATH, "r") as f: - return json.load(f) - - -_layout = _load_april_tag_layout() - - -def _get_tag_pose(tag_id: int) -> Dict[str, Any]: - for tag in _layout["tags"]: - if tag["ID"] == tag_id: - return tag["pose"] - raise ValueError(f"AprilTag with ID {tag_id} not found in layout") - - -def _quaternion_to_yaw_degrees(q: Dict[str, float]) -> float: - """Convert a quaternion (W, X, Y, Z) to a yaw angle in degrees.""" - radians = math.atan2( - 2.0 * (q["W"] * q["Z"] + q["X"] * q["Y"]), - 1.0 - 2.0 * (q["Y"] ** 2 + q["Z"] ** 2), - ) - return math.degrees(radians) - - -def _tag_pose_to_pose2d(tag_pose: Dict[str, Any]) -> Pose2d: - return _pose2d( - tag_pose["translation"]["x"], - tag_pose["translation"]["y"], - _quaternion_to_yaw_degrees(tag_pose["rotation"]["quaternion"]), - ) - - -# --------------------------------------------------------------------------- -# FieldConstants -# --------------------------------------------------------------------------- - - -class FieldConstants: - """Top-level namespace for all field constants.""" - - # AprilTag related constants - april_tag_count = len(_layout["tags"]) - april_tag_width = _inches_to_meters(6.5) - - # Field dimensions - field_length: float = _layout["field"]["length"] - field_width: float = _layout["field"]["width"] - - # ------------------------------------------------------------------------- - # LinesVertical - # ------------------------------------------------------------------------- - - class LinesVertical: - center = FieldConstants.field_length / 2.0 if False else 0.0 # set below - - starting = _get_tag_pose(26)["translation"]["x"] - - alliance_zone = starting - - neutral_zone_near = 0.0 # set below - neutral_zone_far = 0.0 # set below - - opp_alliance_zone = _get_tag_pose(10)["translation"]["x"] - - @staticmethod - def hub_center() -> float: - return _get_tag_pose(26)["translation"]["x"] + FieldConstants.Hub.width / 2.0 - - @staticmethod - def opp_hub_center() -> float: - return _get_tag_pose(4)["translation"]["x"] + FieldConstants.Hub.width / 2.0 - - # ------------------------------------------------------------------------- - # LinesHorizontal - # ------------------------------------------------------------------------- - - class LinesHorizontal: - center = FieldConstants.field_width / 2.0 if False else 0.0 # set below - - left_trench_open_start = 0.0 # set below - - right_trench_open_end = 0.0 - - @staticmethod - def right_bump_start() -> float: - return FieldConstants.Hub.near_right_corner().y - - @staticmethod - def right_bump_end() -> float: - return FieldConstants.LinesHorizontal.right_bump_start() - FieldConstants.RightBump.width - - @staticmethod - def right_trench_open_start() -> float: - return FieldConstants.LinesHorizontal.right_bump_end() - _inches_to_meters(12.0) - - @staticmethod - def left_bump_end() -> float: - return FieldConstants.Hub.near_left_corner().y - - @staticmethod - def left_bump_start() -> float: - return FieldConstants.LinesHorizontal.left_bump_end() + FieldConstants.LeftBump.width - - @staticmethod - def left_trench_open_end() -> float: - return FieldConstants.LinesHorizontal.left_bump_start() + _inches_to_meters(12.0) - - # ------------------------------------------------------------------------- - # Hub - # ------------------------------------------------------------------------- - - class Hub: - width = _inches_to_meters(47.0) - height = _inches_to_meters(72.0) - inner_width = _inches_to_meters(41.7) - inner_height = _inches_to_meters(56.5) - - @staticmethod - def top_center_point() -> Translation3d: - return _translation3d( - _get_tag_pose(26)["translation"]["x"] + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0, - FieldConstants.Hub.height, - ) - - @staticmethod - def inner_center_point() -> Translation3d: - return _translation3d( - _get_tag_pose(26)["translation"]["x"] + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0, - FieldConstants.Hub.inner_height, - ) - - @staticmethod - def near_left_corner() -> Translation2d: - tcp = FieldConstants.Hub.top_center_point() - return _translation2d( - tcp.x - FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 + FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def near_right_corner() -> Translation2d: - tcp = FieldConstants.Hub.top_center_point() - return _translation2d( - tcp.x - FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 - FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def far_left_corner() -> Translation2d: - tcp = FieldConstants.Hub.top_center_point() - return _translation2d( - tcp.x + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 + FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def far_right_corner() -> Translation2d: - tcp = FieldConstants.Hub.top_center_point() - return _translation2d( - tcp.x + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 - FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def opp_top_center_point() -> Translation3d: - return _translation3d( - _get_tag_pose(4)["translation"]["x"] + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0, - FieldConstants.Hub.height, - ) - - @staticmethod - def opp_inner_center_point() -> Translation3d: - return _translation3d( - _get_tag_pose(4)["translation"]["x"] + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0, - FieldConstants.Hub.inner_height, - ) - - @staticmethod - def opp_near_left_corner() -> Translation2d: - tcp = FieldConstants.Hub.opp_top_center_point() - return _translation2d( - tcp.x - FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 + FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def opp_near_right_corner() -> Translation2d: - tcp = FieldConstants.Hub.opp_top_center_point() - return _translation2d( - tcp.x - FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 - FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def opp_far_left_corner() -> Translation2d: - tcp = FieldConstants.Hub.opp_top_center_point() - return _translation2d( - tcp.x + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 + FieldConstants.Hub.width / 2.0, - ) - - @staticmethod - def opp_far_right_corner() -> Translation2d: - tcp = FieldConstants.Hub.opp_top_center_point() - return _translation2d( - tcp.x + FieldConstants.Hub.width / 2.0, - FieldConstants.field_width / 2.0 - FieldConstants.Hub.width / 2.0, - ) - - # Hub faces - @staticmethod - def near_face() -> Pose2d: - return _tag_pose_to_pose2d(_get_tag_pose(26)) - - @staticmethod - def far_face() -> Pose2d: - return _tag_pose_to_pose2d(_get_tag_pose(20)) - - @staticmethod - def right_face() -> Pose2d: - return _tag_pose_to_pose2d(_get_tag_pose(18)) - - @staticmethod - def left_face() -> Pose2d: - return _tag_pose_to_pose2d(_get_tag_pose(21)) - - # ------------------------------------------------------------------------- - # LeftBump - # ------------------------------------------------------------------------- - - class LeftBump: - width = _inches_to_meters(73.0) - height = _inches_to_meters(6.513) - depth = _inches_to_meters(44.4) - - @staticmethod - def near_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() - FieldConstants.LeftBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def near_right_corner() -> Translation2d: - return FieldConstants.Hub.near_left_corner() - - @staticmethod - def far_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() + FieldConstants.LeftBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def far_right_corner() -> Translation2d: - return FieldConstants.Hub.far_left_corner() - - @staticmethod - def opp_near_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() - FieldConstants.LeftBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def opp_near_right_corner() -> Translation2d: - return FieldConstants.Hub.opp_near_left_corner() - - @staticmethod - def opp_far_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() + FieldConstants.LeftBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def opp_far_right_corner() -> Translation2d: - return FieldConstants.Hub.opp_far_left_corner() - - # ------------------------------------------------------------------------- - # RightBump - # ------------------------------------------------------------------------- - - class RightBump: - width = _inches_to_meters(73.0) - height = _inches_to_meters(6.513) - depth = _inches_to_meters(44.4) - - @staticmethod - def near_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() + FieldConstants.RightBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def near_right_corner() -> Translation2d: - return FieldConstants.Hub.near_left_corner() - - @staticmethod - def far_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() - FieldConstants.RightBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def far_right_corner() -> Translation2d: - return FieldConstants.Hub.far_left_corner() - - @staticmethod - def opp_near_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() + FieldConstants.RightBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def opp_near_right_corner() -> Translation2d: - return FieldConstants.Hub.opp_near_left_corner() - - @staticmethod - def opp_far_left_corner() -> Translation2d: - return _translation2d( - FieldConstants.LinesVertical.hub_center() - FieldConstants.RightBump.width / 2, - _inches_to_meters(255), - ) - - @staticmethod - def opp_far_right_corner() -> Translation2d: - return FieldConstants.Hub.opp_far_left_corner() - - # ------------------------------------------------------------------------- - # LeftTrench - # ------------------------------------------------------------------------- - - class LeftTrench: - width = _inches_to_meters(65.65) - depth = _inches_to_meters(47.0) - height = _inches_to_meters(40.25) - opening_width = _inches_to_meters(50.34) - opening_height = _inches_to_meters(22.25) - - @staticmethod - def opening_top_left() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.hub_center(), - FieldConstants.field_width, - FieldConstants.LeftTrench.opening_height, - ) - - @staticmethod - def opening_top_right() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.hub_center(), - FieldConstants.field_width - FieldConstants.LeftTrench.opening_width, - FieldConstants.LeftTrench.opening_height, - ) - - @staticmethod - def opening_floor_center() -> Pose3d: - return _make_pose3d( - x=FieldConstants.LinesVertical.hub_center(), - y=FieldConstants.field_width - FieldConstants.LeftTrench.opening_width / 2, - z=0.0, - ) - - @staticmethod - def opp_opening_top_left() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.opp_hub_center(), - FieldConstants.field_width, - FieldConstants.LeftTrench.opening_height, - ) - - @staticmethod - def opp_opening_top_right() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.opp_hub_center(), - FieldConstants.field_width - FieldConstants.LeftTrench.opening_width, - FieldConstants.LeftTrench.opening_height, - ) - - # ------------------------------------------------------------------------- - # RightTrench - # ------------------------------------------------------------------------- - - class RightTrench: - width = _inches_to_meters(65.65) - depth = _inches_to_meters(47.0) - height = _inches_to_meters(40.25) - opening_width = _inches_to_meters(50.34) - opening_height = _inches_to_meters(22.25) - - @staticmethod - def opening_top_left() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.hub_center(), - FieldConstants.RightTrench.opening_width, - FieldConstants.RightTrench.opening_height, - ) - - @staticmethod - def opening_top_right() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.hub_center(), - 0, - FieldConstants.RightTrench.opening_height, - ) - - @staticmethod - def opening_floor_center() -> Pose3d: - return _make_pose3d( - x=FieldConstants.LinesVertical.hub_center(), - y=FieldConstants.RightTrench.opening_width / 2, - z=0.0, - ) - - @staticmethod - def opp_opening_top_left() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.opp_hub_center(), - FieldConstants.RightTrench.opening_width, - FieldConstants.RightTrench.opening_height, - ) - - @staticmethod - def opp_opening_top_right() -> Translation3d: - return _translation3d( - FieldConstants.LinesVertical.opp_hub_center(), - 0, - FieldConstants.RightTrench.opening_height, - ) - - # ------------------------------------------------------------------------- - # Tower - # ------------------------------------------------------------------------- - - class Tower: - width = _inches_to_meters(49.25) - depth = _inches_to_meters(45.0) - height = _inches_to_meters(78.25) - inner_opening_width = _inches_to_meters(32.25) - front_face_x = _inches_to_meters(43.51) - - upright_height = _inches_to_meters(72.1) - - low_rung_height = _inches_to_meters(27.0) - mid_rung_height = _inches_to_meters(45.0) - high_rung_height = _inches_to_meters(63.0) - - @staticmethod - def center_point() -> Translation2d: - return _translation2d( - FieldConstants.Tower.front_face_x, - _get_tag_pose(31)["translation"]["y"], - ) - - @staticmethod - def left_upright() -> Translation2d: - return _translation2d( - FieldConstants.Tower.front_face_x, - _get_tag_pose(31)["translation"]["y"] - + FieldConstants.Tower.inner_opening_width / 2 - + _inches_to_meters(0.75), - ) - - @staticmethod - def right_upright() -> Translation2d: - return _translation2d( - FieldConstants.Tower.front_face_x, - _get_tag_pose(31)["translation"]["y"] - - FieldConstants.Tower.inner_opening_width / 2 - - _inches_to_meters(0.75), - ) - - @staticmethod - def opp_center_point() -> Translation2d: - return _translation2d( - FieldConstants.field_length - FieldConstants.Tower.front_face_x, - _get_tag_pose(15)["translation"]["y"], - ) - - @staticmethod - def opp_left_upright() -> Translation2d: - return _translation2d( - FieldConstants.field_length - FieldConstants.Tower.front_face_x, - _get_tag_pose(15)["translation"]["y"] - + FieldConstants.Tower.inner_opening_width / 2 - + _inches_to_meters(0.75), - ) - - @staticmethod - def opp_right_upright() -> Translation2d: - return _translation2d( - FieldConstants.field_length - FieldConstants.Tower.front_face_x, - _get_tag_pose(15)["translation"]["y"] - - FieldConstants.Tower.inner_opening_width / 2 - - _inches_to_meters(0.75), - ) - - # ------------------------------------------------------------------------- - # Depot - # ------------------------------------------------------------------------- - - class Depot: - width = _inches_to_meters(42.0) - depth = _inches_to_meters(27.0) - height = _inches_to_meters(1.125) - distance_from_center_y = _inches_to_meters(75.93) - - @staticmethod - def depot_center() -> Translation3d: - fw = FieldConstants.field_width - return _translation3d( - FieldConstants.Depot.depth, - fw / 2 + FieldConstants.Depot.distance_from_center_y, - FieldConstants.Depot.height, - ) - - @staticmethod - def left_corner() -> Translation3d: - fw = FieldConstants.field_width - return _translation3d( - FieldConstants.Depot.depth, - fw / 2 + FieldConstants.Depot.distance_from_center_y + FieldConstants.Depot.width / 2, - FieldConstants.Depot.height, - ) - - @staticmethod - def right_corner() -> Translation3d: - fw = FieldConstants.field_width - return _translation3d( - FieldConstants.Depot.depth, - fw / 2 + FieldConstants.Depot.distance_from_center_y - FieldConstants.Depot.width / 2, - FieldConstants.Depot.height, - ) - - # ------------------------------------------------------------------------- - # Outpost - # ------------------------------------------------------------------------- - - class Outpost: - width = _inches_to_meters(31.8) - opening_distance_from_floor = _inches_to_meters(28.1) - height = _inches_to_meters(7.0) - - @staticmethod - def center_point() -> Translation2d: - return _translation2d(0, _get_tag_pose(29)["translation"]["y"]) - - # ------------------------------------------------------------------------- - # Alliance / OppAlliance - # ------------------------------------------------------------------------- - - class Alliance: - pass # populated below - - class OppAlliance: - pass # populated below - - - class Center: - @staticmethod - def center_point() -> Translation2d: - return _translation2d( - FieldConstants.field_length / 2.0, FieldConstants.field_width / 2.0 - ) - -# --------------------------------------------------------------------------- -# Deferred initialization for values that depend on other FieldConstants members -# --------------------------------------------------------------------------- - -FieldConstants.LinesVertical.center = FieldConstants.field_length / 2.0 -FieldConstants.LinesVertical.neutral_zone_near = ( - FieldConstants.LinesVertical.center - _inches_to_meters(120) -) -FieldConstants.LinesVertical.neutral_zone_far = ( - FieldConstants.LinesVertical.center + _inches_to_meters(120) -) - -FieldConstants.LinesHorizontal.center = FieldConstants.field_width / 2.0 -FieldConstants.LinesHorizontal.left_trench_open_start = FieldConstants.field_width - -FieldConstants.Alliance.center = _pose2d( - FieldConstants.LinesVertical.alliance_zone / 2.0, - FieldConstants.LinesHorizontal.center, - 0, -) - -FieldConstants.OppAlliance.center = _pose2d( - FieldConstants.LinesVertical.opp_alliance_zone - + (FieldConstants.field_length - FieldConstants.LinesVertical.opp_alliance_zone) / 2.0, - FieldConstants.LinesHorizontal.center, - 0, -) diff --git a/auto_generator/src/routines.py b/auto_generator/src/routines.py deleted file mode 100644 index a69f620c..00000000 --- a/auto_generator/src/routines.py +++ /dev/null @@ -1,148 +0,0 @@ -""" -Reusable auto subroutines (e.g. climb lineup). -""" - -from __future__ import annotations - -from typing import Optional - -from . import auto_action as AutoAction -from .auto_lib import auto, parallel, routine, routines, sequence -from . import constants as Constants -from .field_locations import FieldConstants -from .shorthands import ( - autopilot, - climb_hang, - climb_search, - pose2d, - rotation2d, - wait, - x_based_autopilot, -) - -# I really want to rename these commands or do something about them - -def go_to_alliance_under_left_trench(velocity = Constants.default_trench_velocity, rotation = rotation2d(0), first_entry_angle = rotation2d(180), second_entry_angle = rotation2d(180)): - transform = AutoAction.Transform2d( - rotation=rotation, - ) - with sequence(): - x_based_autopilot( - target_pose=Constants.left_trench_center_side_pose.transform_by(transform), - velocity=velocity, - entry_angle=first_entry_angle - ) - # x_based_autopilot( - # target_pose=Constants.left_trench_alliance_side_pose.transform_by(transform), - # velocity=velocity, - # entry_angle=second_entry_angle - # ) - AutoAction.FollowPathPlannerPath(path_name="Left Trench To Alliance").add() - -def go_to_alliance_under_right_trench(velocity = Constants.default_trench_velocity, rotation = rotation2d(0), first_entry_angle = rotation2d(180), second_entry_angle = rotation2d(180)): - transform = AutoAction.Transform2d( - rotation=rotation, - ) - with sequence(): - x_based_autopilot( - target_pose=Constants.right_trench_center_side_pose.transform_by(transform), - velocity=velocity, - entry_angle=first_entry_angle - ) - # x_based_autopilot( - # target_pose=Constants.right_trench_alliance_side_pose.transform_by(transform), - # velocity=velocity, - # entry_angle=second_entry_angle - # ) - AutoAction.FollowPathPlannerPath(path_name="Right Trench To Alliance").add() - -def go_to_center_under_left_trench_from_alliance(velocity = Constants.default_trench_velocity, rotation = rotation2d(), first_entry_angle = rotation2d(0), second_entry_angle = rotation2d(0)): - transform = AutoAction.Transform2d( - rotation=rotation, - ) - with sequence(): - x_based_autopilot( - target_pose=Constants.left_trench_alliance_side_pose.transform_by(transform), - velocity=velocity, - entry_angle=first_entry_angle - ) - # x_based_autopilot( - # target_pose=Constants.left_trench_center_side_pose.transform_by(transform), - # velocity=velocity, - # entry_angle=second_entry_angle - # ) - AutoAction.FollowPathPlannerPath(path_name="Left Trench To Center").add() - -def go_to_center_under_left_trench_from_alliance_intake_in(velocity = Constants.default_trench_velocity, rotation = rotation2d(-90), first_entry_angle = rotation2d(0), second_entry_angle = rotation2d(0)): - transform = AutoAction.Transform2d( - rotation=rotation, - ) - with sequence(): - x_based_autopilot( - target_pose=pose2d(4.0, 7.55, -90), - velocity=velocity, - entry_angle=first_entry_angle - ) - # x_based_autopilot( - # target_pose=Constants.left_trench_center_side_pose.transform_by(transform), - # velocity=velocity, - # entry_angle=second_entry_angle - # ) - AutoAction.FollowPathPlannerPath(path_name="Left Trench To Center Intake In").add() - -def go_to_center_under_right_trench_from_alliance(velocity = Constants.default_trench_velocity, rotation = rotation2d(0), first_entry_angle = rotation2d(0), second_entry_angle = rotation2d(0)): - transform = AutoAction.Transform2d( - rotation=rotation, - ) - with sequence(): - x_based_autopilot( - target_pose=Constants.right_trench_alliance_side_pose.transform_by(transform), - velocity=velocity, - entry_angle=first_entry_angle - ) - # x_based_autopilot( - # target_pose=Constants.right_trench_center_side_pose.transform_by(transform), - # velocity=velocity, - # entry_angle=second_entry_angle - # ) - AutoAction.FollowPathPlannerPath(path_name="Right Trench To Center").add() - - -def _climb( - target_pose: AutoAction.Pose2d, - entry_angle: Optional[AutoAction.Rotation2d] = None, - velocity: Optional[float] = None, -) -> None: - """Reusable climb lineup subroutine. Call inside a context.""" - with parallel(): - with sequence(): - # x_based_autopilot( - # target_pose=FieldConstants.Alliance.center.transform_by(Constants.climb_offset), - # constraints=Constants.climb_constraints - # ) - x_based_autopilot( - target_pose=target_pose, - entry_angle=entry_angle, - constraints=Constants.climb_constraints - ) - climb_search() - wait(0.5) - climb_hang() - -# Register climb lineup routines - - -@routine("LeftClimb") -def _left_climb(): - _climb( - target_pose=Constants.left_climb_location, - entry_angle=Constants.climb_left_lineup_entry_angle - ) - - -@routine("RightClimb") -def _right_climb(): - _climb( - target_pose=Constants.right_climb_location, - entry_angle=Constants.climb_right_lineup_entry_angle - ) diff --git a/auto_generator/src/shorthands.py b/auto_generator/src/shorthands.py deleted file mode 100644 index 875358c1..00000000 --- a/auto_generator/src/shorthands.py +++ /dev/null @@ -1,206 +0,0 @@ -""" -Geometry helper functions and command shorthand wrappers for the -auto builder DSL. - -Context-manager containers (``sequence``, ``parallel``, ``race``) are -re-exported from ``auto_lib`` for convenience. -""" - -from __future__ import annotations - -from typing import Optional - -from . import auto_action as AutoAction -from .units import Measure, Second - -# Re-export context-manager containers so callers can do: -# from .shorthands import sequence, parallel, race -from .auto_lib import parallel, race, sequence # noqa: F401 - - -# --------------------------------------------------------------------------- -# Geometry helpers -# --------------------------------------------------------------------------- - - -def translation2d(x: float = 0.0, y: float = 0.0) -> AutoAction.Translation2d: - return AutoAction.Translation2d(x=x, y=y) - - -def rotation2d(degrees: float = 0.0) -> AutoAction.Rotation2d: - return AutoAction.Rotation2d(degrees=degrees) - - -def pose2d( - x: float = 0.0, y: float = 0.0, angle_degrees: float = 0.0 -) -> AutoAction.Pose2d: - return AutoAction.Pose2d( - translation=translation2d(x, y), - rotation=rotation2d(angle_degrees), - ) - - -def translation3d( - x: float = 0.0, y: float = 0.0, z: float = 0.0 -) -> AutoAction.Translation3d: - return AutoAction.Translation3d(x=x, y=y, z=z) - - -def rotation3d( - roll_degrees: float = 0.0, - pitch_degrees: float = 0.0, - yaw_degrees: float = 0.0, -) -> AutoAction.Rotation3d: - return AutoAction.Rotation3d( - roll=roll_degrees, pitch=pitch_degrees, yaw=yaw_degrees - ) - - -def pose3d( - x: float = 0.0, - y: float = 0.0, - z: float = 0.0, - roll_degrees: float = 0.0, - pitch_degrees: float = 0.0, - yaw_degrees: float = 0.0, -) -> AutoAction.Pose3d: - return AutoAction.Pose3d( - translation=translation3d(x, y, z), - rotation=rotation3d(roll_degrees, pitch_degrees, yaw_degrees), - ) - - -def transform2d( - translation: Optional[AutoAction.Translation2d] = None, - rotation: Optional[AutoAction.Rotation2d] = None, -) -> AutoAction.Transform2d: - return AutoAction.Transform2d( - translation=translation if translation is not None else AutoAction.Translation2d(), - rotation=rotation if rotation is not None else AutoAction.Rotation2d(), - ) - - -# --------------------------------------------------------------------------- -# Primitive action shorthands -# --------------------------------------------------------------------------- - - -def wait(seconds: float) -> None: - """Insert a Wait action into the current context.""" - AutoAction.Wait(delay=Second.of(seconds)).add() - - -def print_msg(message: str) -> None: - """Insert a Print action into the current context.""" - AutoAction.Print(message=message).add() - - -def autopilot( - target: Optional[AutoAction.APTarget] = None, - *, - target_pose: Optional[AutoAction.Pose2d] = None, - entry_angle: Optional[AutoAction.Rotation2d] = None, - velocity: Optional[float] = None, - rotation_radius: Optional[AutoAction.Measure] = None, - constraints: Optional[AutoAction.APConstraints] = None, - profile: Optional[AutoAction.APProfile] = None, - pid_gains: Optional[AutoAction.PIDGains] = None, - can_mirror: bool = True, -) -> None: - """Insert an AutoPilotAction into the current context. - - You can pass a pre-built ``APTarget`` as the first argument, or use the - convenience keyword arguments ``target_pose``, ``entry_angle``, - ``velocity``, and ``rotation_radius`` to build one inline. - """ - if target is None: - target = AutoAction.APTarget( - reference=target_pose, - entry_angle=entry_angle, - velocity=velocity if velocity is not None else 0.0, - rotation_radius=rotation_radius, - ) - AutoAction.AutoPilotAction( - target=target, - constraints=constraints, - profile=profile, - pid_gains=pid_gains, - can_mirror=can_mirror, - ).add() - - -def x_based_autopilot( - target: Optional[AutoAction.APTarget] = None, - *, - target_pose: Optional[AutoAction.Pose2d] = None, - entry_angle: Optional[AutoAction.Rotation2d] = None, - velocity: Optional[float] = None, - rotation_radius: Optional[AutoAction.Measure] = None, - constraints: Optional[AutoAction.APConstraints] = None, - profile: Optional[AutoAction.APProfile] = None, - pid_gains: Optional[AutoAction.PIDGains] = None, - can_mirror: bool = True, -) -> None: - """Insert an XBasedAutoPilotAction into the current context.""" - if target is None: - target = AutoAction.APTarget( - reference=target_pose, - entry_angle=entry_angle, - velocity=velocity if velocity is not None else 0.0, - rotation_radius=rotation_radius, - ) - AutoAction.XBasedAutoPilotAction( - target=target, - constraints=constraints, - profile=profile, - pid_gains=pid_gains, - can_mirror=can_mirror, - ).add() - - -# Keep the old name as an alias for backwards compat -x_based_autopilot_action = x_based_autopilot - - -def stop_drive() -> None: - """Insert a StopDriveAction into the current context.""" - AutoAction.StopDriveAction().add() - - -def deploy_intake() -> None: - """Insert a DeployIntakeAction into the current context.""" - AutoAction.DeployIntakeAction().add() - - -def stow_intake() -> None: - """Insert a StowIntakeAction into the current context.""" - AutoAction.StowIntakeAction().add() - - -def climb_search() -> None: - """Insert a ClimbSearchAction into the current context.""" - AutoAction.ClimbSearchAction().add() - - -def climb_hang() -> None: - """Insert a ClimbHangAction into the current context.""" - AutoAction.ClimbHangAction().add() - - -def reference(auto_name: str) -> None: - """Insert an AutoReference into the current context.""" - AutoAction.AutoReference(name=auto_name).add() - -def startShooting() -> None: - """Insert a StartShootingAction into the current context.""" - AutoAction.StartShooting().add() - -def stopShooting() -> None: - """Insert a StopShootingAction into the current context.""" - AutoAction.StopShooting().add() - -def followPath(path_name, mirror_path=False, can_mirror=True): - AutoAction.FollowPathPlannerPath(path_name=path_name, mirror_path=mirror_path, can_mirror=can_mirror).add() - -def networkConfigurableWait(name: str, default_wait: Optional[Measure]=None): - AutoAction.NetworkConfigurableWait(name, default_wait).add() diff --git a/auto_generator/src/units.py b/auto_generator/src/units.py deleted file mode 100644 index 0ebf3e28..00000000 --- a/auto_generator/src/units.py +++ /dev/null @@ -1,122 +0,0 @@ -""" -Auto-generated by PythonGenerator. Do not edit manually. - -Provides unit types and measure creation functions that serialize -to the same JSON format the Java robot code expects. -""" - -from __future__ import annotations - -from dataclasses import dataclass - - -@dataclass -class Measure: - """A measure with a numeric value and a unit name string.""" - value: float - unit: str - - def to_dict(self) -> dict: - return {"value": self.value, "unit": self.unit} - - -@dataclass -class Unit: - """A named unit that can create Measure instances.""" - name: str - - def of(self, value: float) -> Measure: - return Measure(value=value, unit=self.name) - - -# AngularVelocityPerTime unit type -Rotation_per_Second_per_Second = Unit("Rotation per Second per Second") -# CurrentPerTime unit type -Amp_per_Second = Unit("Amp per Second") -# Voltage unit type -Volt = Unit("Volt") -# LinearVelocityPerTime unit type -Meter_per_Second_per_Second = Unit("Meter per Second per Second") -# Time unit type -Minute = Unit("Minute") -Foot_per_Second_per_Second = Unit("Foot per Second per Second") -# AnglePerTime unit type -Degree_per_Second = Unit("Degree per Second") -# Torque unit type -Inch_Pound_force = Unit("Inch-Pound-force") -Radian_per_Second = Unit("Radian per Second") -# VoltagePerLinearAcceleration unit type -Volt_per_Meter_per_Second_per_Second = Unit("Volt per Meter per Second per Second") -# DimensionlessPerTime unit type -Hertz = Unit("Hertz") -# Distance unit type -Centimeter = Unit("Centimeter") -Inch_Ounce_force = Unit("Inch-Ounce-force") -# DistancePerTime unit type -Inch_per_Second = Unit("Inch per Second") -Meter_Newton = Unit("Meter-Newton") -Degree_per_Second_per_Second = Unit("Degree per Second per Second") -# EnergyPerTime unit type -Watt = Unit("Watt") -Rotation_per_Second = Unit("Rotation per Second") -# Current unit type -Amp = Unit("Amp") -# Force unit type -Newton = Unit("Newton") -# Temperature unit type -Celsius = Unit("Celsius") -Fahrenheit = Unit("Fahrenheit") -G = Unit("G") -Inch = Unit("Inch") -Foot_per_Second = Unit("Foot per Second") -# Energy unit type -Joule = Unit("Joule") -Kelvin = Unit("Kelvin") -Rotation_per_Minute = Unit("Rotation per Minute") -# LinearMomentum unit type -Kilogram_Meter_per_Second = Unit("Kilogram-Meter per Second") -# VoltagePerCurrent unit type -Kiloohm = Unit("Kiloohm") -# Mass unit type -Gram = Unit("Gram") -Millimeter = Unit("Millimeter") -Millihertz = Unit("Millihertz") -Millijoule = Unit("Millijoule") -# Angle unit type -Degree = Unit("Degree") -Rotation = Unit("Rotation") -Foot_Pound_force = Unit("Foot-Pound-force") -Microsecond = Unit("Microsecond") -Radian_per_Second_per_Second = Unit("Radian per Second per Second") -Revolution_per_Second = Unit("Revolution per Second") -Millivolt = Unit("Millivolt") -Rotation_per_Minute_per_Second = Unit("Rotation per Minute per Second") -Revolution = Unit("Revolution") -Kilogram = Unit("Kilogram") -Kilojoule = Unit("Kilojoule") -Radian = Unit("Radian") -Milliwatt = Unit("Milliwatt") -Foot = Unit("Foot") -# AngularMomentum unit type -Kilogram_Meter_per_Second_Meter = Unit("Kilogram-Meter per Second-Meter") -# AngularMomentumPerAngularVelocity unit type -Kilogram_Meter_per_Second_Meter_per_Radian_per_Second = Unit("Kilogram-Meter per Second-Meter per Radian per Second") -Milliohm = Unit("Milliohm") -Volt_per_Radian_per_Second_per_Second = Unit("Volt per Radian per Second per Second") -Volt_per_Radian_per_Second = Unit("Volt per Radian per Second") -# Dimensionless unit type -Percent = Unit("Percent") -Millisecond = Unit("Millisecond") -Pound = Unit("Pound") -Second = Unit("Second") -Meter = Unit("Meter") -Horsepower = Unit("Horsepower") -Meter_per_Second = Unit("Meter per Second") -Milliamp = Unit("Milliamp") -Ounce = Unit("Ounce") -Unitless = Unit("") -Ohm = Unit("Ohm") -Ounce_force = Unit("Ounce-force") -Pound_force = Unit("Pound-force") -Rotation_per_Second_per_Second_per_Second = Unit("Rotation per Second per Second per Second") -Volt_per_Meter_per_Second = Unit("Volt per Meter per Second") diff --git a/ci-scripts/check_auto_roundtrip.py b/ci-scripts/check_auto_roundtrip.py index c3eac289..1eee0d6f 100644 --- a/ci-scripts/check_auto_roundtrip.py +++ b/ci-scripts/check_auto_roundtrip.py @@ -129,7 +129,32 @@ def run_build_autos() -> None: print(line, flush=True) +def bootstrap_generator() -> None: + """Compile robot classes + dump the classpath for the Java auto generator. + + Done before launching simulateJava so the generator never has to invoke gradle + concurrently with the running simulation. build_autos.py --no-file --bootstrap + runs the full generator pipeline once (gradle compile + classpath dump + javac), + discarding the output. + """ + print("Bootstrapping Java auto generator (compile + classpath dump)...", flush=True) + result = subprocess.run( + [sys.executable, "auto_generator/build_autos.py", "--no-file", "--bootstrap"], + cwd=ROOT, + text=True, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + timeout=PUBLISH_TIMEOUT_SECONDS * 4, + check=False, + ) + if result.returncode != 0: + print(result.stdout, end="") + raise RuntimeError(f"generator bootstrap failed with status {result.returncode}") + + def main() -> int: + bootstrap_generator() + lines: "queue.Queue[str]" = queue.Queue() process = subprocess.Popen( ["./gradlew", "--console=plain", "simulateJava"], diff --git a/src/main/deploy/constants/comp/Autos.json b/src/main/deploy/constants/comp/Autos.json index 1b9a404c..154634e0 100644 --- a/src/main/deploy/constants/comp/Autos.json +++ b/src/main/deploy/constants/comp/Autos.json @@ -1,3312 +1,3312 @@ { - "autos": { - "Literally just shoot Auto": { - "rootAction": { - "type": "Sequence", + "autos": { + "Aggressive Depot From Bump": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "type": "StartShooting" + }, + { + "delay": { + "value": 1.5, + "unit": "Second" + }, + "type": "Wait" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 3.5, + "y": 7.55 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + } + ], + "type": "Sequence" + }, + { + "type": "StopShooting" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 5.1 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 200.0, + "jerk": 0.0 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Aggressive Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { "actions": [ - { + { + "actions": [ + { + "delay": { + "value": 0.6, + "unit": "Second" + }, + "type": "Wait" + }, + { "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 } - ] - }, - "canBeMirrored": false, - "shouldBeFlipped": false - }, - "Aggressive Depot": { - "rootAction": { - "type": "Sequence", + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 5.1 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 200.0, - "jerk": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Aggressive Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 3.5, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StopShooting" - }, - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 4.0, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Trench To Center Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Close 2nd Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.4, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -180 - }, - "translation": { - "x": 1.5, - "y": 5.9 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] - } - ] + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -180.0 + }, + "translation": { + "x": 1.5, + "y": 5.9 } - ] - }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Aggressive No Depot": { - "rootAction": { - "type": "Sequence", + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 5.1 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 200.0, - "jerk": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Aggressive Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 3.5, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StopShooting" - }, - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 4.0, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Trench To Center Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Close 2nd Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.4, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 1, - "unit": "Second" - } - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] - } - ] + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Literally just shoot Auto": { + "rootAction": { + "actions": [ + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + "canBeMirrored": false, + "shouldBeFlipped": false + }, + "Aggressive No Depot": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 5.2, + "y": 7.4 } - ] - }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Aggressive Depot From Bump": { - "rootAction": { - "type": "Sequence", + }, + "velocity": 5.1 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 200.0, + "jerk": 0.0 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "StartShooting" - }, - { - "type": "Wait", - "delay": { - "value": 1.5, - "unit": "Second" - } - }, - { - "type": "Sequence", - "actions": [ - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 3.5, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - } - ] - }, - { - "type": "StopShooting" - }, - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 5.1 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 200.0, - "jerk": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Aggressive Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -180 - }, - "translation": { - "x": 1.5, - "y": 5.9 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Aggressive Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.6, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 3.5, + "y": 7.55 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StopShooting" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] + "translation": { + "x": 4.0, + "y": 7.55 } - ] + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "pathName": "Left Trench To Center Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Sequence" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Close 2nd Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.4, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 } - ] - }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Conservative Depot": { - "rootAction": { - "type": "Sequence", + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 1.0, + "unit": "Second" + }, + "type": "Wait" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 0.0 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Starting Position Left Trench To Center", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Conservative Sweep", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 2.5, - "unit": "Second" - } - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 3.5, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StopShooting" - }, - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 4.0, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Trench To Center Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Close 2nd Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.4, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -180 - }, - "translation": { - "x": 1.5, - "y": 5.9 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Conservative No Depot": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": 0.0 + }, + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "pathName": "Starting Position Left Trench To Center", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Conservative Sweep", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.6, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 2.5, + "unit": "Second" + }, + "type": "Wait" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 3.5, + "y": 7.55 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StopShooting" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] + "translation": { + "x": 4.0, + "y": 7.55 } - ] + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "pathName": "Left Trench To Center Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Sequence" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Close 2nd Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.4, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 1.0, + "unit": "Second" + }, + "type": "Wait" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Center Depot": { + "rootAction": { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "type": "StartShooting" + }, + { + "name": "Center Depot - Shoot Preload", + "defaultDelay": { + "value": 4.0, + "unit": "Second" + }, + "type": "NetworkConfigurableWait" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 1.5, + "y": 5.1 + } + }, + "velocity": 1.0 + }, + "profile": { + "constraints": { + "velocity": 5.1, + "acceleration": 10.0, + "jerk": 3.0 + }, + "errorXY": { + "value": 0.1, + "unit": "Meter" + }, + "errorTheta": { + "value": 4.0, + "unit": "Degree" + }, + "beelineRadius": { + "value": 0.2, + "unit": "Meter" + } + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 0.715, + "y": 5.1 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 1.0, + "acceleration": 3.0, + "jerk": 3.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 0.715, + "y": 6.4 } - ] + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 1.0, + "acceleration": 3.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 1.0, + "y": 6.8 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 5.0, + "jerk": 3.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + } + ], + "type": "Sequence" + }, + { + "delay": { + "value": 2.0, + "unit": "Second" }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Conservative No Depot": { - "rootAction": { - "type": "Sequence", + "type": "Wait" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": false, + "shouldBeFlipped": true + }, + "Aggressive Depot": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 5.1 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 200.0, + "jerk": 0.0 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 0.0 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Starting Position Left Trench To Center", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Conservative Sweep", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 2.5, - "unit": "Second" - } - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 3.5, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StopShooting" - }, - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 4.0, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Trench To Center Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Close 2nd Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.4, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 1, - "unit": "Second" - } + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Aggressive Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.6, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 3.5, + "y": 7.55 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StopShooting" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] + "translation": { + "x": 4.0, + "y": 7.55 } - ] + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "pathName": "Left Trench To Center Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Sequence" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Close 2nd Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.4, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 } - ] - }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Conservative Depot From Bump": { - "rootAction": { - "type": "Sequence", + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 1.0, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -180.0 + }, + "translation": { + "x": 1.5, + "y": 5.9 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "StartShooting" - }, - { - "type": "Wait", - "delay": { - "value": 1.5, - "unit": "Second" - } - }, - { - "type": "Sequence", - "actions": [ - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 0 - }, - "translation": { - "x": 3.5, - "y": 7.55 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 4.2 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - } - ] - }, - { - "type": "StopShooting" - }, - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 0.0 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 4.2 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Starting Position Left Trench To Center", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Conservative Sweep", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 2.5, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -180 - }, - "translation": { - "x": 1.5, - "y": 5.9 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Conservative Depot": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": 0.0 + }, + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "pathName": "Starting Position Left Trench To Center", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Conservative Sweep", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.6, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 2.5, + "unit": "Second" + }, + "type": "Wait" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 3.5, + "y": 7.55 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StopShooting" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] + "translation": { + "x": 4.0, + "y": 7.55 } - ] + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "pathName": "Left Trench To Center Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Sequence" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Close 2nd Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { + "delay": { + "value": 0.4, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 1.0, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -180.0 + }, + "translation": { + "x": 1.5, + "y": 5.9 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Follower": { + "rootAction": { + "actions": [ + { + "name": "Follower - Preload", + "defaultDelay": { + "value": 2.5, + "unit": "Second" + }, + "type": "NetworkConfigurableWait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 2.0 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 200.0, + "jerk": 0.0 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Follower Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + }, + { + "name": "Follower - Before Bump Return", + "defaultDelay": { + "value": 0.0, + "unit": "Second" + }, + "type": "NetworkConfigurableWait" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 1.5, + "y": 5.1 + } + }, + "velocity": 0.0 + }, + "profile": { + "constraints": { + "velocity": 5.1, + "acceleration": 10.0, + "jerk": 3.0 + }, + "errorXY": { + "value": 0.1, + "unit": "Meter" + }, + "errorTheta": { + "value": 4.0, + "unit": "Degree" + }, + "beelineRadius": { + "value": 0.2, + "unit": "Meter" + } + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "type": "StartShooting" + }, + { + "name": "Follower - Before Depot", + "defaultDelay": { + "value": 0.0, + "unit": "Second" + }, + "type": "NetworkConfigurableWait" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 1.5, + "y": 5.1 + } + }, + "velocity": 1.0 + }, + "profile": { + "constraints": { + "velocity": 5.1, + "acceleration": 10.0, + "jerk": 3.0 + }, + "errorXY": { + "value": 0.1, + "unit": "Meter" + }, + "errorTheta": { + "value": 4.0, + "unit": "Degree" + }, + "beelineRadius": { + "value": 0.2, + "unit": "Meter" + } + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 0.715, + "y": 5.1 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 1.0, + "acceleration": 3.0, + "jerk": 3.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 0.715, + "y": 6.4 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 1.0, + "acceleration": 3.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 1.0, + "y": 6.8 } - ] + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 5.0, + "jerk": 3.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + } + ], + "type": "Sequence" + }, + { + "delay": { + "value": 2.0, + "unit": "Second" }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Follower": { - "rootAction": { - "type": "Sequence", + "type": "Wait" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Conservative Depot From Bump": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "type": "StartShooting" + }, + { + "delay": { + "value": 1.5, + "unit": "Second" + }, + "type": "Wait" + }, + { "actions": [ - { - "type": "NetworkConfigurableWait", - "name": "Follower - Preload", - "defaultDelay": { - "value": 2.5, - "unit": "Second" + { + "target": { + "reference": { + "rotation": { + "degrees": 0.0 + }, + "translation": { + "x": 3.5, + "y": 7.55 } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 4.2 }, - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 2.0 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 200.0, - "jerk": 0.0 - }, - "canMirror": true + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 }, - { - "type": "DeployIntakeAction" + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Follower Sweep Intake In", - "mirrorPath": false, - "canMirror": true - }, - { - "type": "NetworkConfigurableWait", - "name": "Follower - Before Bump Return", - "defaultDelay": { - "value": 0.0, - "unit": "Second" - } + "canMirror": true, + "type": "AutoPilotAction" + } + ], + "type": "Sequence" + }, + { + "type": "StopShooting" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 0.0 }, - { - "type": "Wait", + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 4.2 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "pathName": "Starting Position Left Trench To Center", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Conservative Sweep", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 + "value": 0.6, + "unit": "Second" }, - "canMirror": true + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.5, - "y": 5.1 - } - }, - "velocity": 0.0 - }, - "profile": { - "constraints": { - "velocity": 5.1, - "acceleration": 10.0, - "jerk": 3.0 - }, - "errorXY": { - "value": 0.1, - "unit": "Meter" - }, - "errorTheta": { - "value": 4.0, - "unit": "Degree" - }, - "beelineRadius": { - "value": 0.2, - "unit": "Meter" - } - }, - "canMirror": true + "translation": { + "x": 2.7, + "y": 5.75 + } + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 2.5, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -180.0 }, - { - "type": "StartShooting" + "translation": { + "x": 1.5, + "y": 5.9 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "actions": [ + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" }, - { - "type": "NetworkConfigurableWait", - "name": "Follower - Before Depot", - "defaultDelay": { - "value": 0.0, - "unit": "Second" - } + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" }, - { - "type": "Sequence", - "actions": [ - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.5, - "y": 5.1 - } - }, - "velocity": 1.0 - }, - "profile": { - "constraints": { - "velocity": 5.1, - "acceleration": 10.0, - "jerk": 3.0 - }, - "errorXY": { - "value": 0.1, - "unit": "Meter" - }, - "errorTheta": { - "value": 4.0, - "unit": "Degree" - }, - "beelineRadius": { - "value": 0.2, - "unit": "Meter" - } - }, - "canMirror": true - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 0.715, - "y": 5.1 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 1.0, - "acceleration": 3.0, - "jerk": 3.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 0.715, - "y": 6.4 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 1.0, - "acceleration": 3.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.0, - "y": 6.8 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 5.0, - "jerk": 3.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - } - ] + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": true, + "shouldBeFlipped": true + }, + "Single Swipe Then Depot": { + "rootAction": { + "actions": [ + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "Wait", + "translation": { + "x": 5.2, + "y": 7.4 + } + }, + "velocity": 5.1 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 200.0, + "jerk": 0.0 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" + }, + { + "actions": [ + { + "type": "DeployIntakeAction" + }, + { + "pathName": "Left Side Aggressive Sweep Intake In", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "actions": [ + { + "actions": [ + { "delay": { - "value": 2.0, - "unit": "Second" - } + "value": 0.6, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StartShooting" + } + ], + "type": "Sequence" + }, + { + "pathName": "Left Bump To Alliance", + "mirrorPath": false, + "canMirror": true, + "type": "FollowPathPlannerPath" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": -90.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] + "translation": { + "x": 2.7, + "y": 5.75 } - ] - }, - "canBeMirrored": true, - "shouldBeFlipped": true - }, - "Single Swipe Then Depot": { - "rootAction": { - "type": "Sequence", + }, + "velocity": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 5.2, - "y": 7.4 - } - }, - "velocity": 5.1 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 200.0, - "jerk": 0.0 - }, - "canMirror": true - }, - { - "type": "Parallel", - "actions": [ - { - "type": "DeployIntakeAction" - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Side Aggressive Sweep Intake In", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "Wait", - "delay": { - "value": 0.6, - "unit": "Second" - } - }, - { - "type": "StartShooting" - } - ] - }, - { - "type": "FollowPathPlannerPath", - "pathName": "Left Bump To Alliance", - "mirrorPath": false, - "canMirror": true - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90 - }, - "translation": { - "x": 2.7, - "y": 5.75 - } - }, - "velocity": 0.0 - }, - "canMirror": true - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.25, - "unit": "Second" - } - } - ] - }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] - } - ] - }, - { - "type": "Sequence", - "actions": [ - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.5, - "y": 5.1 - } - }, - "velocity": 1.0 - }, - "profile": { - "constraints": { - "velocity": 5.1, - "acceleration": 10.0, - "jerk": 3.0 - }, - "errorXY": { - "value": 0.1, - "unit": "Meter" - }, - "errorTheta": { - "value": 4.0, - "unit": "Degree" - }, - "beelineRadius": { - "value": 0.2, - "unit": "Meter" - } - }, - "canMirror": true - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 0.715, - "y": 5.1 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 1.0, - "acceleration": 3.0, - "jerk": 3.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 0.715, - "y": 6.4 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 1.0, - "acceleration": 3.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.0, - "y": 6.8 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 5.0, - "jerk": 3.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - } - ] + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" }, - { - "type": "Wait", - "delay": { - "value": 1.0, - "unit": "Second" - } + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] - } - ] - }, - "canBeMirrored": false, - "shouldBeFlipped": true - }, - "Center Depot": { - "rootAction": { - "type": "Sequence", + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.25, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + }, + { "actions": [ - { - "type": "DeployIntakeAction" + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" }, - { - "type": "StartShooting" + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" }, - { - "type": "NetworkConfigurableWait", - "name": "Center Depot - Shoot Preload", - "defaultDelay": { - "value": 4.0, - "unit": "Second" - } + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + { + "actions": [ + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.5, - "y": 5.1 - } - }, - "velocity": 1.0 - }, - "profile": { - "constraints": { - "velocity": 5.1, - "acceleration": 10.0, - "jerk": 3.0 - }, - "errorXY": { - "value": 0.1, - "unit": "Meter" - }, - "errorTheta": { - "value": 4.0, - "unit": "Degree" - }, - "beelineRadius": { - "value": 0.2, - "unit": "Meter" - } - }, - "canMirror": true - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 0.715, - "y": 5.1 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 1.0, - "acceleration": 3.0, - "jerk": 3.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 0.715, - "y": 6.4 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 1.0, - "acceleration": 3.0, - "jerk": 2.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - }, - { - "type": "Wait", - "delay": { - "value": 0.1, - "unit": "Second" - } - }, - { - "type": "AutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": 135 - }, - "translation": { - "x": 1.0, - "y": 6.8 - } - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 5.1, - "acceleration": 5.0, - "jerk": 3.0 - }, - "pidGains": { - "kP": 1.5, - "kI": 0.0, - "kD": 0.0, - "kS": 0.0, - "kG": 0.0, - "kV": 0.0, - "kA": 0.0 - }, - "canMirror": true - } - ] + "translation": { + "x": 1.5, + "y": 5.1 + } + }, + "velocity": 1.0 + }, + "profile": { + "constraints": { + "velocity": 5.1, + "acceleration": 10.0, + "jerk": 3.0 + }, + "errorXY": { + "value": 0.1, + "unit": "Meter" + }, + "errorTheta": { + "value": 4.0, + "unit": "Degree" + }, + "beelineRadius": { + "value": 0.2, + "unit": "Meter" + } + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 }, - { - "type": "Wait", - "delay": { - "value": 2.0, - "unit": "Second" - } + "translation": { + "x": 0.715, + "y": 5.1 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 1.0, + "acceleration": 3.0, + "jerk": 3.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 }, - { - "type": "Sequence", - "actions": [ - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "StowIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - }, - { - "type": "DeployIntakeAction" - }, - { - "type": "Wait", - "delay": { - "value": 0.16666666666666666, - "unit": "Second" - } - } - ] + "translation": { + "x": 0.715, + "y": 6.4 + } + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 1.0, + "acceleration": 3.0, + "jerk": 2.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + }, + { + "delay": { + "value": 0.1, + "unit": "Second" + }, + "type": "Wait" + }, + { + "target": { + "reference": { + "rotation": { + "degrees": 135.0 + }, + "translation": { + "x": 1.0, + "y": 6.8 } - ] + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 5.1, + "acceleration": 5.0, + "jerk": 3.0 + }, + "pidGains": { + "kP": 1.5, + "kI": 0.0, + "kD": 0.0, + "kS": 0.0, + "kG": 0.0, + "kV": 0.0, + "kA": 0.0 + }, + "canMirror": true, + "type": "AutoPilotAction" + } + ], + "type": "Sequence" + }, + { + "delay": { + "value": 1.0, + "unit": "Second" }, - "canBeMirrored": false, - "shouldBeFlipped": true - } - }, - "routines": { - "LeftClimb": { - "type": "Sequence", + "type": "Wait" + }, + { "actions": [ - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90.0 - }, - "translation": { - "x": 1.515154, - "y": 4.1813182 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 0.1 - }, - "canMirror": true - } - ] - }, - { - "type": "ClimbSearchAction" - } - ] + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" }, - { - "type": "Wait", - "delay": { - "value": 0.5, - "unit": "Second" - } + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "StowIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "DeployIntakeAction" + }, + { + "delay": { + "value": 0.16666666666666666, + "unit": "Second" + }, + "type": "Wait" + } + ], + "type": "Sequence" + } + ], + "type": "Sequence" + }, + "canBeMirrored": false, + "shouldBeFlipped": true + } + }, + "routines": { + "RightClimb": { + "actions": [ + { + "actions": [ + { + "actions": [ { - "type": "ClimbHangAction" + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 1.515154, + "y": 3.3395875999999998 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 0.1 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" } - ] + ], + "type": "Sequence" + }, + { + "type": "ClimbSearchAction" + } + ], + "type": "Parallel" }, - "RightClimb": { - "type": "Sequence", - "actions": [ - { - "type": "Parallel", - "actions": [ - { - "type": "Sequence", - "actions": [ - { - "type": "XBasedAutoPilotAction", - "target": { - "reference": { - "rotation": { - "degrees": -90.0 - }, - "translation": { - "x": 1.515154, - "y": 3.3240682 - } - }, - "entryAngle": { - "degrees": 0 - }, - "velocity": 0.0 - }, - "constraints": { - "velocity": 2.0, - "acceleration": 2.0, - "jerk": 0.1 - }, - "canMirror": true - } - ] - }, - { - "type": "ClimbSearchAction" - } - ] - }, - { - "type": "Wait", - "delay": { - "value": 0.5, - "unit": "Second" - } - }, + { + "delay": { + "value": 0.5, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "ClimbHangAction" + } + ], + "type": "Sequence" + }, + "LeftClimb": { + "actions": [ + { + "actions": [ + { + "actions": [ { - "type": "ClimbHangAction" + "target": { + "reference": { + "rotation": { + "degrees": -90.0 + }, + "translation": { + "x": 1.515154, + "y": 4.196837599999999 + } + }, + "entryAngle": { + "degrees": 0.0 + }, + "velocity": 0.0 + }, + "constraints": { + "velocity": 2.0, + "acceleration": 2.0, + "jerk": 0.1 + }, + "canMirror": true, + "type": "XBasedAutoPilotAction" } - ] + ], + "type": "Sequence" + }, + { + "type": "ClimbSearchAction" + } + ], + "type": "Parallel" + }, + { + "delay": { + "value": 0.5, + "unit": "Second" + }, + "type": "Wait" + }, + { + "type": "ClimbHangAction" } + ], + "type": "Sequence" } -} \ No newline at end of file + } +} diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 0cc8fead..6b5a6ff2 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -2,14 +2,23 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.auto.AutoAction.AutoActionContext; -import frc.robot.util.ts.GeneratedOptional; public class Auto { private AutoAction rootAction; - @GeneratedOptional private boolean canBeMirrored = true; - @GeneratedOptional private boolean shouldBeFlipped = true; + private boolean canBeMirrored = true; + private boolean shouldBeFlipped = true; + + /** No-arg constructor retained for JSON deserialization. */ + public Auto() {} + + /** Authoring constructor used by the Java auto generator. */ + public Auto(AutoAction rootAction, boolean canBeMirrored, boolean shouldBeFlipped) { + this.rootAction = rootAction; + this.canBeMirrored = canBeMirrored; + this.shouldBeFlipped = shouldBeFlipped; + } public Command toCommand(AutoActionContext data) { return rootAction.toCommand(data); diff --git a/src/main/java/frc/robot/auto/AutoAction.java b/src/main/java/frc/robot/auto/AutoAction.java index d8bf9fe7..bbbc4662 100644 --- a/src/main/java/frc/robot/auto/AutoAction.java +++ b/src/main/java/frc/robot/auto/AutoAction.java @@ -23,34 +23,9 @@ import frc.robot.auto.general.Sequence; import frc.robot.auto.general.Wait; import frc.robot.subsystems.drive.DriveCoordinator; -import frc.robot.util.ts.PythonAppend; -import frc.robot.util.ts.PythonMethod; +import java.util.HashMap; +import java.util.Map; -@PythonAppend( - value = - "\n_add_command_hook: Callable[[Any], None] | None = None\n\n\n" - + "def set_add_command_hook(hook: Callable[[Any], None]) -> None:\n" - + " global _add_command_hook\n" - + " _add_command_hook = hook\n\n\n" - + "def _call_hook(obj: Any) -> None:\n" - + " if _add_command_hook is not None:\n" - + " _add_command_hook(obj)\n\n\n" - + "def _to_dict(obj: Any) -> Any:\n" - + " \"\"\"Recursively convert an object to a JSON-serializable dict.\"\"\"\n" - + " if obj is None:\n" - + " return None\n" - + " if hasattr(obj, 'to_dict'):\n" - + " return obj.to_dict()\n" - + " if isinstance(obj, list):\n" - + " return [_to_dict(item) for item in obj]\n" - + " if isinstance(obj, dict):\n" - + " return {k: _to_dict(v) for k, v in obj.items()}\n" - + " return obj\n") -@PythonMethod( - name = "add", - returnType = "self", - body = {"_call_hook(self)", "return self"}, - comment = "Adds this command to the current auto and returns itself for chaining.") @JsonType( property = "type", subtypes = { @@ -78,7 +53,25 @@ }) public abstract class AutoAction { - public String type; + /** Maps each concrete AutoAction class to its JSON discriminator, from the @JsonType table. */ + private static final Map, String> TYPE_NAMES = buildTypeNames(); + + private static Map, String> buildTypeNames() { + Map, String> names = new HashMap<>(); + for (JsonSubtype subtype : AutoAction.class.getAnnotation(JsonType.class).subtypes()) { + names.put(subtype.clazz(), subtype.name()); + } + return names; + } + + /** + * The polymorphic discriminator written to JSON. coppercore's adapter uses @JsonType/@JsonSubtype + * only to pick the subclass when *deserializing*; on *serialize* it just emits this field. Gson + * populates it from the JSON when loading (bypassing this initializer via Unsafe), so this + * default only matters when an action is constructed directly in Java (e.g. the auto generator), + * where it resolves the discriminator from the runtime class using the @JsonSubtype table above. + */ + public String type = TYPE_NAMES.get(getClass()); public record AutoActionContext( DriveCoordinator driveCoordinator, @@ -102,4 +95,34 @@ public AutoActionContext mirror() { } public abstract Command toCommand(AutoActionContext data); + + // --------------------------------------------------------------------------- + // Fluent combinators for composing actions when authoring autos. + // --------------------------------------------------------------------------- + + /** Returns a {@link Sequence} that runs this action, then {@code next} in order. */ + public Sequence andThen(AutoAction... next) { + AutoAction[] all = new AutoAction[next.length + 1]; + all[0] = this; + System.arraycopy(next, 0, all, 1, next.length); + return Sequence.of(all); + } + + /** + * Returns a {@link Sequence} that runs this action, then runs {@code parallelActions} together in + * parallel. Sugar for {@code andThen(Parallel.of(parallelActions))} — the parallel grouping is + * explicit, so there is no ambiguity about what runs alongside what. + */ + public Sequence andThenInParallel(AutoAction... parallelActions) { + return andThen(Parallel.of(parallelActions)); + } + + /** + * Returns a {@link Sequence} that runs this action, then races {@code raceActions} against each + * other (the group ends when the first finishes). Sugar for {@code + * andThen(Race.of(raceActions))}. + */ + public Sequence andThenRacing(AutoAction... raceActions) { + return andThen(Race.of(raceActions)); + } } diff --git a/src/main/java/frc/robot/auto/drive/AutoPilotAction.java b/src/main/java/frc/robot/auto/drive/AutoPilotAction.java index 1e5a5653..d0a95526 100644 --- a/src/main/java/frc/robot/auto/drive/AutoPilotAction.java +++ b/src/main/java/frc/robot/auto/drive/AutoPilotAction.java @@ -10,7 +10,6 @@ import frc.robot.auto.Autos; import frc.robot.constants.JsonConstants; import frc.robot.subsystems.drive.DriveCoordinatorCommands; -import frc.robot.util.ts.GeneratedOptional; import java.util.Objects; import java.util.Optional; @@ -18,9 +17,26 @@ public class AutoPilotAction extends DriveAutoAction { private APTarget target = null; // original target in blue field coordinates - @GeneratedOptional public APProfile profile = null; - @GeneratedOptional public APConstraints constraints = null; - @GeneratedOptional public PIDGains pidGains = null; + public APProfile profile = null; + public APConstraints constraints = null; + public PIDGains pidGains = null; + + /** No-arg constructor retained for JSON deserialization. */ + public AutoPilotAction() {} + + /** Authoring constructor used by the Java auto generator. */ + public AutoPilotAction( + APTarget target, + APConstraints constraints, + APProfile profile, + PIDGains pidGains, + boolean canMirror) { + this.target = target; + this.constraints = constraints; + this.profile = profile; + this.pidGains = pidGains; + this.canMirror = canMirror; + } private void ensureTargetAndPoseAreNotNull() { Objects.requireNonNull(target, "Target cannot be null for AutoPilotAction"); diff --git a/src/main/java/frc/robot/auto/drive/DriveAutoAction.java b/src/main/java/frc/robot/auto/drive/DriveAutoAction.java index bf764ec1..f7d5734a 100644 --- a/src/main/java/frc/robot/auto/drive/DriveAutoAction.java +++ b/src/main/java/frc/robot/auto/drive/DriveAutoAction.java @@ -1,12 +1,10 @@ package frc.robot.auto.drive; import frc.robot.auto.AutoAction; -import frc.robot.util.ts.GeneratedDefault; public abstract class DriveAutoAction extends AutoAction { // Just tells us if we can mirror for things // Because for things like climb we can not mirror the climb poses - @GeneratedDefault("True") public boolean canMirror = true; } diff --git a/src/main/java/frc/robot/auto/drive/XBasedAutoPilotAction.java b/src/main/java/frc/robot/auto/drive/XBasedAutoPilotAction.java index e0824012..c5d726ce 100644 --- a/src/main/java/frc/robot/auto/drive/XBasedAutoPilotAction.java +++ b/src/main/java/frc/robot/auto/drive/XBasedAutoPilotAction.java @@ -1,11 +1,28 @@ package frc.robot.auto.drive; +import com.therekrab.autopilot.APConstraints; +import com.therekrab.autopilot.APProfile; +import com.therekrab.autopilot.APTarget; import com.therekrab.autopilot.Autopilot; +import coppercore.wpilib_interface.tuning.PIDGains; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.DriveCoordinatorCommands; public class XBasedAutoPilotAction extends AutoPilotAction { + /** No-arg constructor retained for JSON deserialization. */ + public XBasedAutoPilotAction() {} + + /** Authoring constructor used by the Java auto generator. */ + public XBasedAutoPilotAction( + APTarget target, + APConstraints constraints, + APProfile profile, + PIDGains pidGains, + boolean canMirror) { + super(target, constraints, profile, pidGains, canMirror); + } + @Override public Command toCommand(AutoActionContext context) { var realTarget = prepareAutoAction(context); diff --git a/src/main/java/frc/robot/auto/general/NetworkConfigurableWait.java b/src/main/java/frc/robot/auto/general/NetworkConfigurableWait.java index db16f1a6..24654a61 100644 --- a/src/main/java/frc/robot/auto/general/NetworkConfigurableWait.java +++ b/src/main/java/frc/robot/auto/general/NetworkConfigurableWait.java @@ -25,6 +25,15 @@ public class NetworkConfigurableWait extends AutoAction { private Time defaultDelay; @JSONExclude private LoggedNetworkNumber waitTimeSeconds; + /** No-arg constructor retained for JSON deserialization. */ + public NetworkConfigurableWait() {} + + /** Authoring constructor used by the Java auto generator. */ + public NetworkConfigurableWait(String name, Time defaultDelay) { + this.name = name; + this.defaultDelay = defaultDelay; + } + @JSONExclude private static final Map nameToLoggedNetworkNumber = new HashMap<>(); diff --git a/src/main/java/frc/robot/auto/general/Parallel.java b/src/main/java/frc/robot/auto/general/Parallel.java index f51e5071..17b672bb 100644 --- a/src/main/java/frc/robot/auto/general/Parallel.java +++ b/src/main/java/frc/robot/auto/general/Parallel.java @@ -3,11 +3,24 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.auto.AutoAction; +import java.util.List; import java.util.Objects; import java.util.stream.Stream; public class Parallel extends AutoAction { - public AutoAction[] actions; + private AutoAction[] actions; + + /** Creates a Parallel of the given actions. */ + public static Parallel of(AutoAction... actions) { + Parallel parallel = new Parallel(); + parallel.actions = actions; + return parallel; + } + + /** Creates a Parallel from a list of actions (convenient for conditional building). */ + public static Parallel of(List actions) { + return of(actions.toArray(new AutoAction[0])); + } @Override public Command toCommand(AutoActionContext data) { diff --git a/src/main/java/frc/robot/auto/general/Race.java b/src/main/java/frc/robot/auto/general/Race.java index 4639775c..77e0331d 100644 --- a/src/main/java/frc/robot/auto/general/Race.java +++ b/src/main/java/frc/robot/auto/general/Race.java @@ -3,11 +3,24 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import frc.robot.auto.AutoAction; +import java.util.List; import java.util.Objects; import java.util.stream.Stream; public class Race extends AutoAction { - public AutoAction[] actions; + private AutoAction[] actions; + + /** Creates a Race of the given actions (all run until the first finishes). */ + public static Race of(AutoAction... actions) { + Race race = new Race(); + race.actions = actions; + return race; + } + + /** Creates a Race from a list of actions. */ + public static Race of(List actions) { + return of(actions.toArray(new AutoAction[0])); + } @Override public Command toCommand(AutoActionContext data) { diff --git a/src/main/java/frc/robot/auto/general/Sequence.java b/src/main/java/frc/robot/auto/general/Sequence.java index b277ef76..baa9e762 100644 --- a/src/main/java/frc/robot/auto/general/Sequence.java +++ b/src/main/java/frc/robot/auto/general/Sequence.java @@ -3,12 +3,34 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.auto.AutoAction; +import java.util.List; import java.util.Objects; import java.util.stream.Stream; public class Sequence extends AutoAction { - public AutoAction[] actions; + private AutoAction[] actions; + + /** Creates a Sequence of the given actions. */ + public static Sequence of(AutoAction... actions) { + Sequence sequence = new Sequence(); + sequence.actions = actions; + return sequence; + } + + /** Creates a Sequence from a list of actions (convenient for conditional building). */ + public static Sequence of(List actions) { + return of(actions.toArray(new AutoAction[0])); + } + + /** Appends {@code next} to this sequence, returning a new flattened Sequence. */ + @Override + public Sequence andThen(AutoAction... next) { + AutoAction[] all = new AutoAction[actions.length + next.length]; + System.arraycopy(actions, 0, all, 0, actions.length); + System.arraycopy(next, 0, all, actions.length, next.length); + return of(all); + } @Override public Command toCommand(AutoActionContext data) { diff --git a/src/main/java/frc/robot/autogen/Dsl.java b/src/main/java/frc/robot/autogen/Dsl.java new file mode 100644 index 00000000..28a05a00 --- /dev/null +++ b/src/main/java/frc/robot/autogen/Dsl.java @@ -0,0 +1,298 @@ +package frc.robot.autogen; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; + +import com.therekrab.autopilot.APConstraints; +import com.therekrab.autopilot.APProfile; +import com.therekrab.autopilot.APTarget; +import coppercore.wpilib_interface.tuning.PIDGains; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; +import frc.robot.auto.AutoAction; +import frc.robot.auto.coordinationLayer.ClimbHangAction; +import frc.robot.auto.coordinationLayer.ClimbSearchAction; +import frc.robot.auto.coordinationLayer.DeployIntakeAction; +import frc.robot.auto.coordinationLayer.StartShooting; +import frc.robot.auto.coordinationLayer.StopShooting; +import frc.robot.auto.coordinationLayer.StowIntakeAction; +import frc.robot.auto.drive.AutoPilotAction; +import frc.robot.auto.drive.FollowPathPlannerPath; +import frc.robot.auto.drive.StopDriveAction; +import frc.robot.auto.drive.XBasedAutoPilotAction; +import frc.robot.auto.general.AutoReference; +import frc.robot.auto.general.NetworkConfigurableWait; +import frc.robot.auto.general.Parallel; +import frc.robot.auto.general.Print; +import frc.robot.auto.general.Race; +import frc.robot.auto.general.Sequence; +import frc.robot.auto.general.Wait; + +/** + * Small authoring DSL for building auto routines in Java. Mirrors the helpers that used to live in + * the Python {@code shorthands.py}/{@code auto_lib.py}. Factory methods return plain {@link + * AutoAction} objects; sequencing is expressed with {@code seq.andThen(...)} (or {@code + * action.andThen(...)}), and parallel/race groups are built explicitly with the {@link #inParallel} + * / {@link #inRace} factories, e.g. {@code seq.andThen(a, inParallel(b, c))}. + */ +public final class Dsl { + private Dsl() {} + + // --------------------------------------------------------------------------- + // Geometry helpers + // --------------------------------------------------------------------------- + + public static Rotation2d rotation2d(double degrees) { + return Rotation2d.fromDegrees(degrees); + } + + public static Translation2d translation2d(double x, double y) { + return new Translation2d(x, y); + } + + public static Pose2d pose2d(double x, double y, double angleDegrees) { + return new Pose2d(x, y, Rotation2d.fromDegrees(angleDegrees)); + } + + public static Pose2d pose2d(double x, double y) { + return pose2d(x, y, 0.0); + } + + public static Transform2d transform2d(Translation2d translation, Rotation2d rotation) { + return new Transform2d(translation, rotation); + } + + // --------------------------------------------------------------------------- + // Units + // --------------------------------------------------------------------------- + + public static Time seconds(double value) { + return Seconds.of(value); + } + + public static Distance meters(double value) { + return Meters.of(value); + } + + public static Angle degrees(double value) { + return Degrees.of(value); + } + + // --------------------------------------------------------------------------- + // Autopilot building blocks + // --------------------------------------------------------------------------- + + public static APConstraints apConstraints(double velocity, double acceleration, double jerk) { + return new APConstraints(velocity, acceleration, jerk); + } + + public static APConstraints apConstraints(double velocity, double acceleration) { + // NB: the autopilot library's 2-arg constructor means (acceleration, jerk) with velocity + // unlimited. The Python generator's APConstraints(v, a) meant (velocity, acceleration) with + // jerk = 0, so build that explicitly via the 3-arg constructor. + return new APConstraints(velocity, acceleration, 0.0); + } + + public static APProfile apProfile( + APConstraints constraints, Distance errorXY, Angle errorTheta, Distance beelineRadius) { + return new APProfile(constraints) + .withErrorXY(errorXY) + .withErrorTheta(errorTheta) + .withBeelineRadius(beelineRadius); + } + + public static PIDGains pidGains(double kP) { + return PIDGains.kPID(kP, 0.0, 0.0); + } + + /** Starts building an autopilot action targeting the given pose. */ + public static Ap autoPilotTo(double x, double y, double angleDegrees) { + return new Ap(pose2d(x, y, angleDegrees)); + } + + /** Starts building an autopilot action targeting (x, y) with zero heading. */ + public static Ap autoPilotTo(double x, double y) { + return autoPilotTo(x, y, 0.0); + } + + /** Starts building an autopilot action targeting the given pose. */ + public static Ap autoPilotTo(Pose2d reference) { + return new Ap(reference); + } + + /** Fluent builder for {@link AutoPilotAction} / {@link XBasedAutoPilotAction}. */ + public static final class Ap { + private final Pose2d reference; + private Rotation2d entryAngle; + private double velocity = 0.0; + private Distance rotationRadius; + private APConstraints constraints; + private APProfile profile; + private PIDGains pidGains; + private boolean canMirror = true; + + private Ap(Pose2d reference) { + this.reference = reference; + } + + public Ap withVelocity(double velocity) { + this.velocity = velocity; + return this; + } + + public Ap withEntryAngle(double degrees) { + this.entryAngle = rotation2d(degrees); + return this; + } + + public Ap withEntryAngle(Rotation2d entryAngle) { + this.entryAngle = entryAngle; + return this; + } + + public Ap withRotationRadius(Distance rotationRadius) { + this.rotationRadius = rotationRadius; + return this; + } + + public Ap withConstraints(APConstraints constraints) { + this.constraints = constraints; + return this; + } + + public Ap withProfile(APProfile profile) { + this.profile = profile; + return this; + } + + public Ap withPidGains(PIDGains pidGains) { + this.pidGains = pidGains; + return this; + } + + public Ap withCanMirror(boolean canMirror) { + this.canMirror = canMirror; + return this; + } + + private APTarget target() { + APTarget target = new APTarget(reference); + if (entryAngle != null) { + target = target.withEntryAngle(entryAngle); + } + target = target.withVelocity(velocity); + if (rotationRadius != null) { + target = target.withRotationRadius(rotationRadius); + } + return target; + } + + public AutoPilotAction toAutoAction() { + return new AutoPilotAction(target(), constraints, profile, pidGains, canMirror); + } + + public XBasedAutoPilotAction toXBasedAutoAction() { + return new XBasedAutoPilotAction(target(), constraints, profile, pidGains, canMirror); + } + } + + // --------------------------------------------------------------------------- + // Primitive action shorthands + // --------------------------------------------------------------------------- + + public static AutoAction startShooting() { + return new StartShooting(); + } + + public static AutoAction stopShooting() { + return new StopShooting(); + } + + public static AutoAction deployIntake() { + return new DeployIntakeAction(); + } + + public static AutoAction stowIntake() { + return new StowIntakeAction(); + } + + public static AutoAction climbSearch() { + return new ClimbSearchAction(); + } + + public static AutoAction climbHang() { + return new ClimbHangAction(); + } + + public static AutoAction stopDrive() { + return new StopDriveAction(); + } + + public static AutoAction waitSeconds(double secondsValue) { + Wait wait = new Wait(); + wait.delay = seconds(secondsValue); + return wait; + } + + public static AutoAction print(String message) { + Print print = new Print(); + print.message = message; + return print; + } + + public static AutoAction reference(String autoName) { + AutoReference ref = new AutoReference(); + ref.name = autoName; + return ref; + } + + public static AutoAction networkConfigurableWait(String name, Time defaultDelay) { + return new NetworkConfigurableWait(name, defaultDelay); + } + + public static AutoAction followPath(String pathName) { + return followPath(pathName, false, true); + } + + public static AutoAction followPath(String pathName, boolean mirrorPath, boolean canMirror) { + FollowPathPlannerPath path = new FollowPathPlannerPath(); + path.pathName = pathName; + path.mirrorPath = mirrorPath; + path.canMirror = canMirror; + return path; + } + + // --------------------------------------------------------------------------- + // Containers + // + // Sequencing uses the fluent combinators on AutoAction; parallel/race groups are + // built explicitly with the inParallel(...) / inRace(...) factories so grouping is + // never ambiguous: + // seq.andThen(a, b, c) // Sequence of a, b, c + // seq.andThen(a, inParallel(b, c)) // a, then b and c together + // a.andThenInParallel(b, c) // sugar for a.andThen(inParallel(b, c)) + // inParallel(seq.andThen(a, b), c) // [a then b] alongside c + // `seq` is an empty starter; andThen returns a new (flattened) Sequence, so the + // shared starter is never mutated. For conditional building, accumulate with + // `result = result.andThen(...)`. + // --------------------------------------------------------------------------- + + /** Empty Sequence starter: {@code seq.andThen(...)}. */ + public static final Sequence seq = Sequence.of(); + + /** Builds a Parallel that runs the given actions together. */ + public static Parallel inParallel(AutoAction... actions) { + return Parallel.of(actions); + } + + /** Builds a Race of the given actions (the group ends when the first finishes). */ + public static Race inRace(AutoAction... actions) { + return Race.of(actions); + } +} diff --git a/src/main/java/frc/robot/autogen/Field.java b/src/main/java/frc/robot/autogen/Field.java new file mode 100644 index 00000000..0bb07462 --- /dev/null +++ b/src/main/java/frc/robot/autogen/Field.java @@ -0,0 +1,42 @@ +package frc.robot.autogen; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.JsonConstants; + +/** + * Sets up the robot environment the generator runs in and exposes the field-derived climb poses. + * + *

The generator runs under a JVM that has the WPILib native libraries on its library path (see + * generate.py), so it can use robot code that touches HAL/Filesystem just like the real robot. We + * initialize HAL and then run the robot's own constant-loading sequence ({@link + * JsonConstants#loadConstants()}), which makes every constant (FieldConstants, AprilTagConstants, + * drive constants, ...) available exactly as on the robot — no duplicated loading logic. + */ +public final class Field { + private Field() {} + + // climb_offset from the old constants.py: translate (0.41, 0.0225), rotate -90 degrees. + private static final Transform2d CLIMB_OFFSET = + new Transform2d(new Translation2d(0.41, 0.0225), Rotation2d.fromDegrees(-90.0)); + + /** Initializes HAL and loads all robot constants for the active (config.json) environment. */ + public static void initialize() { + HAL.initialize(500, 0); + JsonConstants.loadConstants(); + } + + public static Pose2d leftClimbLocation() { + return new Pose2d(FieldConstants.Tower.leftUpright(), new Rotation2d()) + .transformBy(CLIMB_OFFSET); + } + + public static Pose2d rightClimbLocation() { + return new Pose2d(FieldConstants.Tower.rightUpright(), new Rotation2d()) + .transformBy(CLIMB_OFFSET); + } +} diff --git a/src/main/java/frc/robot/autogen/GenerateAutos.java b/src/main/java/frc/robot/autogen/GenerateAutos.java new file mode 100644 index 00000000..5c9ae57d --- /dev/null +++ b/src/main/java/frc/robot/autogen/GenerateAutos.java @@ -0,0 +1,397 @@ +package frc.robot.autogen; + +import static frc.robot.autogen.Dsl.apConstraints; +import static frc.robot.autogen.Dsl.apProfile; +import static frc.robot.autogen.Dsl.autoPilotTo; +import static frc.robot.autogen.Dsl.climbHang; +import static frc.robot.autogen.Dsl.climbSearch; +import static frc.robot.autogen.Dsl.degrees; +import static frc.robot.autogen.Dsl.deployIntake; +import static frc.robot.autogen.Dsl.followPath; +import static frc.robot.autogen.Dsl.inParallel; +import static frc.robot.autogen.Dsl.meters; +import static frc.robot.autogen.Dsl.networkConfigurableWait; +import static frc.robot.autogen.Dsl.pidGains; +import static frc.robot.autogen.Dsl.pose2d; +import static frc.robot.autogen.Dsl.rotation2d; +import static frc.robot.autogen.Dsl.seconds; +import static frc.robot.autogen.Dsl.seq; +import static frc.robot.autogen.Dsl.startShooting; +import static frc.robot.autogen.Dsl.stopShooting; +import static frc.robot.autogen.Dsl.stowIntake; +import static frc.robot.autogen.Dsl.waitSeconds; + +import com.therekrab.autopilot.APConstraints; +import com.therekrab.autopilot.APTarget; +import coppercore.parameter_tools.json.JSONSync; +import coppercore.parameter_tools.json.JSONSyncConfig; +import coppercore.parameter_tools.json.JSONSyncConfigBuilder; +import coppercore.parameter_tools.json.helpers.JSONConverter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.auto.Auto; +import frc.robot.auto.AutoAction; +import frc.robot.auto.Autos; +import frc.robot.auto.general.Sequence; +import frc.robot.util.json.JSONAPTarget; + +/** + * Builds every auto routine in Java and serializes them to the Autos.json format using the robot's + * own coppercore Gson configuration. Replaces the former Python auto generator. + * + *

Sequencing uses {@code seq.andThen(...)} (or {@code action.andThen(...)}); parallel/race + * groups are built explicitly with {@code inParallel(...)} / {@code inRace(...)} so grouping is + * never ambiguous, e.g. {@code seq.andThen(a, inParallel(b, c))}. Conditional sequences accumulate + * with {@code result = result.andThen(...)} (andThen flattens, so this just appends). + * + *

Usage: {@code java frc.robot.autogen.GenerateAutos [outputFile]} (environment comes from the + * deploy constants/config.json). With no outputFile the JSON is printed to stdout. + */ +public final class GenerateAutos { + + // TODO: Add alliance-relative coordinate utilities. + // TODO: Replace placeholder coordinates with real field positions. + // TODO: Switch to AutoPilotAction with entry angle and exit velocity for trench segments. + + private static final double DEFAULT_TRENCH_VELOCITY = 4.2; + private static final double AGGRESSIVE_TRENCH_VELOCITY = 5.1; + private static final Pose2d LEFT_TRENCH_CENTER_SIDE_POSE = pose2d(5.2, 7.4, -90); + + // Shooting position in the middle of the left side of the alliance zone. + // This is where we end up after coming over the bump. + private static final Pose2d LEFT_ALLIANCE_ZONE_MIDDLE_POSE = pose2d(2.700, 5.75, -90); + + private static final APConstraints CLIMB_CONSTRAINTS = apConstraints(2.0, 2.0, 0.1); + + public static void main(String[] args) throws java.io.IOException { + // The JSON is written to this file rather than stdout: initializing HAL prints native + // diagnostics to stdout, which would otherwise corrupt the serialized output. + String outputFile = args.length > 0 ? args[0] : null; + + // Initialize HAL and load all robot constants (the environment is selected by the deploy + // constants/config.json, exactly as on the robot). + Field.initialize(); + + Autos autos = build(); + + JSONConverter.addConversion(APTarget.class, JSONAPTarget.class); + JSONSyncConfig config = new JSONSyncConfigBuilder().build(); + JSONSync sync = new JSONSync<>(autos, "", config); + String json = sync.serialize(); + + if (outputFile != null) { + java.nio.file.Files.writeString(java.nio.file.Path.of(outputFile), json); + } else { + System.out.println(json); + } + } + + private static Autos build() { + Autos autos = new Autos(); + + autos.autos.put( + "Literally just shoot Auto", new Auto(seq.andThen(startShooting()), false, false)); + + autos.autos.put("Aggressive Depot", auto(seq.andThen(aggressive(true, false, false, true)))); + autos.autos.put( + "Aggressive No Depot", auto(seq.andThen(aggressive(false, false, false, true)))); + autos.autos.put( + "Aggressive Depot From Bump", auto(seq.andThen(aggressive(true, true, true, false)))); + + autos.autos.put( + "Conservative Depot", auto(seq.andThen(conservative(true, false, false, true)))); + autos.autos.put( + "Conservative No Depot", auto(seq.andThen(conservative(false, false, false, true)))); + autos.autos.put( + "Conservative Depot From Bump", auto(seq.andThen(conservative(true, true, true, false)))); + + autos.autos.put("Follower", auto(follower())); + + autos.autos.put( + "Single Swipe Then Depot", + new Auto( + seq.andThen( + aggressive(false, false, false, false), + goToDepotAndIntake(), + waitSeconds(1.0), + cycleIntake(6.0 / 3.0, 6)), + false, + true)); + + autos.autos.put( + "Center Depot", + new Auto( + seq.andThen( + deployIntake(), + startShooting(), + networkConfigurableWait("Center Depot - Shoot Preload", seconds(4.0)), + goToDepotAndIntake(), + // startShooting(); + waitSeconds(2.0), + cycleIntake(6.0 / 3.0, 6)), + false, + true)); + + autos.routines.put("LeftClimb", climb(Field.leftClimbLocation(), rotation2d(0))); + autos.routines.put("RightClimb", climb(Field.rightClimbLocation(), rotation2d(0))); + + return autos; + } + + /** A mirrorable, flippable auto (the @auto defaults). */ + private static Auto auto(AutoAction root) { + return new Auto(root, true, true); + } + + // --------------------------------------------------------------------------- + // Reusable command builders (ported from autos.py / routines.py) + // --------------------------------------------------------------------------- + + private static AutoAction cycleIntake(double time, int count) { + double delayEach = time / 2 / count; + Sequence result = seq; + for (int i = 0; i < count; i++) { + result = + result.andThen( + stowIntake(), waitSeconds(delayEach), deployIntake(), waitSeconds(delayEach)); + } + return result; + } + + private static AutoAction fromBumpPrepareForTrench(double angle) { + return seq.andThen( + autoPilotTo(3.5, 7.55, angle) + .withVelocity(DEFAULT_TRENCH_VELOCITY) + .withEntryAngle(0) + .withConstraints(apConstraints(2.0, 2.0, 2.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction()); + } + + private static AutoAction goToCenterUnderLeftTrenchFromAllianceIntakeIn() { + return seq.andThen( + autoPilotTo(4.0, 7.55, -90) + .withVelocity(DEFAULT_TRENCH_VELOCITY) + .withEntryAngle(0) + .toXBasedAutoAction(), + // xBasedAutopilot(LEFT_TRENCH_CENTER_SIDE_POSE transformed by rotation, + // velocity, entryAngle=secondEntryAngle) + followPath("Left Trench To Center Intake In")); + } + + private static AutoAction goToDepotAndIntake() { + // drive near the depot and slow down so that we don't go too fast while intaking + return seq.andThen( + autoPilotTo(1.5, 5.1, 135) + .withVelocity(1.0) + .withProfile( + apProfile(apConstraints(5.1, 10.0, 3.0), meters(0.1), degrees(4.0), meters(0.2))) + .toAutoAction(), + autoPilotTo(0.715, 5.1, 135) + .withConstraints(apConstraints(1.0, 3.0, 3.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction(), + // Wait to stop autopilot from seeing initial velocity and panicking. + // Autopilot is probably more robust and simpler. Also removes a path planner + // path and enables us to use hot reload to tune it: followPath("Intake Depot"). + waitSeconds(0.1), + autoPilotTo(0.715, 6.4, 135) + .withConstraints(apConstraints(1.0, 3.0, 2.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction(), + // Wait to stop autopilot from seeing initial velocity and panicking. + waitSeconds(0.1), + autoPilotTo(1.0, 6.8, 135) + .withConstraints(apConstraints(5.1, 5.0, 3.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction()); + } + + private static AutoAction aggressive( + boolean useDepot, boolean fromBump, boolean shootPreload, boolean doSecondSweep) { + double intakeCycleTime = 1.0 / 3.0; + int intakeCycleCount = 1; + + Sequence result = seq; + if (shootPreload) { + result = result.andThen(startShooting(), waitSeconds(1.5)); + } + if (fromBump) { + result = result.andThen(fromBumpPrepareForTrench(-90)); + } + if (shootPreload) { + result = result.andThen(stopShooting()); + } + + // Cycle 1 + result = + result.andThen( + // Autopilot under the trench. + // Gives us solid acceleration and makes us resilient to unpredictable starting + // location. + autoPilotTo(LEFT_TRENCH_CENTER_SIDE_POSE) + .withVelocity(AGGRESSIVE_TRENCH_VELOCITY) + // Constraints here are unique to this very high speed, high + // acceleration movement, so almost infinite acceleration limit is + // hardcoded in here. + .withConstraints(apConstraints(AGGRESSIVE_TRENCH_VELOCITY, 200.0)) + // No entry angle, we just want to beeline to trench exit. + .toXBasedAutoAction(), + // inParallel(stowIntake(), + // followPath("Starting Position Left Trench To Center Intake In")) + inParallel(deployIntake(), followPath("Left Side Aggressive Sweep Intake In")), + inParallel( + seq.andThen(waitSeconds(0.6), startShooting()), + followPath("Left Bump To Alliance")), + waitSeconds(0.1), + autoPilotTo(LEFT_ALLIANCE_ZONE_MIDDLE_POSE).toAutoAction(), + cycleIntake(2.5, 5)); + + if (doSecondSweep) { + result = + result.andThen( + cycleIntake(intakeCycleTime, intakeCycleCount), + // wait(1.0), + // Cycle 2 + autoPilotTo(3.5, 7.55, -90) + .withVelocity(DEFAULT_TRENCH_VELOCITY) + .withEntryAngle(0) + .withConstraints(apConstraints(2.0, 2.0, 2.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction(), + inParallel(stopShooting(), goToCenterUnderLeftTrenchFromAllianceIntakeIn()), + inParallel(deployIntake(), followPath("Left Side Close 2nd Sweep Intake In")), + inParallel( + seq.andThen(waitSeconds(0.4), startShooting()), + followPath("Left Bump To Alliance")), + waitSeconds(0.1), + autoPilotTo(2.700, 5.75, -90).toAutoAction(), + waitSeconds(1)); + } + + if (useDepot) { + result = + result.andThen( + autoPilotTo(1.5, 5.9, -180) + .withVelocity(0.0) + .withConstraints(apConstraints(2.0, 2.0, 2.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction()); + } + + return result.andThen(cycleIntake(intakeCycleTime, intakeCycleCount)); + } + + private static AutoAction conservative( + boolean useDepot, boolean fromBump, boolean shootPreload, boolean doSecondSweep) { + double intakeCycleTime = 0.5; + int intakeCycleCount = 1; + + Sequence result = seq; + if (shootPreload) { + result = result.andThen(startShooting(), waitSeconds(1.5)); + } + if (fromBump) { + result = result.andThen(fromBumpPrepareForTrench(0)); + } + if (shootPreload) { + result = result.andThen(stopShooting()); + } + + // Cycle 1 + result = + result.andThen( + autoPilotTo(5.2, 7.4).withVelocity(DEFAULT_TRENCH_VELOCITY).toXBasedAutoAction(), + inParallel(stowIntake(), followPath("Starting Position Left Trench To Center")), + inParallel(deployIntake(), followPath("Left Side Conservative Sweep")), + inParallel( + seq.andThen(waitSeconds(0.6), startShooting()), + followPath("Left Bump To Alliance")), + waitSeconds(0.1), + autoPilotTo(2.700, 5.75, -90).toAutoAction(), + waitSeconds(2.5)); + + // wait(1.0), + // Cycle 2 + if (doSecondSweep) { + result = + result.andThen( + cycleIntake(intakeCycleTime, intakeCycleCount), + autoPilotTo(3.5, 7.55, -90) + .withVelocity(DEFAULT_TRENCH_VELOCITY) + .withEntryAngle(0) + .withConstraints(apConstraints(2.0, 2.0, 2.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction(), + inParallel(stopShooting(), goToCenterUnderLeftTrenchFromAllianceIntakeIn()), + inParallel(deployIntake(), followPath("Left Side Close 2nd Sweep Intake In")), + inParallel( + seq.andThen(waitSeconds(0.4), startShooting()), + followPath("Left Bump To Alliance")), + waitSeconds(0.1), + autoPilotTo(2.700, 5.75, -90).toAutoAction(), + waitSeconds(1)); + } + + if (useDepot) { + result = + result.andThen( + autoPilotTo(1.5, 5.9, -180) + .withVelocity(0.0) + .withConstraints(apConstraints(2.0, 2.0, 2.0)) + .withPidGains(pidGains(1.5)) + .toAutoAction()); + } + + return result.andThen(cycleIntake(intakeCycleTime, intakeCycleCount)); + } + + private static AutoAction follower() { + // Don't deploy the intake to avoid smashing it into the trench. + // deployIntake(); + // startShooting(); + return seq.andThen( + networkConfigurableWait("Follower - Preload", seconds(2.5)), + // stopShooting(); + autoPilotTo(LEFT_TRENCH_CENTER_SIDE_POSE) + .withVelocity(2.0) + // Constraints here are unique to this very high speed, high + // acceleration movement, so almost infinite acceleration limit is + // hardcoded in here. + .withConstraints(apConstraints(AGGRESSIVE_TRENCH_VELOCITY, 200.0)) + // No entry angle, we just want to beeline to trench exit. + .toXBasedAutoAction(), + deployIntake(), + followPath("Left Side Follower Sweep Intake In"), + networkConfigurableWait("Follower - Before Bump Return", seconds(0.0)), + waitSeconds(0.1), + autoPilotTo(2.700, 5.75, -90).toAutoAction(), + autoPilotTo(1.5, 5.1, 135) + .withProfile( + apProfile(apConstraints(5.1, 10.0, 3.0), meters(0.1), degrees(4.0), meters(0.2))) + .toAutoAction(), + startShooting(), + networkConfigurableWait("Follower - Before Depot", seconds(0.0)), + goToDepotAndIntake(), + waitSeconds(2.0), + cycleIntake(6.0 / 3.0, 6)); + } + + /** Reusable climb lineup subroutine. */ + private static AutoAction climb(Pose2d targetPose, Rotation2d entryAngle) { + // xBasedAutopilot(FieldConstants.Alliance.center transformed by + // climb_offset, constraints=CLIMB_CONSTRAINTS) + return seq.andThen( + inParallel( + seq.andThen( + autoPilotTo(targetPose) + .withEntryAngle(entryAngle) + .withConstraints(CLIMB_CONSTRAINTS) + .toXBasedAutoAction()), + climbSearch()), + waitSeconds(0.5), + climbHang()); + } + + private GenerateAutos() {} +} diff --git a/src/main/java/frc/robot/constants/JsonConstants.java b/src/main/java/frc/robot/constants/JsonConstants.java index 1575ebc0..30e0c638 100644 --- a/src/main/java/frc/robot/constants/JsonConstants.java +++ b/src/main/java/frc/robot/constants/JsonConstants.java @@ -18,25 +18,17 @@ import coppercore.parameter_tools.path_provider.EnvironmentHandler; import coppercore.wpilib_interface.controllers.Controllers; import coppercore.wpilib_interface.subsystems.motors.profile.MotionProfileConfig; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.Constants; import frc.robot.RobotContainer; -import frc.robot.auto.Auto; import frc.robot.auto.Autos; import frc.robot.constants.drive.DriveConstants; import frc.robot.constants.drive.PhysicalDriveConstants; import frc.robot.util.json.JSONAPTarget; import frc.robot.util.json.JSONMotionProfileConfig; -import frc.robot.util.ts.PythonGenerator; -import frc.robot.util.ts.PythonGeometryMethods; /** * JsonConstants handles loading and saving of all constants through JSON. Call `loadConstants` @@ -63,7 +55,16 @@ public class JsonConstants { JSONMeasure.registerUnit(RPM.per(Second), "RPM Per Second"); } - public static JSONHandler loadConstants(RobotContainer robotContainer) { + /** + * Loads every constant from JSON for the active environment (selected via deploy + * constants/config.json) and returns the configured {@link JSONHandler}. + * + *

This is the shared initialization sequence used both by robot startup ({@link + * #loadConstants(RobotContainer)}) and by the offline auto generator (see frc.robot.autogen), so + * there is a single code path for resolving and deserializing constants. It does not start the + * tuning server. + */ + public static JSONHandler loadConstants() { environmentHandler = EnvironmentHandler.getEnvironmentHandler( @@ -75,8 +76,6 @@ public static JSONHandler loadConstants(RobotContainer robotContainer) { jsonSyncSettings.addJsonTypeAdapterFactory(new OptionalTypeAdapterFactory()); - // jsonSyncSettings.setUpPolymorphAdapter(AutoAction.class); - var pathProvider = environmentHandler.getEnvironmentPathProvider(); System.out.println("[JsonConstants] Environment name: " + pathProvider.getEnvironmentName()); @@ -115,6 +114,20 @@ public static JSONHandler loadConstants(RobotContainer robotContainer) { autos = jsonHandler.getObject(new Autos(), "Autos.json"); + controllers = + jsonHandler.getObject(new Controllers(), operatorConstants.controllerBindingsFile); + + return jsonHandler; + } + + /** + * Robot-startup variant: loads all constants (see {@link #loadConstants()}) and, when the active + * environment enables it, starts the constant/autos tuning server. The {@code robotContainer} is + * used to reload auto commands when the autos route receives a POST. + */ + public static JSONHandler loadConstants(RobotContainer robotContainer) { + loadConstants(); + if (featureFlags.useTuningServer) { // do not crash Robot if routes could not be added for any reason try { @@ -148,23 +161,6 @@ public static JSONHandler loadConstants(RobotContainer robotContainer) { } } - controllers = - jsonHandler.getObject(new Controllers(), operatorConstants.controllerBindingsFile); - - if (Constants.currentMode == Constants.Mode.SIM) { - PythonGeometryMethods.registerAll(); - PythonGenerator.generateForClasses( - "auto_action.py", - Auto.class, - Transform2d.class, - Transform3d.class, - Rotation2d.class, - Rotation3d.class, - Pose2d.class, - Pose3d.class, - Translation2d.class, - Translation3d.class); - } return jsonHandler; } diff --git a/src/main/java/frc/robot/util/json/JSONAPTarget.java b/src/main/java/frc/robot/util/json/JSONAPTarget.java index 8f449ede..5ab6d8d5 100644 --- a/src/main/java/frc/robot/util/json/JSONAPTarget.java +++ b/src/main/java/frc/robot/util/json/JSONAPTarget.java @@ -5,15 +5,14 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Distance; -import frc.robot.util.ts.GeneratedOptional; import java.lang.reflect.Constructor; public class JSONAPTarget extends JSONObject { protected Pose2d reference; - @GeneratedOptional protected Rotation2d entryAngle; + protected Rotation2d entryAngle; protected double velocity; - @GeneratedOptional protected Distance rotationRadius; + protected Distance rotationRadius; public JSONAPTarget(APTarget target) { super(target); diff --git a/src/main/java/frc/robot/util/ts/GeneratedDefault.java b/src/main/java/frc/robot/util/ts/GeneratedDefault.java deleted file mode 100644 index da6a2143..00000000 --- a/src/main/java/frc/robot/util/ts/GeneratedDefault.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.util.ts; - -import java.lang.annotation.ElementType; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -/** Supplies an explicit Python default value for a generated dataclass field. */ -@Retention(RetentionPolicy.RUNTIME) -@Target(ElementType.FIELD) -public @interface GeneratedDefault { - String value(); -} diff --git a/src/main/java/frc/robot/util/ts/GeneratedOptional.java b/src/main/java/frc/robot/util/ts/GeneratedOptional.java deleted file mode 100644 index 56e4b8e2..00000000 --- a/src/main/java/frc/robot/util/ts/GeneratedOptional.java +++ /dev/null @@ -1,14 +0,0 @@ -package frc.robot.util.ts; - -import java.lang.annotation.ElementType; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -/** - * Marks a field as optional in the generated Python dataclass. The field will be emitted as {@code - * Optional[T] = None}. - */ -@Retention(RetentionPolicy.RUNTIME) -@Target(ElementType.FIELD) -public @interface GeneratedOptional {} diff --git a/src/main/java/frc/robot/util/ts/PythonAppend.java b/src/main/java/frc/robot/util/ts/PythonAppend.java deleted file mode 100644 index 4440b513..00000000 --- a/src/main/java/frc/robot/util/ts/PythonAppend.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.util.ts; - -import java.lang.annotation.ElementType; -import java.lang.annotation.Repeatable; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -/** - * Injects raw Python code into the generated file. Place on a Java class that is processed by - * PythonGenerator. - * - *

Use {@code atTop = true} to place the code at the top of the file (e.g., for imports) instead - * of after the class. - */ -@Retention(RetentionPolicy.RUNTIME) -@Target(ElementType.TYPE) -@Repeatable(PythonAppends.class) -public @interface PythonAppend { - /** The raw Python code to inject. */ - String value(); - - /** If true, the code is placed at the top of the file (before any classes). Defaults to false. */ - boolean atTop() default false; -} diff --git a/src/main/java/frc/robot/util/ts/PythonAppends.java b/src/main/java/frc/robot/util/ts/PythonAppends.java deleted file mode 100644 index a054d792..00000000 --- a/src/main/java/frc/robot/util/ts/PythonAppends.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.util.ts; - -import java.lang.annotation.ElementType; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -/** Container annotation for repeatable {@link PythonAppend} annotations. */ -@Retention(RetentionPolicy.RUNTIME) -@Target(ElementType.TYPE) -public @interface PythonAppends { - PythonAppend[] value(); -} diff --git a/src/main/java/frc/robot/util/ts/PythonGenerator.java b/src/main/java/frc/robot/util/ts/PythonGenerator.java deleted file mode 100644 index c0c45546..00000000 --- a/src/main/java/frc/robot/util/ts/PythonGenerator.java +++ /dev/null @@ -1,918 +0,0 @@ -package frc.robot.util.ts; - -import com.google.gson.FieldNamingStrategy; -import coppercore.parameter_tools.json.JSONSyncConfig; -import coppercore.parameter_tools.json.JSONSyncConfigBuilder; -import coppercore.parameter_tools.json.adapters.measure.JSONMeasure; -import coppercore.parameter_tools.json.annotations.JSONExclude; -import coppercore.parameter_tools.json.annotations.JsonSubtype; -import coppercore.parameter_tools.json.annotations.JsonType; -import coppercore.parameter_tools.json.helpers.JSONConverter; -import coppercore.parameter_tools.json.helpers.JSONConverter.ConversionException; -import coppercore.parameter_tools.json.helpers.JSONObject; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.PerUnit; -import edu.wpi.first.units.Unit; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.FileWriter; -import java.io.IOException; -import java.lang.reflect.*; -import java.nio.file.Path; -import java.util.*; - -/** - * Generates Python dataclass definitions from any Java class using the same config rules as - * JSONSync / JSONHandler. - */ -public final class PythonGenerator { - - private static final JSONSyncConfig defaultConfig = new JSONSyncConfigBuilder().build(); - - private static FieldNamingStrategy namingStrategy; - - private static final Map, String> primitiveMap = new HashMap<>(); - private static final Map pythonTypeMap = new HashMap<>(); - private static final Set> generated = new HashSet<>(); - private static final Set defaultChecked = new HashSet<>(); - private static final Set defaultSupported = new HashSet<>(); - private static final StringBuilder output = new StringBuilder(); - private static boolean hasGeneratedUnits = false; - private static final Path BASE_PATH = - Filesystem.getDeployDirectory() - .toPath() - .resolve("../../..") - .normalize() - .resolve("auto_generator/src"); - // Output to auto_generator/src/ - private static String unitFilePath = "units.py"; - private static boolean hasImportedUnits = false; - - public static Set> emitted = new HashSet<>(); - - // Track classes that need to_dict methods generated - private static final List toDictMethods = new ArrayList<>(); - - // Extra methods to inject into generated classes we can't annotate (e.g. WPILib geometry classes) - private static final Map, List> extraMethods = new HashMap<>(); - - // Reverse mapping: JSONConverter wrapper class → original WPILib class - private static final Map, Class> convertedToOriginal = new HashMap<>(); - - /** - * Register additional Python methods to be injected into the generated dataclass for a given Java - * class. Each string is a complete method definition (indented with 4 spaces). Call this before - * generateForClasses. - */ - public static void registerExtraMethods(Class clazz, String... methods) { - extraMethods.computeIfAbsent(clazz, k -> new ArrayList<>()).addAll(Arrays.asList(methods)); - } - - private static boolean shouldSkipField(Field field) { - return Modifier.isStatic(field.getModifiers()) - || Modifier.isTransient(field.getModifiers()) - || field.isAnnotationPresent(JSONExclude.class); - } - - public static void setUnitFilePath(String path) { - unitFilePath = path; - } - - // ============================================ - // Units file generation - // ============================================ - - public static void generateUnitsFile(Path outputDir) { - if (hasGeneratedUnits) return; - hasGeneratedUnits = true; - StringBuilder sb = new StringBuilder(); - sb.append( - "\"\"\"\n" - + "Auto-generated by PythonGenerator. Do not edit manually.\n" - + "\n" - + "Provides unit types and measure creation functions that serialize\n" - + "to the same JSON format the Java robot code expects.\n" - + "\"\"\"\n\n" - + "from __future__ import annotations\n\n" - + "from dataclasses import dataclass\n\n\n" - + "@dataclass\n" - + "class Measure:\n" - + " \"\"\"A measure with a numeric value and a unit name string.\"\"\"\n" - + " value: float\n" - + " unit: str\n\n" - + " def to_dict(self) -> dict:\n" - + " return {\"value\": self.value, \"unit\": self.unit}\n\n\n" - + "@dataclass\n" - + "class Unit:\n" - + " \"\"\"A named unit that can create Measure instances.\"\"\"\n" - + " name: str\n\n" - + " def of(self, value: float) -> Measure:\n" - + " return Measure(value=value, unit=self.name)\n\n\n"); - - Set units = new HashSet<>(); - HashMap, String> baseUnits = new HashMap<>(); - Set emittedMeasureNames = new HashSet<>(); - JSONMeasure.unitMap.forEach( - (name, unitFunc) -> { - var measure = unitFunc.apply(1.0); - var unit = measure.unit(); - var baseUnit = unit.getBaseUnit(); - if (baseUnit == null) return; - Class baseUnitClass = baseUnit.getClass(); - if (!baseUnits.containsKey(baseUnitClass)) { - String measureName; - if (baseUnit instanceof PerUnit perBaseUnit) { - String numeratorName = - stripUnitSuffix(perBaseUnit.numerator().getBaseUnit().getClass().getSimpleName()); - String denominatorName = - stripUnitSuffix( - perBaseUnit.denominator().getBaseUnit().getClass().getSimpleName()); - measureName = numeratorName + "Per" + denominatorName; - } else { - measureName = stripUnitSuffix(baseUnitClass.getSimpleName()); - } - if (emittedMeasureNames.contains(measureName)) return; - emittedMeasureNames.add(measureName); - // In Python, we don't need separate UnitType classes — just comments - sb.append("# ").append(measureName).append(" unit type\n"); - pythonTypeMap.put(baseUnitClass, measureName); - baseUnits.put(baseUnitClass, measureName); - } - if (units.contains(unit)) return; - String registeredName = baseUnits.get(baseUnitClass); - if (registeredName == null) return; - units.add(unit); - pythonTypeMap.put(unit.getClass(), registeredName); - var unitName = unit.name().replace(" ", "_").replace("-", "_"); - if (unitName.equals("")) unitName = "Unitless"; - sb.append(unitName).append(" = Unit(\"").append(unit.name()).append("\")\n"); - }); - - System.out.println("Generated Python definitions for " + units.size() + " units."); - - try (FileWriter writer = new FileWriter(outputDir.resolve(unitFilePath).toString())) { - writer.write(sb.toString()); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - - static { - primitiveMap.put(int.class, "int"); - primitiveMap.put(Integer.class, "int"); - primitiveMap.put(double.class, "float"); - primitiveMap.put(Double.class, "float"); - primitiveMap.put(float.class, "float"); - primitiveMap.put(Float.class, "float"); - primitiveMap.put(long.class, "int"); - primitiveMap.put(Long.class, "int"); - primitiveMap.put(short.class, "int"); - primitiveMap.put(Short.class, "int"); - primitiveMap.put(byte.class, "int"); - primitiveMap.put(Byte.class, "int"); - - primitiveMap.put(boolean.class, "bool"); - primitiveMap.put(Boolean.class, "bool"); - - primitiveMap.put(String.class, "str"); - primitiveMap.put(char.class, "str"); - primitiveMap.put(Character.class, "str"); - - pythonTypeMap.putAll(primitiveMap); - } - - // ============================================ - // Public Entry - // ============================================ - - private static Path outputDir; - - private static void initGenerator() { - generated.clear(); - emitted.clear(); - output.setLength(0); - toDictMethods.clear(); - convertedToOriginal.clear(); - hasImportedUnits = false; - - output.append( - "\"\"\"\n" - + "Auto-generated by PythonGenerator. Do not edit manually.\n" - + "\"\"\"\n\n" - + "from __future__ import annotations\n\n" - + "import json\n" - + "import math as _math\n" - + "from dataclasses import dataclass, field\n" - + "from typing import Any, Callable, List, Optional\n\n"); - - namingStrategy = defaultConfig.namingPolicy(); - } - - private static void writeOutput(String filePath) { - try (FileWriter writer = new FileWriter(outputDir.resolve(filePath).toString())) { - writer.write(output.toString()); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - - public static void generateForObjects(Path outputDir, String filePath, Object... roots) { - PythonGenerator.outputDir = outputDir; - initGenerator(); - - for (Object root : roots) { - resolveType(root.getClass()); - } - - writeOutput(filePath); - } - - public static void generateForClasses(Path outputDir, String filePath, Class... rootClasses) { - PythonGenerator.outputDir = outputDir; - initGenerator(); - - for (Class rootClass : rootClasses) { - resolveType(rootClass); - } - - writeOutput(filePath); - } - - /** Convenience overload that uses the default BASE_PATH as output directory. */ - public static void generateForClasses(String filePath, Class... rootClasses) { - generateForClasses(BASE_PATH, filePath, rootClasses); - } - - // ============================================ - // Core Generation - // ============================================ - - private static void generateType(Class clazz, String suggestedName) { - if (generated.contains(clazz)) return; - if (isPrimitive(clazz)) return; - if (clazz.isEnum()) { - generateEnum(clazz); - return; - } - - generated.add(clazz); - - if (clazz.isAnnotationPresent(JsonType.class)) { - generateDiscriminatedUnion(clazz); - return; - } - - generateDataclass(clazz, suggestedName); - } - - // ============================================ - // Dataclass generation - // ============================================ - - private record PythonField( - String name, - String pyName, - String type, - Class originalType, - boolean isOptional, - boolean supportsDefault, - String generatedDefault) {} - - /** Convert camelCase to snake_case */ - private static String toSnakeCase(String name) { - StringBuilder sb = new StringBuilder(); - for (int i = 0; i < name.length(); i++) { - char c = name.charAt(i); - if (Character.isUpperCase(c)) { - if (i > 0) sb.append('_'); - sb.append(Character.toLowerCase(c)); - } else { - sb.append(c); - } - } - return sb.toString(); - } - - public static void generateDataclass(Class clazz, String suggestedName) { - suggestedName = suggestedName != null ? suggestedName : clazz.getSimpleName(); - if (emitted.contains(clazz)) return; - emitted.add(clazz); - pythonTypeMap.put(clazz, suggestedName); - - StringBuilder sb = new StringBuilder(); - sb.append("\n@dataclass\n"); - sb.append("class ").append(suggestedName).append(":\n"); - - ArrayList fields = new ArrayList<>(); - - for (Field jField : clazz.getDeclaredFields()) { - if (shouldSkipField(jField)) continue; - - Class fieldType = jField.getType(); - Class originalFieldType = fieldType; - - try { - fieldType = JSONConverter.convert(fieldType); - } catch (ConversionException ignored) { - } - - String pyType = resolveType(jField.getGenericType(), originalFieldType.getSimpleName()); - String jsonName = namingStrategy.translateName(jField); - String pyName = toSnakeCase(jsonName); - - boolean isOptional = - jField.isAnnotationPresent(GeneratedOptional.class) - || Optional.class.isAssignableFrom(fieldType); - - boolean supportsDefault = supportsDefaultValue(originalFieldType); - - fields.add( - new PythonField( - jsonName, - pyName, - pyType, - originalFieldType, - isOptional, - supportsDefault, - getGeneratedDefault(jField))); - } - - // Emit fields — fields with defaults must come after fields without - // First: fields without defaults, then fields with defaults - List noDefault = new ArrayList<>(); - List withDefault = new ArrayList<>(); - for (PythonField f : fields) { - if (f.isOptional || f.supportsDefault || f.generatedDefault != null) { - withDefault.add(f); - } else { - noDefault.add(f); - } - } - - if (noDefault.isEmpty() && withDefault.isEmpty()) { - sb.append(" pass\n"); - } - - for (PythonField f : noDefault) { - sb.append(" ").append(f.pyName).append(": "); - sb.append(f.type); - sb.append("\n"); - } - - for (PythonField f : withDefault) { - sb.append(" ").append(f.pyName).append(": "); - if (f.isOptional) { - sb.append("Optional[").append(f.type).append("]"); - sb.append(" = None"); - } else { - sb.append(f.type); - String defaultVal = - f.generatedDefault != null - ? f.generatedDefault - : getPythonDefaultValue(f.originalType, f.type); - sb.append(" = ").append(defaultVal); - } - sb.append("\n"); - } - - // Generate __post_init__ for complex default values - List needsPostInit = new ArrayList<>(); - for (PythonField f : withDefault) { - if (!f.isOptional && isComplexType(f.originalType)) { - needsPostInit.add(f); - } - } - - // Emit to_dict method - sb.append("\n def to_dict(self) -> dict:\n"); - if (fields.isEmpty()) { - sb.append(" return {}\n"); - } else { - sb.append(" d: dict = {}\n"); - for (PythonField f : fields) { - if (f.isOptional) { - sb.append(" if self.").append(f.pyName).append(" is not None:\n"); - sb.append(" d[\"") - .append(f.name) - .append("\"] = _to_dict(self.") - .append(f.pyName) - .append(")\n"); - } else { - sb.append(" d[\"") - .append(f.name) - .append("\"] = _to_dict(self.") - .append(f.pyName) - .append(")\n"); - } - } - sb.append(" return d\n"); - } - - // Emit PythonMethod annotations - emitPythonMethods(clazz, sb); - emitExtraMethods(clazz, sb); - - sb.append("\n"); - output.append(sb); - emitPythonAppends(clazz); - } - - // ============================================ - // Polymorphism - // ============================================ - - private static void generateDiscriminatedUnion(Class baseClass) { - JsonType typeAnnotation = baseClass.getAnnotation(JsonType.class); - String discriminator = typeAnnotation.property(); - - List subtypeNames = new ArrayList<>(); - - pythonTypeMap.put(baseClass, baseClass.getSimpleName()); - - for (JsonSubtype subtype : typeAnnotation.subtypes()) { - Class subClass = subtype.clazz(); - String name = subtype.name(); - - subtypeNames.add(subClass.getSimpleName()); - generateSubtype(subClass, discriminator, name, baseClass); - } - - // Union type alias - output - .append("\n") - .append(baseClass.getSimpleName()) - .append(" = Optional[") - .append(String.join(" | ", subtypeNames)) - .append("]\n\n"); - - emitPythonAppends(baseClass); - } - - private static void generateSubtype( - Class clazz, String discriminator, String discriminatorValue, Class baseClass) { - if (generated.contains(clazz)) return; - generated.add(clazz); - - pythonTypeMap.put(clazz, clazz.getSimpleName()); - - StringBuilder sb = new StringBuilder(); - - sb.append("\n@dataclass\n"); - sb.append("class ").append(clazz.getSimpleName()).append(":\n"); - - // Discriminator field - sb.append(" ") - .append(toSnakeCase(discriminator)) - .append(": str = field(default=\"") - .append(discriminatorValue) - .append("\", init=False)\n"); - - ArrayList fields = new ArrayList<>(); - - for (Field jField : getAllFields(clazz)) { - if (shouldSkipField(jField)) continue; - if (jField.getName().equals(discriminator)) continue; - - Class fieldType = jField.getType(); - Class originalFieldType = fieldType; - - try { - fieldType = JSONConverter.convert(fieldType); - } catch (ConversionException ignored) { - } - - String pyType = resolveType(jField.getGenericType(), originalFieldType.getSimpleName()); - String jsonName = namingStrategy.translateName(jField); - String pyName = toSnakeCase(jsonName); - - boolean isOptional = - jField.isAnnotationPresent(GeneratedOptional.class) - || Optional.class.isAssignableFrom(fieldType); - - boolean supportsDefault = supportsDefaultValue(originalFieldType); - - fields.add( - new PythonField( - jsonName, - pyName, - pyType, - originalFieldType, - isOptional, - supportsDefault, - getGeneratedDefault(jField))); - } - - // Fields without defaults first, then with defaults - List noDefault = new ArrayList<>(); - List withDefault = new ArrayList<>(); - for (PythonField f : fields) { - if (f.isOptional || f.supportsDefault || f.generatedDefault != null) { - withDefault.add(f); - } else { - noDefault.add(f); - } - } - - if (noDefault.isEmpty() && withDefault.isEmpty()) { - // only the discriminator - } - - for (PythonField f : noDefault) { - sb.append(" ").append(f.pyName).append(": "); - sb.append(f.type); - sb.append("\n"); - } - - for (PythonField f : withDefault) { - sb.append(" ").append(f.pyName).append(": "); - if (f.isOptional) { - sb.append("Optional[").append(f.type).append("]"); - sb.append(" = None"); - } else { - sb.append(f.type); - String defaultVal = - f.generatedDefault != null - ? f.generatedDefault - : getPythonDefaultValue(f.originalType, f.type); - sb.append(" = ").append(defaultVal); - } - sb.append("\n"); - } - - // to_dict - sb.append("\n def to_dict(self) -> dict:\n"); - sb.append(" d: dict = {\"") - .append(discriminator) - .append("\": self.") - .append(toSnakeCase(discriminator)) - .append("}\n"); - for (PythonField f : fields) { - if (f.isOptional) { - sb.append(" if self.").append(f.pyName).append(" is not None:\n"); - sb.append(" d[\"") - .append(f.name) - .append("\"] = _to_dict(self.") - .append(f.pyName) - .append(")\n"); - } else { - sb.append(" d[\"") - .append(f.name) - .append("\"] = _to_dict(self.") - .append(f.pyName) - .append(")\n"); - } - } - sb.append(" return d\n"); - - // Methods from base class and subclass - emitPythonMethods(baseClass, sb); - emitPythonMethods(clazz, sb); - emitExtraMethods(baseClass, sb); - emitExtraMethods(clazz, sb); - - sb.append("\n"); - output.append(sb); - emitPythonAppends(clazz); - } - - // ============================================ - // Type Resolution - // ============================================ - - private static String resolveType(Type type) { - return resolveType(type, null); - } - - private static String resolveType(Type type, String suggestedName) { - if (type instanceof Class clazz) { - boolean isUnit = isUnitType(clazz); - if (isUnit) { - ensureUnitsIncluded(); - } - - try { - Class> wrapper = JSONConverter.convert(clazz); - if (wrapper != null) { - suggestedName = clazz.getSimpleName(); - convertedToOriginal.put(wrapper, clazz); - clazz = wrapper; - } - } catch (JSONConverter.ConversionException ignored) { - } - - if (primitiveMap.containsKey(clazz)) { - return primitiveMap.get(clazz); - } - - if (clazz.isArray()) { - return "List[" + resolveType(clazz.getComponentType()) + "]"; - } - - if (Map.class.isAssignableFrom(clazz)) { - return "dict"; - } - - suggestedName = suggestedName != null ? suggestedName : clazz.getSimpleName(); - if (isUnit) { - return "units.Measure"; - } - generateType(clazz, suggestedName); - return suggestedName; - } - - if (type instanceof ParameterizedType pt) { - Type raw = pt.getRawType(); - Type[] args = pt.getActualTypeArguments(); - if (raw instanceof Class rawClass) { - boolean isUnit = isUnitType(rawClass); - if (isUnit) { - ensureUnitsIncluded(); - } - - if (Collection.class.isAssignableFrom(rawClass)) { - return "List[" + resolveType(args[0]) + "]"; - } - - if (Map.class.isAssignableFrom(rawClass)) { - return "dict"; - } - - if (Optional.class.isAssignableFrom(rawClass)) { - return "Optional[" + resolveType(args[0]) + "]"; - } - - try { - Class> wrapper = JSONConverter.convert(rawClass); - if (wrapper != null) { - suggestedName = rawClass.getSimpleName(); - convertedToOriginal.put(wrapper, rawClass); - rawClass = wrapper; - } - } catch (JSONConverter.ConversionException ignored) { - } - - if (isUnitType(rawClass)) { - isUnit = true; - ensureUnitsIncluded(); - } - - suggestedName = suggestedName != null ? suggestedName : rawClass.getSimpleName(); - generateType(rawClass, suggestedName); - if (isUnit) { - return "units.Measure"; - } - return suggestedName; - } - } - - return "Any"; - } - - // ============================================ - // Enums - // ============================================ - - private static void generateEnum(Class clazz) { - if (generated.contains(clazz)) return; - generated.add(clazz); - pythonTypeMap.put(clazz, clazz.getSimpleName()); - - Object[] constants = clazz.getEnumConstants(); - List values = new ArrayList<>(); - for (Object constant : constants) { - values.add("\"" + constant.toString() + "\""); - } - - output - .append("\n") - .append(clazz.getSimpleName()) - .append(" = str # Literal[") - .append(String.join(", ", values)) - .append("]\n\n"); - } - - // ============================================ - // Python-specific code injection - // ============================================ - - private static void emitPythonMethods(Class clazz, StringBuilder sb) { - PythonMethod[] methods = clazz.getAnnotationsByType(PythonMethod.class); - for (PythonMethod method : methods) { - sb.append("\n"); - if (!method.comment().isEmpty()) { - // Emit as docstring inside the method - } - sb.append(" "); - if (method.isStatic()) { - sb.append("@staticmethod\n "); - } - sb.append("def ").append(method.name()).append("("); - if (!method.isStatic()) { - sb.append("self"); - } - sb.append(")"); - if (!method.returnType().equals("None") && !method.returnType().equals("self")) { - sb.append(" -> ").append(method.returnType()); - } - sb.append(":\n"); - if (!method.comment().isEmpty()) { - sb.append(" \"\"\"").append(method.comment()).append("\"\"\"\n"); - } - for (String line : method.body()) { - sb.append(" ").append(line).append("\n"); - } - } - } - - private static void emitExtraMethods(Class clazz, StringBuilder sb) { - List methods = extraMethods.get(clazz); - // If not found directly, try the original (pre-JSONConverter) class - if (methods == null) { - Class original = convertedToOriginal.get(clazz); - if (original != null) { - methods = extraMethods.get(original); - } - } - if (methods == null) return; - for (String methodCode : methods) { - sb.append("\n"); - // Re-indent each line with 4-space class-level indent. - // Java text blocks strip the common leading whitespace, so the code - // arrives unindented — we need to add the class body indent back. - for (String line : methodCode.split("\n", -1)) { - if (line.isEmpty()) { - sb.append("\n"); - } else { - sb.append(" ").append(line).append("\n"); - } - } - } - } - - private static void emitPythonAppends(Class clazz) { - PythonAppend[] appends = clazz.getAnnotationsByType(PythonAppend.class); - for (PythonAppend append : appends) { - if (append.atTop()) { - // Insert after the initial imports block - int insertPos = output.indexOf("from typing import"); - if (insertPos >= 0) { - insertPos = output.indexOf("\n", insertPos) + 1; - output.insert(insertPos, append.value() + "\n"); - } else { - output.insert(0, append.value() + "\n"); - } - } else { - output.append(append.value()).append("\n"); - } - } - } - - // ============================================ - // Utilities - // ============================================ - - private static String stripUnitSuffix(String name) { - return name.endsWith("Unit") ? name.substring(0, name.length() - 4) : name; - } - - private static boolean isPrimitive(Class clazz) { - return clazz.isPrimitive() || primitiveMap.containsKey(clazz); - } - - private static List getAllFields(Class clazz) { - List fields = new ArrayList<>(); - while (clazz != null) { - fields.addAll(Arrays.asList(clazz.getDeclaredFields())); - clazz = clazz.getSuperclass(); - } - return fields; - } - - private static boolean isUnitType(Class clazz) { - return Measure.class.isAssignableFrom(clazz) || Unit.class.isAssignableFrom(clazz); - } - - private static boolean isComplexType(Class type) { - return !isPrimitive(type) && !type.isEnum() && !isUnitType(type); - } - - private static String getGeneratedDefault(Field field) { - GeneratedDefault generatedDefault = field.getAnnotation(GeneratedDefault.class); - return generatedDefault == null ? null : generatedDefault.value(); - } - - private static boolean supportsDefaultValue(Type type) { - if (!defaultChecked.contains(type)) { - getPythonDefaultValue(type, ""); - } - return defaultSupported.contains(type); - } - - private static String getPythonDefaultValue(Type type, String pyTypeName) { - if (type instanceof Class clazz) { - if (primitiveMap.containsKey(clazz)) { - defaultChecked.add(type); - defaultSupported.add(type); - String pyType = primitiveMap.get(clazz); - switch (pyType) { - case "int": - return "0"; - case "float": - return "0.0"; - case "bool": - return "False"; - case "str": - return "\"\""; - default: - return "None"; - } - } - - if (clazz.isArray() || Collection.class.isAssignableFrom(clazz)) { - defaultChecked.add(type); - defaultSupported.add(type); - return "field(default_factory=list)"; - } - - if (clazz.isEnum()) { - Object[] constants = clazz.getEnumConstants(); - if (constants.length > 0) { - defaultChecked.add(type); - defaultSupported.add(type); - return "\"" + constants[0].toString() + "\""; - } - } - - if (Modifier.isAbstract(clazz.getModifiers())) { - defaultChecked.add(type); - defaultSupported.add(type); - return "None"; - } - - if (isUnitType(clazz)) { - defaultChecked.add(type); - defaultSupported.add(type); - return "None"; - } - - try { - var wrapper = JSONConverter.convert(clazz); - if (wrapper != null) { - String wrapperName = pythonTypeMap.getOrDefault(clazz, clazz.getSimpleName()); - defaultChecked.add(type); - defaultSupported.add(type); - return "field(default_factory=" + wrapperName + ")"; - } - } catch (ConversionException ignored) { - } - - // Check if all sub-fields support defaults - for (Field field : getAllFields(clazz)) { - if (shouldSkipField(field)) continue; - if (field.isAnnotationPresent(GeneratedOptional.class)) continue; - if (!supportsDefaultValue(field.getType())) { - defaultChecked.add(type); - return "None"; - } - } - - defaultChecked.add(type); - defaultSupported.add(type); - String className = pythonTypeMap.getOrDefault(clazz, clazz.getSimpleName()); - return "field(default_factory=" + className + ")"; - } - - if (type instanceof ParameterizedType pt) { - Type raw = pt.getRawType(); - if (raw instanceof Class rawClass) { - if (Collection.class.isAssignableFrom(rawClass)) { - defaultChecked.add(type); - defaultSupported.add(type); - return "field(default_factory=list)"; - } - if (Map.class.isAssignableFrom(rawClass)) { - defaultChecked.add(type); - defaultSupported.add(type); - return "field(default_factory=dict)"; - } - if (Optional.class.isAssignableFrom(rawClass)) { - defaultChecked.add(type); - defaultSupported.add(type); - return "None"; - } - } - } - - defaultChecked.add(type); - defaultSupported.add(type); - return "None"; - } - - private static void ensureUnitsIncluded() { - if (!hasGeneratedUnits) { - generateUnitsFile(outputDir); - } - if (hasImportedUnits) return; - hasImportedUnits = true; - output.append("from . import units\n\n"); - } -} diff --git a/src/main/java/frc/robot/util/ts/PythonGeometryMethods.java b/src/main/java/frc/robot/util/ts/PythonGeometryMethods.java deleted file mode 100644 index 895ad113..00000000 --- a/src/main/java/frc/robot/util/ts/PythonGeometryMethods.java +++ /dev/null @@ -1,399 +0,0 @@ -package frc.robot.util.ts; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; - -/** - * Registers WPILib-style geometry methods (plus, minus, transform_by, etc.) with PythonGenerator so - * they appear on the generated dataclasses in auto_action.py. - * - *

Call {@link #registerAll()} before {@code PythonGenerator.generateForClasses(...)}. - */ -public final class PythonGeometryMethods { - - private PythonGeometryMethods() {} - - public static void registerAll() { - registerRotation2dMethods(); - registerTranslation2dMethods(); - registerPose2dMethods(); - registerTransform2dMethods(); - registerRotation3dMethods(); - registerTranslation3dMethods(); - registerPose3dMethods(); - registerTransform3dMethods(); - } - - // ========================================================================= - // Rotation2d - // ========================================================================= - - private static void registerRotation2dMethods() { - PythonGenerator.registerExtraMethods( - Rotation2d.class, - // properties - m( - """ - @property - def radians(self) -> float: - return _math.radians(self.degrees)"""), - m( - """ - @property - def cos(self) -> float: - return _math.cos(self.radians)"""), - m( - """ - @property - def sin(self) -> float: - return _math.sin(self.radians)"""), - // constructors - m( - """ - @staticmethod - def from_radians(radians: float) -> Rotation2d: - return Rotation2d(degrees=_math.degrees(radians))"""), - // operators - m( - """ - def plus(self, other: Rotation2d) -> Rotation2d: - return Rotation2d(degrees=self.degrees + other.degrees)"""), - m( - """ - def minus(self, other: Rotation2d) -> Rotation2d: - return Rotation2d(degrees=self.degrees - other.degrees)"""), - m( - """ - def unary_minus(self) -> Rotation2d: - return Rotation2d(degrees=-self.degrees)"""), - m( - """ - def rotate_by(self, other: Rotation2d) -> Rotation2d: - return self.plus(other)"""), - // dunder operators - m( - """ - def __neg__(self) -> Rotation2d: - return self.unary_minus()"""), - m( - """ - def __add__(self, other: Rotation2d) -> Rotation2d: - return self.plus(other)"""), - m( - """ - def __sub__(self, other: Rotation2d) -> Rotation2d: - return self.minus(other)""")); - } - - // ========================================================================= - // Translation2d - // ========================================================================= - - private static void registerTranslation2dMethods() { - PythonGenerator.registerExtraMethods( - Translation2d.class, - // properties - m( - """ - @property - def norm(self) -> float: - return _math.hypot(self.x, self.y)"""), - // operators - m( - """ - def plus(self, other: Translation2d) -> Translation2d: - return Translation2d(x=self.x + other.x, y=self.y + other.y)"""), - m( - """ - def minus(self, other: Translation2d) -> Translation2d: - return Translation2d(x=self.x - other.x, y=self.y - other.y)"""), - m( - """ - def unary_minus(self) -> Translation2d: - return Translation2d(x=-self.x, y=-self.y)"""), - m( - """ - def times(self, scalar: float) -> Translation2d: - return Translation2d(x=self.x * scalar, y=self.y * scalar)"""), - m( - """ - def div(self, scalar: float) -> Translation2d: - return Translation2d(x=self.x / scalar, y=self.y / scalar)"""), - m( - """ - def rotate_by(self, rotation: Rotation2d) -> Translation2d: - c = rotation.cos - s = rotation.sin - return Translation2d(x=self.x * c - self.y * s, y=self.x * s + self.y * c)"""), - m( - """ - def distance(self, other: Translation2d) -> float: - return self.minus(other).norm"""), - // dunder operators - m( - """ - def __neg__(self) -> Translation2d: - return self.unary_minus()"""), - m( - """ - def __add__(self, other: Translation2d) -> Translation2d: - return self.plus(other)"""), - m( - """ - def __sub__(self, other: Translation2d) -> Translation2d: - return self.minus(other)"""), - m( - """ - def __mul__(self, scalar: float) -> Translation2d: - return self.times(scalar)"""), - m( - """ - def __truediv__(self, scalar: float) -> Translation2d: - return self.div(scalar)"""), - // conversions - m( - """ - def to_pose2d(self, rotation: Optional[Rotation2d] = None) -> Pose2d: - \"\"\"Convert to a Pose2d, optionally with a rotation (defaults to 0 deg).\"\"\" - return Pose2d( - translation=Translation2d(x=self.x, y=self.y), - rotation=rotation if rotation is not None else Rotation2d(), - )""")); - } - - // ========================================================================= - // Pose2d - // ========================================================================= - - private static void registerPose2dMethods() { - PythonGenerator.registerExtraMethods( - Pose2d.class, - m( - """ - def plus(self, other: Transform2d) -> Pose2d: - \"\"\"Apply a transform (equivalent to transform_by).\"\"\" - return self.transform_by(other)"""), - m( - """ - def transform_by(self, transform: Transform2d) -> Pose2d: - \"\"\"Apply a Transform2d to this pose (WPILib transformBy).\"\"\" - t_trans = transform.translation if transform.translation else Translation2d() - t_rot = transform.rotation if transform.rotation else Rotation2d() - new_translation = self.translation.plus(t_trans.rotate_by(self.rotation)) - new_rotation = self.rotation.plus(t_rot) - return Pose2d(translation=new_translation, rotation=new_rotation)"""), - m( - """ - def translate_by(self, translation: Translation2d) -> Pose2d: - \"\"\"Translate this pose by a Translation2d (no rotation change).\"\"\" - return Pose2d( - translation=self.translation.plus(translation), - rotation=Rotation2d(degrees=self.rotation.degrees), - )"""), - m( - """ - def rotate_by(self, rotation: Rotation2d) -> Pose2d: - \"\"\"Rotate this pose by a Rotation2d.\"\"\" - return Pose2d( - translation=self.translation.rotate_by(rotation), - rotation=self.rotation.plus(rotation), - )"""), - m( - """ - def rotate_around(self, point: Translation2d, rotation: Rotation2d) -> Pose2d: - \"\"\"Rotate this pose around a given point.\"\"\" - new_translation = self.translation.minus(point).rotate_by(rotation).plus(point) - new_rotation = self.rotation.plus(rotation) - return Pose2d(translation=new_translation, rotation=new_rotation)"""), - m( - """ - def relative_to(self, other: Pose2d) -> Pose2d: - \"\"\"Express this pose relative to other's coordinate frame.\"\"\" - inv_rot = other.rotation.unary_minus() - delta = self.translation.minus(other.translation).rotate_by(inv_rot) - new_rot = self.rotation.minus(other.rotation) - return Pose2d(translation=delta, rotation=new_rot)"""), - m( - """ - def inverse(self) -> Pose2d: - inv_rot = self.rotation.unary_minus() - inv_trans = self.translation.unary_minus().rotate_by(inv_rot) - return Pose2d(translation=inv_trans, rotation=inv_rot)"""), - // conversions - m( - """ - def to_pose3d(self, z: float = 0.0) -> Pose3d: - return Pose3d( - translation=Translation3d(x=self.translation.x, y=self.translation.y, z=z), - rotation=Rotation3d(yaw=self.rotation.degrees), - )"""), - m( - """ - def to_translation2d(self) -> Translation2d: - return Translation2d(x=self.translation.x, y=self.translation.y)""")); - } - - // ========================================================================= - // Transform2d - // ========================================================================= - - private static void registerTransform2dMethods() { - PythonGenerator.registerExtraMethods( - Transform2d.class, - m( - """ - def plus(self, other: Transform2d) -> Transform2d: - \"\"\"Compose two transforms.\"\"\" - return Transform2d( - translation=self.translation.plus(other.translation.rotate_by(self.rotation)), - rotation=self.rotation.plus(other.rotation), - )"""), - m( - """ - def inverse(self) -> Transform2d: - inv_rot = self.rotation.unary_minus() - inv_trans = self.translation.unary_minus().rotate_by(inv_rot) - return Transform2d(translation=inv_trans, rotation=inv_rot)""")); - } - - // ========================================================================= - // Rotation3d - // ========================================================================= - - private static void registerRotation3dMethods() { - PythonGenerator.registerExtraMethods( - Rotation3d.class, - m( - """ - def plus(self, other: Rotation3d) -> Rotation3d: - return Rotation3d(roll=self.roll + other.roll, pitch=self.pitch + other.pitch, yaw=self.yaw + other.yaw)"""), - m( - """ - def minus(self, other: Rotation3d) -> Rotation3d: - return Rotation3d(roll=self.roll - other.roll, pitch=self.pitch - other.pitch, yaw=self.yaw - other.yaw)"""), - m( - """ - def unary_minus(self) -> Rotation3d: - return Rotation3d(roll=-self.roll, pitch=-self.pitch, yaw=-self.yaw)"""), - m( - """ - def to_rotation2d(self) -> Rotation2d: - return Rotation2d(degrees=self.yaw)""")); - } - - // ========================================================================= - // Translation3d - // ========================================================================= - - private static void registerTranslation3dMethods() { - PythonGenerator.registerExtraMethods( - Translation3d.class, - m( - """ - def plus(self, other: Translation3d) -> Translation3d: - return Translation3d(x=self.x + other.x, y=self.y + other.y, z=self.z + other.z)"""), - m( - """ - def minus(self, other: Translation3d) -> Translation3d: - return Translation3d(x=self.x - other.x, y=self.y - other.y, z=self.z - other.z)"""), - m( - """ - def unary_minus(self) -> Translation3d: - return Translation3d(x=-self.x, y=-self.y, z=-self.z)"""), - m( - """ - def times(self, scalar: float) -> Translation3d: - return Translation3d(x=self.x * scalar, y=self.y * scalar, z=self.z * scalar)"""), - m( - """ - @property - def norm(self) -> float: - return _math.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)"""), - m( - """ - def distance(self, other: Translation3d) -> float: - return self.minus(other).norm"""), - // conversions - m( - """ - def to_translation2d(self) -> Translation2d: - return Translation2d(x=self.x, y=self.y)"""), - m( - """ - def to_pose2d(self, rotation: Optional[Rotation2d] = None) -> Pose2d: - \"\"\"Convert to a Pose2d (dropping Z), optionally with a rotation.\"\"\" - return Pose2d( - translation=Translation2d(x=self.x, y=self.y), - rotation=rotation if rotation is not None else Rotation2d(), - )""")); - } - - // ========================================================================= - // Pose3d - // ========================================================================= - - private static void registerPose3dMethods() { - PythonGenerator.registerExtraMethods( - Pose3d.class, - m( - """ - def translate_by(self, translation: Translation3d) -> Pose3d: - return Pose3d( - translation=self.translation.plus(translation), - rotation=Rotation3d(roll=self.rotation.roll, pitch=self.rotation.pitch, yaw=self.rotation.yaw), - )"""), - // conversions - m( - """ - def to_pose2d(self) -> Pose2d: - return Pose2d( - translation=Translation2d(x=self.translation.x, y=self.translation.y), - rotation=Rotation2d(degrees=self.rotation.yaw), - )"""), - m( - """ - def to_translation2d(self) -> Translation2d: - return Translation2d(x=self.translation.x, y=self.translation.y)""")); - } - - // ========================================================================= - // Transform3d - // ========================================================================= - - private static void registerTransform3dMethods() { - PythonGenerator.registerExtraMethods( - Transform3d.class, - m( - """ - def plus(self, other: Transform3d) -> Transform3d: - return Transform3d( - translation=self.translation.plus(other.translation), - rotation=self.rotation.plus(other.rotation), - )"""), - m( - """ - def inverse(self) -> Transform3d: - return Transform3d( - translation=self.translation.unary_minus(), - rotation=self.rotation.unary_minus(), - )""")); - } - - /** - * Helper to trim text block leading whitespace while keeping relative indentation. Text blocks - * may have leading newlines which we strip. - */ - private static String m(String textBlock) { - // Strip the leading newline that text blocks produce - if (textBlock.startsWith("\n")) { - textBlock = textBlock.substring(1); - } - return textBlock; - } -} diff --git a/src/main/java/frc/robot/util/ts/PythonMethod.java b/src/main/java/frc/robot/util/ts/PythonMethod.java deleted file mode 100644 index 131c50a3..00000000 --- a/src/main/java/frc/robot/util/ts/PythonMethod.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.util.ts; - -import java.lang.annotation.ElementType; -import java.lang.annotation.Repeatable; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -/** - * Defines a Python method to be injected into the generated dataclass. Place on a Java class that - * is processed by PythonGenerator. - * - *

Example: - * - * 

- * @PythonMethod(
- *     name = "add",
- *     returnType = "self",
- *     body = {"_call_hook(self)", "return self"},
- *     comment = "Adds this command to the current auto and returns itself for chaining."
- * )
- *
- */ -@Retention(RetentionPolicy.RUNTIME) -@Target(ElementType.TYPE) -@Repeatable(PythonMethods.class) -public @interface PythonMethod { - /** The method name. */ - String name(); - - /** The Python return type annotation (e.g. "self", "None"). Defaults to "None". */ - String returnType() default "None"; - - /** The method body as individual statements. Each element becomes one indented line. */ - String[] body(); - - /** An optional docstring for the method. */ - String comment() default ""; - - /** Whether this is a static method. Defaults to false. */ - boolean isStatic() default false; -} diff --git a/src/main/java/frc/robot/util/ts/PythonMethods.java b/src/main/java/frc/robot/util/ts/PythonMethods.java deleted file mode 100644 index f6de875e..00000000 --- a/src/main/java/frc/robot/util/ts/PythonMethods.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.util.ts; - -import java.lang.annotation.ElementType; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -/** Container annotation for repeatable {@link PythonMethod} annotations. */ -@Retention(RetentionPolicy.RUNTIME) -@Target(ElementType.TYPE) -public @interface PythonMethods { - PythonMethod[] value(); -}