even more docs

.docs/drivetrainControlStrategy.md

@@ -1,16 +1,30 @@
 # Picking a Drivtrain Control Strategy
 
+There are multiple parts of robot code which might want to control the drivetrain. The following logic is used to determine what is actually controlling it.
+
 ## Arbitration
 
-pass off drivetrain command from unit to unit
+The drivetrain logic uses a "chained" approach to arbitration. Each feature takes in the command from the "upstream" features, has an *opportunity* to modify any part of the command, and passes the result downstream. 
+
+As more automatic assist or control features are added, they should be put into this stack. The further "downstream" in the list they are, the higher priority the feature will have.
 
 ### Manual Control
 
+Manual control takes velocity commands from the driver's joysticks. There is no specific pose desired, only velocity is commanded.
+
+Manual control must always be available. It is the default option if no other feature is taking control.
+
 ### Autonomous Path Following
 
+During autonomous, pre-planned paths are often executed. At each time step, a desired pose and velocity is read out of a file and passed to the drivetrain. This only happens if the autonomous path planning logic is active.
+
 ### Teleop Navigation
 
-### TODO - Others
+The navigation stack can create commands to automatically drive the robot toward a goal, avoiding obstacles. This generates velocity commands, and a desired pose.
+
+### TODO - align to gamepices
+
+This logic should create rotational commands which point the robot at a gamepiece, but otherwise do not alter the X/Y velocity commands.
 
-drive toward gamepiece
+This has not yet been written
 

.docs/localization.md

@@ -0,0 +1,64 @@
+# Localization
+
+Localization is the process of estimating where, on the field, the robot is at.
+
+This "where" is answered by three numbers:
+
+X position - in meters - from the origin
+Y position - in meters - from the origin
+Rotation - in radians - in angular deflection from the positive X axis, toward the positive Y axis.
+
+Collectively, these are known as a "Pose" - specifically, a Pose2d
+
+The sensors on our robot provide clues as to these numbers, but always some inaccuracy. Theses clues provide overlapping information, which we must *fuse* into a single estimate.
+
+We use WPILib's *Kalman Filter* to fuse these pieces of information together, accounting for the fact each has some inaccuracy.
+
+## About Inaccuracy
+
+All sensors measure a particular quantity, and add some amount of random *noise* to that signal. The noise is in the same units as the quantity being measured.
+
+When looking at sensor data, you can often see the signal "jittering" around the real value. This noise can be at least roughly measured.
+
+The most common way to understand this noise is to assume it is random, with a *Gaussian Distribution*. The amount of noise will be proportional to the *standard deviation* of the noise's distribution.
+
+The Kalman Filter takes the standard deviation of each measurement as an input, as a way to know how much to "trust" the measurement. Measurements with high standard deviations have a lot of noise, and are not particularly trustworthy.
+
+Trustworthy measurements will change the the estimate of the robot's Pose rapidly. Untrustworthy measurements will take a lot of time to have an impact on the Pose estimate.
+
+## Data Sources
+
+### Gyroscope
+
+FRC robots almost always include a gyroscope, which measures rotational velocity. By adding up the rotational velocity measurements over time, the sensor measures changes in the robot's angular position.
+
+The gyroscope is one of the most accurate, least-noisy measurements of robot position. It should only drift by a degree or two every minute. However, it only measures one component of pose. Additionally, it is at best relative to a given starting pose, which must be accurate.
+
+### Swerve Module Encoders
+
+The encoders inside the swerve modules (wheel motors, and absolute encoders measuring azimuth position) provide a good estimate of movement. As long as wheels are rolling along the ground (and not slipping), linear displacement can be derived through determining distance rolled from number of rotations, multiplied by the circumference of the wheel.
+
+The wheel encoders are also generally very accurate, with most noise being driven by slippage during high-acceleration maneuvers. Additionally, it is at best relative to a given starting pose, which must be accurate.
+
+
+### AprilTags
+
+Using a camera and a coprocessor, we can estimate our pose relative to the *fiducial markers* that FIRST provides.
+
+These estimates provide all three components of Pose, and are absolute - they do not care whether the initial pose estimate was accurate or not. However, the signal often has a lot of latency (as it takes 100ms or more to get the image, process it, and send the result to the roboRIO). Additionally, their accuracy varies, depending on how far away the observed tag is, and whether or not [the observed pose is impacted by a common optical illusion.](https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html#d-to-3d-ambiguity).
+
+## Initial Position
+
+Software must make an accurate initial assumption of pose. This is done through one of two ways:
+
+### Autonomous Init
+
+Each autonomous routine must provide an assumed starting pose, where the drive team places the robot at the start of the routine.
+
+This pose is returned from the sequencer, and used by the pose estimation logic to reset pose at the start of the match.
+
+### Teleop Gyro Reset
+
+The code support resetting the rotational component of pose to be aligned toward "downfield" - this helps during development or driver practice where autonomous is not run.
+
+This only fixes the rotational component of pose. While rotational position is most important, X/Y translation can only be corrected by looking at an apriltag.

.docs/navigation.md

@@ -82,4 +82,53 @@ It is useful for marking certain areas of the field as "desireable" to travel th
 
 ### 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.
+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.
+
+
+## Repulsor Field Planner
+
+### Transient Obstacles
+
+In general, transient obstacles decay in strength whenever they are not observed, eventually being removed once they have zero strength.
+
+Whenever an obstacle is observed by a camera, the existing list of obstacles is searched to find if any obstacles are close to the new observation. If so, the existing obstacle is reset to max strength, and moved into the observed position. If no obstacle is close enough, a new obstacle is added to the list.
+
+There is a maximum number of observed obstacles - if more than this max are observed, the oldest obstacle is removed from the list.
+
+Fixed obstacles must be tracked in a separate list, as they do not decay and are constant.
+
+### Path Traversal Speed
+
+Two "slowdown factors" are set up:
+
+1) Start Slow Factor - ramps from 0.0 up to 1.0 after the goal changes. This makes sure the commanded position slowly accelerates from a stop, providing a more realistic change in velocity.
+2) End Slow Factor - as the commanded position gets closer to the goal, the commanded speed reduces to slow the robot down as it approaches the goal.
+
+Other than these two factors, the step size will be set to the maximum desired speed of the path planner. This is usually some fraction (~85%?) of the maximum speed of the drivetrain. The bigger this fraction, the less margin the drivetrain has to correct for disturbances.
+
+### Next-Pose Solver
+
+In general, the basic algorithm for going from the current commanded pose to the next one is:
+
+1) Calculate the direction the force on the current commanded pose points in
+2) Take a step of size (max_vel) * (loop time) * (slow_factors) in that direction
+
+However, when the force changes rapidly from one pose to another, instability in the path can result, as the pose bounces back-and-forth across the sharp change in force.
+
+As Larry indicates: the main way to solve this is to take smaller steps.
+
+Right now, we are configured to take smaller "substeps" (half the size of a normal step) in a short loop, until the next pose is at least one full step away from the current commanded pose. Care is taken to rescale this step to the correct length, so the correct velocity is commanded. 
+
+The number of subsets must be limited, to prevent an infinite loop in the case of a local minima.
+
+### Stuck Detection
+
+TODO.
+
+Should be looking at a short history of commanded positions, and determining whether "forward progress" is being made. If the last couple points are at similar spots, declare "stuck".
+
+### Lookahead
+
+This functionality is only active in sim, as it is computationally intensive and (so far) only useful for telemetry and testing.
+
+The lookahead operation plans a number of steps ahead of the position, to help visualize what the pathplanner's behavior will be. These poses are sent to the telemetry logic to display, but are otherwise discarded.