Quote:
Originally Posted by davidthefat
...do not do it if you are not doing real time calculations. ..."drive forward 10 feet and score" autonomy real autonomy. They are pre-written instructions...
|
How much math is there in a "drive forward 10 feet and score"?
In my code:
The autonomous script would have a command "DRIVE_STRAIGHT 120 6" to drive the 120 inches at 6 ft/sec
The beescript interpreter would find and call the Drive Straight function (drive_straight.vi)
The Drive Straight function would:
Reset the encoders
Calculate the remaining distance to the target plus an overshoot constant
Determine the desired average output speed using a proportional controller
Limit it to the 6 ft/sec
Determine drift by using the differential of the two encoder distances
Determine how much to compensate using another proportional controller
Add the two and take the remainder of the larger number and subtract it from the smaller (so the difference in the motor output is always the number specified by the second P controller)
Ramp the output to prevent lurching (which can cause twisting and innacuracy)
Feed the speed, which is passed to the drive thread which actually drives (closed loop is turned off during auto since auto does its own calculations).
The score would also include:
Setting the elevator state to be fed to the state machine
The state machine would pickup the state request (back in its thread) and lookup the position
The position would be run through the state machine which would see if it is trying to crossover backwards or handle a special sequence. Since it isn't, I'll skip that.
The resulting position would be checked against the active tube color, and if it's a circle (or if it's an ubertube and we're pretending ubertubes are circles) it will add the white tube bump for the current score state
The resulting position will be fed through the P controller to determine power of the elevator and wrist
The gain scheduler will modify the gains above based on several parameters (such as the sine of the angle of the wrist).
The resulting power will be fed through the limits VI which will bring it into range
The resulting power will be fed to the motor if the state machine has been initialized (set) since last exiting disabled (safety feature - no movement after exiting disabled until commanded)
Lots and lots of math.