pathfinder

I've been mulling over the math behind a possible track learning control strategy. The way I see it, there are three stages: positional tracking, track learning, and predictive control (braking) interrupts.

Positional Tracking:
We'll be using our favorite rotation group SO(2) for this. Our unit orientation vector D begins with initial condition [1, 0]. We update this vector whenever the microcontroller obtains a new value for the servo angle Theta (whether this be from instructions given to the servo or from feedback via the ADC). Upon receiving a new Theta, D = R_z(Theta)*D. This requires 4 multiplies and 2 additions since we are in SO(2).
Using D and absolute velocity v (converted from angular velocity from brushless motor), we maintain our global position X = Integrate[D*v,dt] where X is 2x1 with initial condition [0,0]. This should run at every time-step, which requires 4 multiplies (orientation*velocity*time for 2x1) and 2 additions.

Track Learning:
We record a list of straight-aways, ListSX, each element being a pair of initial and final global positions corresponding to begin and end-points of straight-aways. This can be done by maintaining a position variable StraightX with initial condition [0,0]. If the servo angle is beyond a certain threshold angle, and the current global position X is less than a threshold distance away from StraightX, we discard the previous value and update it with current X. If the servo angle is beyond a certain threshold angle, and X is more than a threshold distance away from StraightX, we record (StraightX, X) at the end of ListSX and update StraightX with X.
This is repeated until X goes within a threshold distance away from our initial position [0,0] and D is within a threshold offset of initial orientation [1,0] (this is in case we have a lap cross-over near the origin). This flag signifies the end of a complete lap, at which point we want to begin using our track information.

Predictive Control Interrupts:
The primary PD servo and P braking controller must complete the first lap. Once this is done, this controller still serves as the primary guidance system. However, whenever the global position X goes within a threshold distance of the begin-point of the ith pair in ListSX, the predictive controller interrupts the braking control to instruct the motor for full torque. As soon as X approaches a threshold distance from the end-point of the ith pair of ListSX (this threshold must be larger because the whole point is to predict the end of a straight-away), the predictive controller will again interrupt the braking control to instruct braking proportional to the current speed v. The index is incremented by one to prepare for the next straight-away, until the end of the lap is reached and the index is reset to zero.

This might be a lot of math for the microcontroller to handle at every "timestep" (whether that be a cycle or every several cycles). Rick, how many operations do you think the microcontroller can handle? You guys have any comments on this strategy?