At the heart of a micromouse is usually a pair of position controllers. These maintain the mouse position for one of two kinds of movement – forward and rotation. Each can be configured independently. Like any position controller, the idea is to get a fast, stable response to changes in the demanded position…
Position control by DC motor is a classic control engineering task. However, be absolutely clear about one thing – I am not a control engineer so, if this does not work for you, I am sorry but I can be of limited help. Here is an example of how you might design a common type of position controller. The example system is a simple metal disk attached directly to the output shaft of a Faulhaber 1717SR006 motor with attached 512 pulse encoder. The disk was chosen to present a load similar to that seen by each motor on a small micromouse. The motor is driven using the micromouse board through its motor driver and using the on-board mouse software. For experimenting with the technique, this lets me try several options without the risk of damage to the actual mouse or its motors in the event of everything getting badly out of shape. And, while experimenting with motor controllers, that is quite likely. I have no desire to watch my micromouse fly off the table – again. A future post will show another method and the working for an actual mouse.
The first step in any case is to characterise the system as described in the previous port here.
From the results, I obtain a transfer function for my test rig of:
So, the DC gain, Km, is 142 encoder counts/sec/pwm count and the time constant, Tm, is 0.165 seconds. All the tests were done with a supply voltage of 7.25 Volts. An odd value but that happens to be what the bench supply was set to. the PWM system has a range of +/-1024.
In practice, these constants means that, if I apply a PWM value of 200, I get (7.25*200/1024) = 1.416 Volts at the motor and it will turn at (200*142) = 28400 counts/sec = 3330 RPM. The data sheet for the motor tells me that the voltage constant for the motor is 2410 RPM/Volt which would mean I should see 3400 RPM. Close enough. The motor driver will have losses and the actual voltage at the motor will be less than calculated from the PWM value. This is a good validation of the accuracy of the measurement method. The time constant indicates that the motor will take about 3*0.165 = 0.5 seconds to get within 5% of the final speed and about 0.75 seconds to get within 1% of the final value. Now that is not very fast really. This will bee true for any applied voltage. With a higher voltage, the motor will spin faster but it will always take 0.75 seconds to get to 99% of its target speed.
For the mouse, I use position control. This is done by comparing the output position with a set point. The error value is the difference between the two and is used to generate a PWM signal to make the motor move towards its set point. Clearly, if there is no error, there will be no PWM drive and the motor will not move. If the error signal is used as it is, we have a simple unity feedback system.
With the current position (OUT) at zero, we can apply a new set point of, say, 256. This generates a PWM drive of 256 and the motor will start to accelerate toward the new position. As it gets closer, the error decreases and the drive gets less. When the motor is at position 256, there will be no drive. However, by now, the motor is spinning and the inertia of the load makes it want to continue so it will overshoot. The error grows but in the opposite direction and eventually, the motor will slow to a halt and then reverse direction until it is heading once again to the set point. Where it will overshoot again. It should be clear that the system will oscillate about the set point. Losses in the system will cause it to eventually rest at the new set point but that could take a couple of seconds during which time the motor is spinning back and forth.
It might seem a good idea to increase the drive level a bit more to get the motor moving toward its set point faster in the hope it will settle down faster. The problem with this is that it will also pass through the set point faster and be even harder to stop. A DC motor position system like this is inherently stable and will always settle to the set point eventually but it clearly is not much good as a position controller.
The fundamental problem here is that of slowing the motor down as it approaches the set point. What is needed is some kind of force acting in proportion to the speed at which the motor moves. While that would prevent the motor from moving to the set point as quickly as before, it would also help prevent the overshoot so that the motor could get within some acceptable error limit faster than it would with just proportional control. Since speed is the derivative of distance, this kind of controller s called a Proportional-Derivative (PD) controller.
The simplest way to implement such a controller, albeit not necessarily the best, is to keep track of the current error and the previous error and generate a drive signal based on both the error and the rate of change of the error. This is called the parallel method and, no doubt, proper control engineers will tell you it is not good. Not to worry, they have to deal with any number of complex systems. It works just fine here. In code, it looks like this:
errorOld = error; error = setPos - currentPos; PWM = kP * error; PWM += kD * (error - errorOld);
Pretty simple really. The hard part is how to determine the value of the constants kP and kD. Searching around, you will come across lots of places that will try and tell you how to tune systems like this. They are mostly aimed at situations where you have a controller and you have a system but you don’t know the characteristics of either terribly well so that you have to then find a way to work out, often by trial and error, what values to use for the controller parameters. trust me, there is no fun in that and it is best avoided.
For this system, however, the characteristics are known. Only the time constant, Tm, and the DC gain, Km, are needed. First though, a bit about step responses.
By examining the response of a system to a step change in input, it is possible to tell quite a lot about it. Conversely, if the nature of the step response is defined as part of the design, it is possible to create a system to satisfy those requirements. In this instance, like the mouse, we want the response to be quick and accurate. In practice that means, how soon can the system get within some specific tolerance band close to the set point. It is possible to design systems that move smartly up to the setpoint without overshooting but it is faster to allow a small overshoot and put up with the output being a little bit in error for a period of time.
The amount of overshoot is determined by the damping factor, ζ, pronounced ‘zeta’. A typical value for a slightly underdamped system with an overshoot of a couple of percent would be around 0.7. You can look up graphs and tables showing how ζ is related to overshoot. For now, I will just go with a value of 0.7 for ζ.
Having overshot, the system will take a certain amount of time to settle dowd. That time is tds and I will arbitrarily set it at 0.070 seconds. There is no point in making his number too small Whatever the control system, there are physical limits to what the system can achieve. Remember that the motor has a time constant of 0.165 seconds and so how can we arrange for it to get to within some small percentage of any step change in value in only 0.070 seconds. Well, that is what control systems are for.
The settling time and the damping factor between them are determined by the natural frequency, ωc, in radians per second. For this system, we have
We already have values for Km and Tm so:
It is not very conveninent to use floating point so each constant can be scaled by 256 and the resulting output divided by 256 afterward. Also note that the derivative value is the derivative for one loop of the control system which, in this case is 0.001 seconds. To get it back to its value for one second, just multiply by 1000. All the variables here are longs (32 bit) since there will be overflows otherwise. Now the code looks like:
kP = 1997L; // 7.8*256 kD = 32256L; // 0.126*1000*256 errorOld = error; error = setPos - currentPos; PWM = kP * error; PWM += kD * (error - errorOld); PWM = (PWM + 128) / 256; // scale back down and round off
How well does that work? The code above was used with the motor and disk combination characterised at the start of this post. The motor was required to perform a 256 count step. That corresponds to half a rotation. During the more, the motor position and PWM output were recorded:
It is perfectly clear that the motor gets within 2% of its goal in under 50ms, exceeding the 70ms settling time requirement – a significant improvement on the unity gain feedback. There is also no appreciable oscillating of the output. In fact, this is a pretty stable system that could probably live with a little higher gain which would speed up the response at the expense of a little more oscillation around the set point. In practice, step sizes of only a couple of encoder counts are likely to be required of the system in any given interval. It should be more than capable of dealing with those. For example, a mouse having 50 counts per mm, might accelerate at 10m/s/s which is only 0.5 counts/ms/ms.
Notice the PWM line. First, the drive system saturates at maximum power for 10ms. If your batteries are not up to much, or if you have losses in the drive system, you might find that the supply voltage could drop dangerously during this period. At best, that means reduced drive and a slower system response. At worst, the processor voltage could drop low enough to reset the processor or at least cause a brown-out event. Be sure to check for these effects or you will have potentially serious problems.
Second, the required PWM is pretty erratic in places. This is, I believe, due to the discrete nature of the encoder feedback. Derivative control is sensitive to noise and the small jumps in encoder values – inevitable as they are discrete values – are amplified by the controller and cause possibly large changes in the PWM output. This is not very helpful but is a feature of this kind of controller. It can be eliminated but it may well not be worth the effort.
Just from curiosity, I doubled the gain, making kP = 15.6 and ran the test again:
You will see that the output manages to get to, and stay within, 12% of the set point in under 30ms. Note also that the first 20ms or so of the response are the same as they were before. After all, the motor drive is saturated and simply has nothing more to give.
Two things to worry about though from a performance point of view. First, notice that, as the motor gets up to maxumim speed and starts to close in on the target, the PWM drive is forced to go negative, almost saturating, in its efforts to slow the motro down in time to prevent excessive overshoot. This system is probably at its limit for this test. A higher drive voltage would help but we do not have one on the mouse.
The second observation is that no account is taken of the speed of the motor. in this test, the motor starts off stationary and finishes stationary. In a mouse, the motor may already be turning at some significant speed and some of the available PWM headroom is being used just to maintain that speed. Further attempts to accelerate or decelerate the mouse may run out of drive sonner than you would expect from a static test like this. It does occur to me, though, that braking is always easier as there will be additional headroom in the direction opposite to the current motion.
This turned out longer than expected so a second post will deal with an alternative control technique that is a bit better suited to the task.