From c7038613368727d82928373d4fde31b514851dcd Mon Sep 17 00:00:00 2001 From: Chris Date: Mon, 25 Nov 2024 08:51:38 -0600 Subject: [PATCH] mas docs --- .docs/architecture.graphml | 1027 +++++++++++++++++ .docs/autoSequencerV2.md | 48 + .../closedLoopDrivetrainControl.md | 0 .docs/docs.md | 9 + {docs => .docs}/drivetrain.md | 0 {docs => .docs}/drivetrainControlStrategy.md | 0 {docs => .docs}/localization.md | 0 .docs/navigation.md | 85 ++ .docs/profiledController.graphml | 975 ++++++++++++++++ {docs => .docs}/utilites.md | 0 {docs => .docs}/vsCodeDeployment.md | 0 docs/autoSequencer.md | 6 - docs/navigation.md | 0 navigation/forceGenerators.py | 6 +- 14 files changed, 2147 insertions(+), 9 deletions(-) create mode 100644 .docs/architecture.graphml create mode 100644 .docs/autoSequencerV2.md rename {docs => .docs}/closedLoopDrivetrainControl.md (100%) create mode 100644 .docs/docs.md rename {docs => .docs}/drivetrain.md (100%) rename {docs => .docs}/drivetrainControlStrategy.md (100%) rename {docs => .docs}/localization.md (100%) create mode 100644 .docs/navigation.md create mode 100644 .docs/profiledController.graphml rename {docs => .docs}/utilites.md (100%) rename {docs => .docs}/vsCodeDeployment.md (100%) delete mode 100644 docs/autoSequencer.md delete mode 100644 docs/navigation.md diff --git a/.docs/architecture.graphml b/.docs/architecture.graphml new file mode 100644 index 0000000..c8e16ef --- /dev/null +++ b/.docs/architecture.graphml @@ -0,0 +1,1027 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + DriverControl + + + + + + + + + + Folder 2 + + + + + + + + + + + + + + + + XBOXCtrl + + + + + + + + + + + + + + + + DrivetrainCtrl + + + + + + + + + + Folder 4 + + + + + + + + + + + + + + + + + + + Pose Estimation + + + + + + + + + + Folder 4 + + + + + + + + + + + + + + + + Gyro + + + + + + + + + + + poseEstPhotonCamera (FL) + + + + + + + + + + + Inverse +Kinematics + + + + + + + + + + + WPILib Pose Estimator + + + + + + + + + + + poseEstPhotonCamera (FR) + + + + + + + + + + + poseEstPhotonCamera (BL) + + + + + + + + + + + poseEstPhotonCamera (BR) + + + + + + + + + + + Telemetry + + + + + + + + + + + + + + + Control Strategies + + + + + + + + + + Folder 2 + + + + + + + + + + + + + + + + + + + Autonomous Trajectory Following + + + + + + + + + + Folder 2 + + + + + + + + + + + + + + + + trajectory.py + + + + + + + + + + + holonomicDriveController.py + + + + + + + + + + + Path Planned +Auto Command + + + + + + + + + + Telemetry + + + + + + + + + + + + + + + Teleop AutoDrive Navigation + + + + + + + + + + Folder 2 + + + + + + + + + + + + + + + + autoDrive.py + + + + + + + + + + + holonomicDriveController.py + + + + + + + + + + + repulsorFieldPlanner.py + + + + + + + + + + + forceGenerators.py + + + + + + + + + + + obstacleDetector.py + + + + + + + + + + + obstaclePhotonCamera +(Front) + + + + + + + + + + + obstaclePhotonCamera +(Rear) + + + + + + + + + + + Telemetry + + + + + + + + + + + + + + + + + Base Control + + + + + + + + + + Folder 2 + + + + + + + + + + + + + + + + SwerveModuleCtrl BR + + + + + + + + + + + Forward +Kinematics + + + + + + + + + + + SwerveModuleCtrl BL + + + + + + + + + + + SwerveModuleCtrl FR + + + + + + + + + + + SwerveModuleCtrl FL + + + + + + + + + + + Telemetry + + + + + + + + + + + + + + + + + LED Control + + + + + + + + + + Folder 2 + + + + + + + + + + + + + + + + PWMControl (LED Color/Blink) + + + + + + + + + + + + + ActualPose + + + + + + + + + + + + + Module State +Command + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Measured Azmth Angles +and Wheel Speeds + + + + + + + + + + + + + + Measured Poses +from Apriltags + + + + + + + + + + + Measured +Angular +Velocity + + + + + + + + + + + + + Measured +chassis speeds + + + + + + + + + + + Est Pose + + + + + + + + + + + + + IsAutoAligning +IsStuck + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + DrivetrainCmd + + + + + + + + + + + + DrivetrainCmd + + + + + + + + + + + + Repulsive +and Attractive +Forces + + + + + + + + + + + + Estimated Obstacle +Poses + + + + + + + + + + + + Estimated Obstacle +Poses + + + + + + + + + + + + DrivetrainCmd + + + + + + + + + + + + All Current +Obstacle Observations + + + + + + + + + + + + + + Des Pose/Vel + + + + + + + + + + + + + + obstacle observations +Goal Pose + + + + + + + + + + + + + + des Pose/Vel + + + + + + + + + + + + + + cmdVel + + + + + + + + + + + + + + cmdVel + + + + + + + + + + + + + + des Pose/Vel + + + + + + + + + + + + + + AutoDrive +Goal + + + + + + + + + + + + + + Manual +Gyro Reset + + + + + + + + + + + + Current Trajectory + + + + + + + + + + + + Obstacles +Lookahead Trajectory + + + + + + + + + + + + DesPose + + + + + + + + + + + + + + + + + + + + + + diff --git a/.docs/autoSequencerV2.md b/.docs/autoSequencerV2.md new file mode 100644 index 0000000..83cad69 --- /dev/null +++ b/.docs/autoSequencerV2.md @@ -0,0 +1,48 @@ +# AutoSequencer V2 + +## Goals +Better flexibility for expressing complex sequences of autonomous routines + +## Key Updates from V1 +Rename `Event` to `Command` for better alignment with wpilib command based + +Introduce `CommandGroup` as an ordered list of `Command`s to be run together + +Introduce the following flavors of groups: +* `CommandRaceGroup` + * member commands run at the same time, finishing when the FIRST one is done + * Extends `Command`, with pass-trough on all functions to do the same thing on all commands in the group, except for `isDone()` implemented as an OR +* `CommandParallelGroup` + * member commands run at the same time, finishing when ALL are done + * Extends `Command`, with pass-trough on all functions to do the same thing on all commands in the group, except for `isDone()` implemented as an AND +* `CommandSequentialGroup` + * member commands run one after another + +Requirements for `Command` +* Abstract (extender-must-implement) methods for: + * `initialize` - called once right before the first call to `execute` + * `execute` - Called at a rate as long as the command is active + * `end(boolean interrupted)` - called once right after the final call to `execute`. `interrupted` indicates whether the end was due to this command finishing "naturally" or because somethign else stopped it prematurely + * `isDone()` - return true if we're done, false otherwise +* Commands also implement convienence "composition" methods: + * `withTimeout` - returns a `raceWith()` a `WaitCommand` + * `raceWith()` - returns a race group with this command the input commands + * `alongWith()` - returns a parallel group with this command the input commands + * `andThen()` - returns a sequential group with this command and the input command +* Commands can `schedule()` or `cancel()` themselves with the `AutoSequencer()` + +Pre-supplied implementations of `Command`: +* `WaitCommand` - waits the given duration + +Existing requirements for `Mode` +* Singular `CommandSequentialGroup` for all `Command`'s in the mode +* Must provide API to: + * Supply the initial pose + +Existing requirements for `AutoSequencer`: +* Top-level state machine for a command group +* Singleton +* Registration of multiple auto `Modes`, including publishing the available list to NT +* NT-based selection of the current auto event +* Ticks the `Mode`'s `CommandSequentialGroup` forward + diff --git a/docs/closedLoopDrivetrainControl.md b/.docs/closedLoopDrivetrainControl.md similarity index 100% rename from docs/closedLoopDrivetrainControl.md rename to .docs/closedLoopDrivetrainControl.md diff --git a/.docs/docs.md b/.docs/docs.md new file mode 100644 index 0000000..b0273b0 --- /dev/null +++ b/.docs/docs.md @@ -0,0 +1,9 @@ +# About Docs + +These documents are designed to help developers of this codebase understand its architecture and purpose. + +## Editing Diagrams + +`.grapahml` files may be edited with the [yEd graph editor](https://www.yworks.com/products/yed/download#download). + +In general, exporting to .pdf will make viewing easier once the diagram is deemed "correct" diff --git a/docs/drivetrain.md b/.docs/drivetrain.md similarity index 100% rename from docs/drivetrain.md rename to .docs/drivetrain.md diff --git a/docs/drivetrainControlStrategy.md b/.docs/drivetrainControlStrategy.md similarity index 100% rename from docs/drivetrainControlStrategy.md rename to .docs/drivetrainControlStrategy.md diff --git a/docs/localization.md b/.docs/localization.md similarity index 100% rename from docs/localization.md rename to .docs/localization.md diff --git a/.docs/navigation.md b/.docs/navigation.md new file mode 100644 index 0000000..43ac992 --- /dev/null +++ b/.docs/navigation.md @@ -0,0 +1,85 @@ +# Navigation + +The navigation stack is a new-for-2025 development the team made to attempt to add flexible, automatic, on-demand driving to teleop. + +The driving is aware of obstacles, both those assumed always-present on the field, and one dynamically detected with cameras. + +## General Principals + +The underlying algorithim is called "Repulsor Fields" or "Potential Fields". See [this Columbia University Lecture](https://www.cs.columbia.edu/~allen/F17/NOTES/potentialfield.pdf) for a brief introduction. + +The planner maintains a list of all obstacles on the field, as well as a goal position. + +Every loop, the robot's last desired pose is used to calculate the force each obstacle or goal has on the robot. + +The sum of these forces produces a vector in a specific direction on the field. The robot path plans one step into that direction. This process is repeated at every loop. + +### Advantages of this Method + +Compared to other solutions, this method... + +1. Is relatively computationally light. It spreads the planning effort over many control loops, only calculating next steps as needed. +2. Does not require complex logic to replan in the background when obstacles change. + +### Disadvantages of this Method + +1. It gets stuck at local minima - Other methods can be more aggressive about looking at the whole map, and providing a reasonable path when "boxed-in" by obstacles +2. Manual Tuning - obstacle/goal force strengths are arbitrary values that were tuned "to taste" in simulation to make paths look nice, and not generate local minima. + +## Force Generators + +Our codebase defines a number of objects which exert force on a given position on the field. + +### Force Shape + +All force generating objects use the [*logistic function*](https://en.wikipedia.org/wiki/Logistic_function). + +Large distances (far away from the obstacle) correspond to large negative values input to the function. Small distances (very close to the obstacle) should produce large positive values. + +While this is not accurate to how forces like gravity work in the real world, it has useful properties for tuning the behavior of the field: + +1. Far away from the obstacle (large negative input values to the function), the strength goes to zero. +2. The X=0 point is always "half strength" +3. By shifting the X axis left/right, the "radius" of effect of the obstacle can be impacted. +4. By stretching or shrinking the Y axis, the strength of the force can be changed +5. By stretching or shrinking the X axis, the "steepness" of the transition from no force to lots of force can be tuned. +6. The function does not go to infinity at any point. + +However, this large number of tuning parameters adds to the complexity of getting a good overall force field, with no local minima. + +### PointObstacle + +Generates a force that radiates outward from the center point, and whose strength is determined by the logistic function of the distance from that point. + +They work well for modeling roughly-circular obstacles (like robots, or posts). + +### HorizontalObstacle + +Generates a force that radiates toward the positive or negative Y axis. The strength of the force is determined by the logistic function of the distance from a given Y coordinate. + +It is useful for keeping the robot away from the longer walls on the field (left or right walls, as viewed from the driver station). + +### VerticalObstacle + +Generates a force that radiates toward the positive or negative X axis. The strength of the force is determined by the logistic function of the distance from a given X coordinate. + +It is useful for keeping the robot away from the shorter walls on the field (red or blue alliance walls). + +### Wall + +Generates a force that radiates outward, perpendicular to a line segment (defined between two points). + +It is useful keeping the robot away from a wall or ramp in the middle of the field. + +Note that there is redundancy in behavior between HorizontalObstacle, VerticalObstacle, Wall types. In the future, this should be removed. + +### Lane + +Generates a force that pushes the robot toward and along a line segment (defined between two points). + +It is useful for marking certain areas of the field as "desireable" to travel through, especially if the default strategy of "go toward the goal" is insufficient. + + +### Goal (TODO) + +This class has not yet been added. The existing functionality is to apply a constant force in the direction of the goal itself. This should be moved into a more well-defined class. \ No newline at end of file diff --git a/.docs/profiledController.graphml b/.docs/profiledController.graphml new file mode 100644 index 0000000..1f65dd4 --- /dev/null +++ b/.docs/profiledController.graphml @@ -0,0 +1,975 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Target + Position + + + + + + + + + + + Profile + + + + + + + + + + + sign(vel) * kS + + + + + + + + + + + vel * kV + + + + + + + + + + + + + + + + + + + + + + + Motor Pos Sensor + + + + + + + + + + + + +- + + + + + + + + + + + Pos Err * kP + + + + + + + + + + + + + + + + + + + + + + + Motor Windings + + + + + + + + + + + On RoboRIO + + + + + + + + + + + On SparkMax + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Target + Position + + + + + + + + + + + Profile + + + + + + + + + + + sign(vel) * kS + + + + + + + + + + + cos(angle) * kG + + + + + + + + + + + vel * kV + + + + + + + + + + + + + + + + + + + + + + + Motor Pos Sensor + + + + + + + + + + + + +- + + + + + + + + + + + Pos Err * kP + + + + + + + + + + + + + + + + + + + + + + + Motor Windings + + + + + + + + + + + On RoboRIO + + + + + + + + + + + On SparkMax + + + + + + + + + + + + Closed-Loop (FeedForward + FeedBack) Architecture for Position Control + + + + + + + + + + + + Closed-Loop (FeedForward + FeedBack) Architecture for Position Control With Gravity Compensation Term + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Target + Position + + + + + + + + + + + Profile + + + + + + + + + + + sign(vel) * kS + + + + + + + + + + + vel * kV + + + + + + + + + + + + + + + + + + + + + + + Motor Pos Sensor + + + + + + + + + + + + +- + + + + + + + + + + + Pos Err * kP + + + + + + + + + + + + + + + + + + + + + + + Motor Windings + + + + + + + + + + + On RoboRIO + + + + + + + + + + + On SparkMax + + + + + + + + + + + + Closed-Loop (FeedForward + FeedBack) Architecture for Position Control With Acceleration Term + + + + + + + + + + + accel * kA + + + + + + + + + + + Unprofiled +Desired Position + + + + + + + + + + + + + + + + + + + + + + + + Des Vel + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Des Pos + + + + + + + + + + + Act Position + + + + + + + + + + + Position Error + + + + + + + + + + + + + FeedForward Voltage + + + + + + + + + + + + + FeedBack Voltage + + + + + + + + + + + + + + + + + + + + + Unprofiled +Desired Position + + + + + + + + + + + + + + + + + + + + + + + + Des Vel + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Des Pos + + + + + + + + + + + Act Position + + + + + + + + + + + Position Error + + + + + + + + + + + + + FeedForward Voltage + + + + + + + + + + + + + FeedBack Voltage + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Unprofiled +Desired Position + + + + + + + + + + + + + + + + + + + + + + + + + + + + Des Vel + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Des Pos + + + + + + + + + + + Act Position + + + + + + + + + + + Position Error + + + + + + + + + + + + + FeedForward Voltage + + + + + + + + + + + + + FeedBack Voltage + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Des Accel + + + + + + + + + diff --git a/docs/utilites.md b/.docs/utilites.md similarity index 100% rename from docs/utilites.md rename to .docs/utilites.md diff --git a/docs/vsCodeDeployment.md b/.docs/vsCodeDeployment.md similarity index 100% rename from docs/vsCodeDeployment.md rename to .docs/vsCodeDeployment.md diff --git a/docs/autoSequencer.md b/docs/autoSequencer.md deleted file mode 100644 index cd1baa0..0000000 --- a/docs/autoSequencer.md +++ /dev/null @@ -1,6 +0,0 @@ -# Auto Sequencer - -## Overall Design Ideas - -## Code Implementation - diff --git a/docs/navigation.md b/docs/navigation.md deleted file mode 100644 index e69de29..0000000 diff --git a/navigation/forceGenerators.py b/navigation/forceGenerators.py index 741cba2..7cccef9 100644 --- a/navigation/forceGenerators.py +++ b/navigation/forceGenerators.py @@ -129,7 +129,7 @@ def getTrans(self) -> list[Translation2d]: Translation2d(self.x,FIELD_Y_M), ] -class LinearForceGenerator(ForceGenerator): +class _LinearForceGenerator(ForceGenerator): """ A linear force generator creates forces based on the relative position of the robot to a specific line segment """ @@ -172,7 +172,7 @@ def getTrans(self) -> list[Translation2d]: self.end ] -class Lane(LinearForceGenerator): +class Lane(_LinearForceGenerator): """ A lane is an attractor - it creates a field force that pulls robots toward and along it, "ejecting" them out the far end. It helps as a "hint" when the robot needs to navigate in a specific way around obstacles, which is not necessarily straight toward the goal. @@ -195,7 +195,7 @@ def getForceAtPosition(self, position: Translation2d) -> Force: return Force(unitX*forceMag, unitY*forceMag) -class Wall(LinearForceGenerator): +class Wall(_LinearForceGenerator): """ Walls obstacles are finite lines between specific coordinates They push the robot away along a perpendicular direction.