Control Laws

This page covers some of the equations I used in building Snuffy.   They are portrayed in block diagram format (as that's what I have available at this time).

 Please excuse the fact that none of them fit conveniently on a screen (unless maybe you have 1024 wide), but I don't have time to downsize them right now ..and just shrinking them looks horrible.

MOTOR ACCELERATION/DECELERATION:

As part of the initialization of each operating mode, several numbers are assigned which are used in these equations.  These include:
        Vmax:   the highest speed the robot should accelerate to.
        Vfinal:   the speed the robot should be at at the end of the mode
        dist:  the distance the robot is to go forward in this specific mode

Other variables are:
        Vcmd: the current commanded speed (initialization is the value carried over from previous operating mode.
        AccMax: a constant representing maximum acceleration (currently 50 inches per second^2)
        AccCmd:  the currently commanded acceleration.
        DistCmd:  the distance the robot is commanded to have traveled at the current time.

How it works:

    The calculation at the lower left corner compares the current value of Vcmd to the maximum desired (Vmax).  If Vcmd is less than Vmax, the integrator causes Vcmd to ramp up to Vmax at the rate AccMax.  At the same time, the acceleration command is picked off as AccCmd.

    Now, the upper left corner continuously computes how fast the robot would have to decelerate to get down to Vfinal by the time it reaches dist.  The required deceleration will increase as the robot approaches the destination dist; and when the required decel reaches 50 ips^2, a flag is set called decelflg.

     When decelflg becomes set, the acceleration equation at lower left is turned off, and a deceleration equation is performed on the top.  This ramps Vcmd down from the current value at the necessary deceleration to reach Vfinal at dist.  This outputs a ramping down Vcmd and the associated value of AccMax.

    Note that there is some logic on the decelerating Vcmd integrator such that as Vcmd gets below 10 ips (P.S., most of my values are in tenths of an inch - hence, Vcmd = 100 is 10 ips), the Vcmd and AccCmd  are set to zero and DistCmd is set to the final distance.  This was to avoid some calculation errors that occurred as the robot got very slow and cause it to waste time sorting it out...very bad in a timed contest.  This logic is not so much a feature as a work around.

    Finally, either the accel or decel values of AccCmd and Vcmd are passed on (to the motor control following), and the Vcmd signal is integrated to provide the commanded distance the wheels should have traveled. 

    The final note on the right modifies the equations if the motors do not have enough speed or torque to keep up with the commands.  If PWM is at 100% duty cycle, a wheel will start to fall behind the command which, over time, can cause large discrepancies between the command and the real world.  Hence, the DistCmd is reduced to the value which has been achieved by the weakest (farthest behind) motor.  This ensures that DistCmd always is very close to the actual robot position.

MOTOR PWM CONTROL:

    The motor equations take in Vcmd, AccCmd and DistCmd signals equivalent to the ones computed in the previous diagram.  The only difference is that the operating mode itself changes the general AccCmd, Vcmd and DistCmd into specific signals for each wheel.  Hence, the suffix of  L or R above. It does this by adding and steering commands to the left and right side as necessary; and, if the operating mode commands a pivot, it will make VcmdL and VcmdR opposite signs (as well as the other two terms).

    So, the above diagram shows both the left and right wheels.  I'll describe the left one on the top.  It receives a command of AccCmdL, VcmdL and DistCmdL.  It also receives a feedback from the wheel encoder which computes a DistAct (distance actual).

    Some experimentation found gain factors which provided the PWM necessary to meet a commanded acceleration and the PWM necessary to maintain a speed. Hence, the first two terms compute the PWM necessary to maintain the commanded acceleration and speed.  VcmdL * KvL + AccCmdL * KaccLs.   note: the s suffix on the last gain is because I found different gains depending on whether the robot was commanded to go straight or to pivot; due to the different inertias, I presume. 

    The next term down compares the commanded distance (DistCmdL) to the actual distance (DistActL).  This is the P part of PID equations.  Neither the I nor D terms were found necessary for satisfactory performance.  The distance error is multiplied by KdistL to give the proportional command.  This was added to the total PWM command.

    Now, the PWM command has to be limited to 100%; which gets a little messy.  I determined there was a minimum PWM necessary just to get the motor to move.  A small command might not cause enough PWM command to move right away (although eventually the proportional error from distance would build up and cause motion).  Hence, anytime I had a command, I started the PWM at the minimum value which I called PwmbaseL.  This required limiting the commanded PWM to 100% minus PwmBaseL, and then adding PwmBaseL back in later.

    The equations compute positive PWM for forward and negative for reverse.  Hence, the last part of the diagram determines the direction commanded and sets the direction bit to the Hbridge; and changes the PWM command from negative to positive if necessary.

WALL FOLLOWING:

    This is, unfortunately, a fairly complex diagram.   Essentially, it computes the robot's direction (hdg) and distance from the wall (Y) using encoder data (dead reckoning).  Then, if IR range data to the walls is available (angest is the angle relative to the wall, and Yest is the distance from the wall), the deadreckoning values are "slowly" corrected to agree with the IR measured data. 

    The reason for "slow" correction is that the encoder data is most accurate in the short run; and the IR data is more accurate in the long run.  However, the IR data is susceptible to having erroneous values occasionally.  Having the slow correction ensures that short term noise on the IR signals won't cause too large an effect on the output values.

    You can see above that hdg is initially computed from the difference in position of the left and right wheels.  Y is computed from the hdg times the distance traveled.

    Finally, a steering command is generated which attempts to reduce heading to zero, and Y to zero (Y is computed to be zero at the center of the hall or 9 inches from a single wall) and also has an integral term (the I in PID) which attempts to minimize the errors.  The resulting steering command is then added to DistCmd to get DistCmdL and subtracted from DistCmd to get DistCmdR.  this happens in the operating mode code which isn't shown here.