I’ve been working on software for improved lift control (I’m targeting linear lifts but this could also apply to 4-bar and 6-bar arms etc.). When I say improved that of course is subjective, it feels improved to me but you could also argue that it’s really just more (way more) complicated and isn’t worth it.
So we’ve all seen the most basic way of controlling lift motors, in ROBOTC it might look something like this.
while(1)
{
if( abs(vexRT Ch2 ]) > 10 )
{
motor liftMotor_1 ] = vexRT Ch2 ];
motor liftMotor_2 ] = vexRT Ch2 ];
}
else
{
motor liftMotor_1 ] = 0;
motor liftMotor_2 ] = 0;
}
wait1Msec(25);
}
Joystick channel 2 directly controls the motors, we have a small deadband so the motors will stop of the joystick does not return exactly to the zero position.
There are a number of problems with this for all but the very simplest of robots.
-
The lift does not hold it’s current position when the joystick returns to zero. The operator has to keep applying some power which can be difficult and will trip the motor PTC protection if too much is applied.
-
It’s hard to move the lift to an exact and repeatable position.
-
The lift can move too high or too low and damage itself.
In order to improve on this, many teams implement the “infamous” PID control, although in reality what is used is a simple P controller. There’s a post about that type of control here.
Simple P controller for arm position
A generalized diagram showing this type of PID control would look something like this.
We give the control function a target position, the PID controller software compares this position to the existing position of the lift (usually the count from an IME or quadrature encoder) and moves the lift up or down as necessary. Now we have the solution to some of the limitations of the manual controller, we can have preset positions, we can have the lift remain at the target position, if the lift starts to drop the PID controller will increase motor power to compensate. We can add code to limit the movement of the lift to the safe limits, however, the PID controller above also has some drawbacks.
-
Manual movement of the lift has now become difficult, the joystick cannot directly control the motor power and must gradually change the target position of the lift instead. If the rate of change of the target position does not match the dynamic response of the lift it will not move smoothly. (what I mean here is that the lift will accelerate and decelerate based on its physical characteristics, how heavy etc. If the lift can move quicker than you are changing the target position then it will sort of judder as you move it).
-
Gravity can be a problem, tuning the PID controller can be more successful when the lift is moving up (or down) but then not work in the other direction.
-
The PID controller can still send too much power to the motors when the lift is not moving.
There’s also an issue with this type of PID control due to the fact that the motors do not respond to control values in a linear fashion. Sending a control value of 64 to a motor does not make it run at half speed (as compared to a control value of 127 causing maximum speed). We have tried in the past to linearize the motor control values by remapping the output of the PID controller to an actual motor control command using a lookup table (LUT), but this only seems to work accurately with a given load on the motor and needs to be setup for a given design.
So what can we do to improve lift control and have all the functionality we would like. My proposed solution separates motor speed control from motor position control. The block diagram looks like this (same as I posted the other day).
Starting on the left, we first have the position PID controller. The input to this is “target position”, this is almost the same code the the PID control I described before, however, instead of the output driving the motor directly the output is now a target speed for the motor. This block allows for presets but still has the issues related to manual control of the motor.
In the next block we modify the target speed in a couple of ways.
-
We limit the speed to a user determined maximum speed. By doing this we can acheive the manual control we desire, the joystick maps to maximum speed and the target is set as either the upper or lower limit of the lift. The position PID control will try and move the lift to that position but the speed is limited.
-
We control acceleration and deceleration, what I have termed “slew rate” control in the past. We don’t want to try and instruct the motors to go from stop to full speed instantly as they cannot do this and the motor current will be high as they accelerate if you try. So motor speed is ramped over a few hundred mS to reduce absolute motor current during this period.
-
We can reduce maximum speed as we near the absolute upper and lower limits of the lift, even when tuned properly a position PID controller can overshoot the target, if we limit the output then we can smoothly stop at the top and bottom whilst still having full speed for intermediate preset position.
The conditioned motor speed now goes into the motor speed PID controller. This controller creates a motor drive signal by comparing the actual motor speed to the requested target speed. If the target speed is 10 revs per minute (rpm) then this controller will try and maintain that speed. This removes the non linear motor response, the motor control values will be unique for that motor with the current load and battery condition, gravity will be compensated for. The key parameter needed for this to work properly is the actual motor speed, this is where things start to get tricky.
motor speed calculation.
We only have one piece of information coming from the motor, the current encoder count (I’m assuming an IME for this discussion and ignoring the fact that in some programming environments the IME can indicate also give velocity). The only way to calculate speed is to use the change in encoder count over time (we know v = dx/dt).
A 393 motor with standard gearing will give 627.2 counts per revolution (I’m going to use 627 for the calculations). This sounds really high, however, lets suppose we want the motor to be turning at 10 revs per minute, that would be 627 x 10 counts per minute or 6270 counts. In one second this will be 6270/60 or 104.5. We could calculate the motor speed every second by reversing this calculation speed = (104 * 60) / 627; speed would be 9.95 rpm.
For the motor to be successfully controlled we need to know the speed much more often, I’m running the speed control PID loop every 25mS (40 times per second), so the number of counts when the motor is running at 10rpm is 6270/(6040) = 2.6125. The encoder does not give us an encoder count as a decimal, the change in encoder count every 25mS can either be 2 or 3, it will average out to 2.6125 over a long period of time but the instantaneous change can only be 2 or 3. If the change is 2 then speed will be calculated as (24060)/627 = 7.65, if the change is 3 then the speed will be calculates to be (340*60)/627 = 11.48, start to see the problem.
If we plot calculated speed vs time with the motor running at exactly 10rpm the graph would be as follows. The green line is the actual speed, 10 rpm, the red line is the speed calculated from change in encoder position, ignore the blue line for now.
[ATTACH]8484[/ATTACH]
The error in speed calculation causes the PID controller to fight the motor when it doesn’t need to. As the speed decreases this situation gets worse, at 1 rpm we only see a change in encoder count every 100mS, above 10rpm things improve but even at full speed the calculated speed will fluctuate. So somehow we need to improve this and really the only tool available to us is to filter this signal somehow and remove the spikes. I decided to use a simple 4 sample low pass filter of the encoder count change (sum the last four delta values and then divide by 4) rather than filter the calculated speed as this gave a better result. This improvement is plotted as the blue line on the above graph. The calculated speed swings between 9.57 and 10.52 (which when I round to be an integer for subsequent calculations becomes 10 and 11), a 1 rpm error rather than a 4 rpm error should be ok.
Using velocity information from the IME can be a bit more accurate but there are still problems. The IME velocity data is a time value, 1/speed, that measures the time for half a revolution of the encoder gear in the 393 (according to the tech support FAQ) this works well for high speed but has similar issues at slow speed. Once the motor is stopped it takes several seconds to figure this out as the time value will keep increasing until it reaches its maximum value. ConVEX only uses this velocity data if the encoder count has changed, so at slow speeds <4 rpm, we have the same problem, an encoder count delta of zero may or may not mean the motor is stopped.
continued in part 2, I hit the 10000 forum character limit again 
but that’s another story) raise perhaps 1 inch above the needed height, move to the sky rise base and drop the new section into position by lowering 4 inches.