drivetrain and vsCode dev

.docs/drivetrain.md

@@ -2,22 +2,86 @@
 
 ## Swerve Drive Overview
 
+A "swerve drive" is the most common type of drivetrain in the FIRST Robotics Competition in the last few years.
+
+It consists of four "modules", each mounted on a corner of the robot.
+
+Each module has two motors - one to point the wheel in a specific direction ("azimuth"), and one to actually spin the wheel ("wheel").
+
+As a coordinate frame reminder: for our rectangular chassis, the robot's origin is defined as lying in the same plane as the floor, and in the exact center of the rectangle of the drivetrain. Positive X points toward the front of the robot, positive Y points toward the robot's left side. Positive rotation swings from +X to +Y axes.
+
 ## Overall Drivetrain Control
 
-xvel yvel rotational velocity commands
+Our control is "field-oreiented" - the commands into the drivetrain are assumed to be velocity commands aligned with the field walls.
+
+A few steps are needed in the overall control algorithm.
+
+1. Rotate the incoming velocity commands to be relative to the robot's heading, not the field's +X axis. WPILib provides these functions.
+2. Figure out the velocity and azimuth angle _at each module's contact patch with the ground_, using kinematics and the drivetrain dimensions. WPILib provides these functions.
+3. Perform per-module state optimization (see below)
+4. Send specific wheel velocity and azimuth angle commands to each module.
 
-kinematics to module commands
+### Module State Optimization
+
+At any given time, there are four possible ways a module could go from its present azimuth angle to a new commanded one:
+
+. Rotate left, keep wheel velocity the same
+. Rotate right, keep wheel velocity the same
+. Rotate left, invert wheel velocity
+. Rotate right, invert wheel velocity
+
+In this was, the maximum number of degrees a module should ever have to be commanded to rotate through is 90. By optimizing the state, we reduce the amount of time the module is in a "skidding" state, where the wheel is not smoothly rolling across the floor.
+
+WPILib provides the functions to do this, we simply have to call them.
 
 ## Module Control
 
-PID + FF on wheel
+Controlling the module requires controlling both motors.
+
+### Wheel
+
+The wheel velocity is achieved through a basic feed-forward, plus a small feedback portion.
 
-PID on angle
+The feed-forward model is the standard motor veloicty model, consisting of:
+
+`kS` - static friction - maximum voltage that can be applied to the motor without motion occurring.
+`kV` - number of volts to achieve a certain rotational velocity
+
+Future adds include `kA` - number of volts to achieve a certain _change_ in rotational velocity.
+
+Feedforward should be doing the vast majority of the control effort. Feedback is just a small additional factor to help compensate.
+
+### Azimuth
+
+For now, we are just using a simple P/D feedback controller on the azimuth angle. This seems to be sufficent.
+
+Future adds could be to motion profile the commanded position of Azimuth angle, and then using that for a velocity feed-forward.
 
 ## Constants & Configuration
 
-wheel ratios
+As with most good code, striving to minimize "magic constants" throughout the code is perferred.
+
+Most drivetrain related constants are in `drivetrainPhysical.py`. Key ones that users may have to adjust:
+
+. `WHEEL_BASE_HALF_*_M` - Distance from the origin of the robot, out to the center of where the wheel makes contact with the ground. Note this is not to the frame rail or the bumper. Note it's only _half_ the distance between two wheels.
+. `WHEEL_GEAR_RATIO` - Reduction ratio in the modules from the driving motord down to the wheel.
+. `WHEEL_RADIUS_IN` - radius of the wheel from center of rotation, to where it contacts the carpet
+. `MAX_DT_MOTOR_SPEED_RPS` - maximum achievable speed from the drive motor. WPILib has most of these internally in their physical plant model configurations. 
+. `*_ENCODER_MOUNT_OFFSET_RAD` - adjusts for the physical mounting offset of the module/magnet and angle sensor on each drivetrain azimuth. 
+. `ROBOT_TO_*_CAM` - 3d Transforms from robot origin to the camera described (including both translation, and angle)
+
+Other constants are present, but they are tied to things that don't change much year to year (robot weight, azimuth steer gear ratio, bumper thickness, etc.)
+
+Most other constants in the file are derived from these constants.
+
+### Encoder Mount Offset Cal Procedure
 
-effective rolling radius
+Must be updated whenever the module is reassembled
 
+1. Put the robot up on blocks.
+2. Reset all these values to 0, deploy code
+3. Pull up dashboard with encoder readings (in radians)
+4. Using a square, twist the modules by hand until they are aligned with the robot's chassis
+5. Read out the encoder readings for each module, put them here
+6. Redeploy code, verify that the  encoder readings are correct as each module is manually rotated
 

.docs/vsCodeDeployment.md

@@ -1,15 +1,41 @@
 # vsCode Project Setup
 
+A few things were added to our vsCode project to try to make vsCode feel closer to Java's workflow.
+
 ## Setup
 
-what file
+Configuration is saved in `.json` files saved in the `.vscode` folder.
+
+## Taks - `task.json`
 
-what extensions
+`tasks.json` creates menu items for the common commands we run from robotpy. These include:
 
-## What is Supported
+. Running the Test Suite in Simulation
+. Deploying code to the RIO (normal)
+. Viewing netconsole (the stdout forwarder that you see in Driver Station)
+. Deploying the code to the RIO (debug mode)
 
-### Basic Deployment
+## Debug Configurations - `launch.json`
 
 ### Simulation
 
-### Debugging on RoboRIO
+This invokes the robotpy command to do simulation, but invokes it within a python debugger context.
+
+### On-RIO
+
+This first re-deploys the code with special flags to enable debug, waits for the code to start up, then connects to the RIO remotely (which _should_ be running a debug server at that point)
+
+There is a small bit required inside of robot code to support debugging. In `roboty.py` - the function `remoteRIODebugSupport()` should be invoked early on in execution. This function shall:
+
+1. Detect that we're running robot code on a RIO, with debug mode active.
+2. Import the `debugpy` module JIT (it's not needed otherwise and was causing some sim problems).
+3. Enable remote debug connections from any client IP on port 5678
+4. Wait indefinitely for a client to connect
+
+Step 4 is critical to be sure that a breakpoint set early in initlization is not missed. This was funcitonality specifically not present in robotpy in 2024. 
+
+This code is fairly intrusive to robot behavior, and overlaps debug support that robotpy was attempting to add in 2024. It needs review in 2025 to see if it's still needed.
+
+### Test
+
+This invokes the robotpy command to run the test suite, but invokes it within a python debugger context

drivetrain/drivetrainPhysical.py

@@ -20,9 +20,6 @@
 WHEEL_BASE_HALF_WIDTH_M = inchesToMeters(23.75 / 2.0)
 WHEEL_BASE_HALF_LENGTH_M = inchesToMeters(23.75 / 2.0)
 
-# Additional distance from the wheel contact patch out to the edge of the bumper
-BUMPER_THICKNESS_M = inchesToMeters(2.5)
-
 # Total mass includes robot, battery, and bumpers
 # more than the "weigh-in" weight
 ROBOT_MASS_KG = lbsToKg(140)

robot.py

@@ -174,6 +174,7 @@ def endCompetition(self):
         super().endCompetition()
 
 def remoteRIODebugSupport():
+    # TODO - is this still needed in 2025+?
     if __debug__ and "run" in sys.argv:
         print("Starting Remote Debug Support....")
         try: