Download Modular Subprocessing

Modular Sub-Processing
Case Study: Quick and Easy Position Controller
Shane Colton
Lead Mentor, FRC 97: CRLS/CHS/RSTA/SSCPS with MIT
[email protected], ZZII 527 on the Chief Delphi Forums
NOTICE: As is, the custom circuit described below violates some of the
FIRST custom circuitry rules (R57 and R59) and would therefore be
ILLEGAL IN FIRST COMPETITIONS. Look for a potential revision with a
FIRST-legal version.
Abstract
“Modular sub-processing” is probably not a real phrase in the technical world, because I made it
up. It was the best way I could think of to describe the concept of using individual microprocessors for
robot modules to take much of the computational burden off of the main processor, the Innovation First,
Inc. Robot Controller (IFI RC). Some FIRST teams have already used this method with great success. This
white paper will attempt to demystify some of this control strategy using the example of a simple modular
position controller built for under $30. It is just one example, and I will purposely avoid going into too
much detail on the control theory or programming specifics so that I can simply highlight the benefits of
modular sub-processing in general. Hopefully, the programming contingent of other teams can use ideas
from this article to develop modular control solutions to many other situations.
Normal Speed Control
Before talking about the position controller, I’ll add a brief note about normal speed control with
the IFI RC and a Victor 884 speed controller. Figure 1 shows the normal setup: The IFI RC sends a pulsewidth modulated (PWM) signal to the Victor 884 through the three-wire PWM cable. This pulse width
varies from 1 to 2 milliseconds, with 1 millisecond corresponding to full reverse, 1.5 milliseconds to
neutral, and 2 milliseconds to full forward. The length of the pulse is set in the RC code as a number
between 0 and 254, with 0 giving a 1-millisecond pulse and 254 giving a 2-millisecond pulse. The Victor
884 uses this signal to generate a duty-cycle modulated voltage that will control the speed of the motor.
Figure 1: normal speed control with the IFI RC directly controlling the Victor 884
Position Control Concept
In many situations, it is desirable to have control over the position of a module rather than its
speed. For example, a turret that can be aimed at a particular angle or an arm that needs to stay horizontal
1
when load is applied. Many teams will be familiar with the use of feedback control to achieve this. The
basic idea is that in order to control position, you need to know both the desired position and the actual
position of the module. You then process the error and do something to correct it. Knowing the desired
position is easy: it could be the position of a joystick, for example, or a pre-programmed angle. Getting the
actual position requires some kind of sensor that will give feedback to the processor. Potentiometers,
encoders, accelerometers, and gyroscopes are all good examples. There are many algorithms for
implementing feedback control, and plenty of other white papers go into this, so I will avoid it here. I will
use a PI (proportional + integral) control scheme to make the position controller.
Adding in a Modular Sub-Processor
The IFI RC can definitely handle feedback control. It has analog and digital inputs for sensors and
is fast enough to process the result. However, its speed is not unlimited and many teams have pushed it to
the upper bound by having it process interrupts from several encoders, do analog to digital conversions for
accelerometers, gyroscopes, and potentiometers, and run feedback control for many modules at the same
time. The goal of modular sub-processing is to use separate microprocessors to do the computationallyintensive tasks like interrupt handling, analog to digital conversion, and floating-point feedback control
calculations. This will free up the RC to handle overall control strategy and round management.
As an example, I set out to build a sub-processing module to handle position control. There are
many commercial servo modules that do the same thing, but my goal is to show how they actually work
and how a team can employ extra processors to come up with creative modular control systems. The basic
setup for my position controller is shown in Figure 2.
Figure 2: Position Control with a Sub-Processor
The only added components are an additional processor and the potentiometer for position feedback. The
“?” block represents the module being driven. Note that the potentiometer can rotate directly with the motor
shaft or after any transmission elements, depending on the desired result. It could also easily be replaced by
an encoder, or any number of sensors that could be used to deduce position.
In this setup, the pulse generated by the IFI RC now corresponds to a position instead of a speed.
For example, a 1.5-millisecond pulse may represent some neutral reference angle. A 1-millisecond pulse
might represent 90º clockwise from the reference angle and a 2-millisecond pulse might represent 90º
counterclockwise from the reference angle. The sub-processor reads in the length of this pulse and
interprets it as the desired position. It also does an analog to digital conversion of the potentiometer voltage
and interprets this as the actual position. It runs the difference between them through a control algorithm,
such as PI, and generates a speed control pulse for the Victor 884 to make the motor turn at a speed that
will help correct this error.
Since the sub-processor handles all of the computationally-intensive control, the IFI RC only has
to generate a desired position and leave it up to the module to achieve that position. This frees up the RC to
deal with just higher-level strategy to determine what that desired position should be. This setup could be
replicated for as many modules as required, subject only to the number of PWM outputs on the RC.
2
An Actual Implementation
To demonstrate that this type of control can be achieved with simple and inexpensive additions to
the regular control setup, I built a position controller based on the model in Figure 2. For the subprocessing unit, I used a PIC 16F877, little cousin of the processors in the IFI RC. Machine Science, now
an official FIRST supplier, provides an excellent online development environment, compiler, and loader for
this chip. Team leaders can obtain free accounts through their FIRST offer. For more details, visit their
FIRST offer website at: www.machinescience.org/firstoffer.html. They sell individual components, like the
PIC 16F877 and serial programmer, as well as kits that are designed for teaching PIC programming to new
FIRST students, with tons of online documentation. (Note that in order to use their web programmer, you
need to purchase a chip with a bootloader installed. One obtained from Digi-Key, for example, would not
work as simply.)
The electrical schematic for my position controller is shown in Figure 3. Its core is the Machine
Science “XBoard” layout (see their website for full documentation), minus the LCD display. It can be built
onto a breadboard for prototyping or soldered onto a PCB for more robust final implementation.
Figure 3: schematic for my position controller, red boxes indicate components added to the standard
Machine Science “XBoard” layout
The Machine Science setup normally runs off a battery pack. To make it FIRST-compatible, a 5V regulator
(top, center) must be used to draw power from the main 12V battery. These are available for around $1 on
Digi-Key (e.g. L7805). Note that the main battery power and ground lines can be very noisy and so extra
capacitance on both sides of the regulator may be necessary. The potentiometer (top-right) should be
attached to whatever rotational position is to be measured and should be wired in such a way that “positive”
motor rotation brings the center pin closer to higher voltage. (Keeping track of positive and negative signs
in feedback control is a nightmare and in general it is best to test everything carefully before running to
avoid positive feedback.) Finally, the PWM cables must be connected to the sub-processor. On these
cables, black is ground and white is the PWM signal. (Red is unused.) Pins C1 and C2 on the PIC 16F877
are linked to its capture/compare module, which is designed to deal with pulses and interrupts. The signal
from the RC is read in through pin C2 and the output pulse to the Victor 884 is generated on C1.
3
I have appended the code used in the sub-processor to measure the input (position) pulse, read the
analog potentiometer value, do the control calculations, and generate an output (speed) pulse. It is under
100 lines (without comments) and can be easily modified for different situations. (For example, the gains
used for the PI control algorithm would vary widely based on the module being driven and some
experimentation would be necessary to tweak it properly.) The interrupt handling is somewhat complicated,
and I will defer explanation to the PIC 16F877 data sheet. For here, I will just provide a pseudo-code
summary of what happens, shown in Figure 4.
Figure 4: pseudo-code diagram showing position controller program flow
Results
To test the position controller, I rigged up a small bench-level prototype using a DC gear motor
directly connected to a potentiometer, with a red flag on the shaft so I could clearly see its position. The
motor was driven by a Victor 884 speed controller. To generate the input pulse, the one which would now
control position, I used a function generator in place of the IFI RC. The experimental setup is shown in
Figure 5.
Figure 5: the bench-level prototype for this position controller
4
The results of the experiment were promising. Even though the setup I was using had a lot of
backlash, making linear control more difficult, I was able to fairly easily tune my gains to get decent
position control. A system under load would more than likely be even easier to control since the preload
would eliminate the backlash. The next step is to test this system on an actual robot module, something I
hope to be able to do this season. I have no doubt that it will work well, though, and I hope more teams can
take advantage of modular sub-processing to enhance their control setup.
Appendix: The Sub-Processor Code
#include <mxapi.h>
// Machine Science library
#define PULSE_OFFSET 988
// min pulse = 988 microseconds
#define
#define
#define
#define
#define
//
//
//
//
//
CAPTURE_RISING_EDGE 0b00000101
CAPTURE_FALLING_EDGE 0b00000100
COMPARE_INTERRUPT 0b00001010
CCP_OFF 0
CAPTURE_VALUE (CCPR1H << 8) + CCPR1L
a few definitions having to do
with the capture/compare/PWM
module configuration. For more
information on these, see the
PIC 16F877 data sheet.
#define OUTPUT 0
#define INPUT 1
// stupid, but I always forget
// which is which on the PIC
#define DELTA_T 0.020
//
//
//
//
//
time step...should be set to the
time between PWM pulses from the
IFI RC (.0262 seconds?), not terribly
important since it will be multiplied
by the integral control gain anyway
#define K_P 0.3
#define K_I 0.5
//
//
//
//
proportional and integral control
gains...will vary widely by system
as a ROUGH guideline,
K_P -> damping, K_I -> springiness
int position_desired = 512;
//
//
//
//
//
//
desired position from IFI RC
0-1023 instead of 0-255
represents the number of microseconds
past 988 microseonds of the input
pulse length
988 + 512 = 1.5 milliseconds
int position_actual;
// from 10-bit potentiometer reading
float error = 0;
float int_error = 0;
//
//
//
//
int speed = 512;
// speed controller output pulse length
// past 988 microseconds
unsigned char getting_position = 0;
unsigned char setting_speed = 0;
// busy flags while waiting for
// position pulse or generating
// speed pulse
void interrupt handler()
// handles three types of pulse
// interrupts...
proportional and integral error
representing offset from desired
position, float for control
calculations
{
if(CCP1IF == 1 && CCP1CON == CAPTURE_RISING_EDGE)
{
TMR1H = TMR1L = 0;
CCP1CON = CAPTURE_FALLING_EDGE;
CCP1IF = 0;
}
5
// rising edge capture:
// reset microsecond timer
// interrupt on falling edge capture
// clear interrupt flag
if(CCP1IF == 1 && CCP1CON == CAPTURE_FALLING_EDGE) // falling edge capture:
{
position_desired = CAPTURE_VALUE - PULSE_OFFSET; // position = captured pulse length
CCP1CON = CCP_OFF;
// turn off capture module
getting_position = 0;
CCP1IF = 0;
}
// no longer busy getting position
// clear interrupt flag
if(CCP2IF == 1)
{
RC1 = 0;
CCP2CON = CCP_OFF;
setting_speed = 0;
CCP2IF = 0;
}
}
// timer matches compare value:
void main(void)
{
TRISC1 = OUTPUT;
TRISC2 = INPUT;
// execution starts here
//
//
//
//
end output pulse on pin C1
turn off compare module
no longer busy setting speed
clear interrupt flag
// configure input and output pins
adc_init(ALL_ANALOG);
// configure AD convertor
GIE = 1;
PEIE = 1;
CCP1IE = 1;
CCP2IE = 1;
//
//
//
//
TMR1ON = 1;
// turn on Timer1 (1MHz)
for(;;)
{
// loop indefinitely
enable interrupt generation for
both capture/compare modules
(one for input pulse measurement
and one for output pulse generation)
getting_position = 1;
CCP1CON = CAPTURE_RISING_EDGE;
while(getting_position == 1);
//
//
//
//
position_actual = adc_read(0);
// read in potentiometer value from
// analog port 0 as actual position
error = position_desired - position_actual;
int_error += error * DELTA_T;
// calculate position difference
// numeric integration of difference
speed = 512 + K_P * error + K_I * int_error;
// calculate speed control signal
// based on proportional and integral
// control
if(speed > 1024)
{ speed = 1024; }
if(speed < 0)
{ speed = 0; }
//
//
//
//
setting_speed = 1;
// busy setting speed
CCPR2H = (PULSE_OFFSET + speed) >> 8;
CCPR2L = (PULSE_OFFSET + speed) & 0xFF;
//
//
//
//
//
set the compare register equal to
the desired pulse length (needs two
8-bit parts) so that an interrupt
will be generated when Timer1
reaches this value
TMR1H = TMR1L = 0;
CCP2CON = COMPARE_INTERRUPT;
RC1 = 1;
while(setting_speed == 1);
//
//
//
//
//
reset Timer1
interrupt on compare match
begin output pulse on pin C1
wait for speed setting busy flag
to be cleared after pulse generation
}
}
6
busy getting position
interrupt on rising edge capture
wait for position pulse busy
flag to be cleared on falling edge
ensure pulse width is betwen
988 and 2012 microseconds (speed
will be added to 988 microsecond
pulse offset)