Miabot Pro PID Controls

From Merlin Systems Wiki

A condensed version of the PWM calculation used in the Miabot Pro, to show how it works + how the EEPROM parameters operate (see also Miabot Pro Parameter Summary).

• all of this is done independently for the two wheels
• the input is a 'clicks' (wheel encoder) count read from the IO processor
• the output is a 'pwm' value (signed byte) which is sent to the IO processor
• all pwm calculations are performed in a "long" (signed, 4-byte fixed-point), for speed. For precision, values are scaled up by a power of 2, and down-shifted at the end to give the actual output value (see last part of code)

The basic 'targetspeed' value is set by the various robot movement commands

-- process new 'clicks' value from io processor
old_clicks = clicks
clicks = io_newclicks
clixrate = clicks - old_clicks

-- convert to 'speed' value (approx 0-1000)
old_speed = speed
speed = clixrate * GETVAR(".pVg")
speed_change = (speed - old_speed)
speed_error = (targetspeed - speed) //+ve = speed up

-- calculate position error (cumulative speed)
dist_error = dist_error + speed_error
if (dist_error > GETVAR(".pMI"))
  dist_error = GETVAR(".pMI")
else if (dist_error < -GETVAR(".pMI"))
  dist_error = -GETVAR(".pMI")
endif

-- form open-loop pwm calculation from wanted speed
if (targetspeed == 0)
  pwm = 0
else if (targetspeed > 0)
  pwm = GETVAR(".poK") + targetspeed * GETVAR("poG")
else
  pwm = -GETVAR(".poK") + targetspeed * GETVAR("poG")
endif

--add in PID terms
pwm = pwm + GETVAR(".pP") * speed_error 
pwm = pwm + GETVAR(".pI") * dist_error
pwm = pwm + GETVAR(".pD") * speed_change

--scale down and clip to one-byte output range
pwm = pwm >> GETVAR(".p>>")
if (pwm > 127)
  pwm=127
else if (pwm < -127)
  pwm = -127
endif

Notes:

1. the pwm value is based on an 'open-loop' value which the error terms are then added to. For the ordinary Miabot Pro, this is actually not a great deal of use as the friction is very low and power levels don't vary a lot between just-moving and high speed. But hopefully this reduces errors overall and prevents gains being too high
2. the 'targetspeed' input value is subject to adjustment by various other factors:

• "speed ramp" code limits acceleration to avoid wheelspin
• "position" brings robot to a stop at a precise point (wheel distance)
• "anti-squeal" cuts power when close to a desired endpoint but no longer moving
• "speed timeout" abandons continued movement when no communications are received for an extended period