Finished product!
The balancing of an inverted pendulum is a classic problem in control theory. We decided to investigate this problem with a robotic system based on the design of Bill Sherman, presented at http://home.comcast.net/~botronics/balibot.html. We used two IR range finders to determine the distance to the ground from both sides of the top of the robot – the difference between these two readings gives an indication of the direction and magnitude of the robot’s tilt. The robot seeks to maintain its balance by rotating its wheels using servo motors, thus moving the base of the robot back under the tilting top. Electronically, a PIC microcontroller is used to read the sensor values and output the appropriate voltage to the servos. Batteries are used to provide power to the sensors, PIC, and servos.
We used two Futaba S148 servos to drive the two wheels of the vehicle. In general, servos are designed to only rotate through 180 degrees; the motor is instructed to move to a certain angle, and the control system inside the servo moves the shaft to reach the desired position. Our servo was modified by Parallax to allow for continuous rotation by removing the control system, so that sending a high angle command results in endless rotation in one direction and a low angle command results in endless rotation in the other direction.
The angle command is sent using Pulse Width Modulation (PWM). Rather
than encoding the command in the amplitude of the signal line, the angle is
set by the length of a high pulse sent every ~20 ms. A pulse of 1.5ms
sets the servo to the neutral position (90 degrees), while longer and shorter
pulse lengths set the shaft to higher and lower angles, respectively.
(image from http://www.servocity.com/html/how_do_servos_work_.html)
One drawback of using the servos for continuous rotation is that we were not
able to control the speed of the motors. Sending any angle command above
90 degrees (pulse length above 1.5ms) resulted in forward motion of the same
speed, regardless of the length of the pulse. This made the servos
somewhat unsuitable as a precise robotic control system.
Moreover, the absence of a pulse in a 20-30ms time window will cause the
wheels to stop turning. The wheels do not continue to spin with the
momentum of the robot. This exacerbates the problem of creating a
control system, since not sending a pulse sends a force to stop moving.
We used two Sharp GPY0A02YK infrared distance measuring sensors to determine
the tilt of the robot. This sensor measures the distance to a nearby
object by using triangulation. A pulse of IR light is emitted from one
side of the sensor, and the reflected light is focused by the receiver lens
onto a small CCD array. This allows the sensor to determine the angle of
the reflected light and calculate the object's distance using basic
trigonometry.
(image from http://www.acroname.com/robotics/info/articles/sharp/sharp.html)
The voltage reading from the sensor is correlated to distance in a non-linear
way, and the function is actually not one-to-one if you allow for objects that
are within a cutoff distance from the sensor. The voltage vs. distance
graph is supplied in the sensor datasheet:
Watch the voltage levels change!
http://www.youtube.com/watch?v=I3nQ633gx-g
| Program Memory Size (Kbytes) | 3.5 |
| RAM (bytes) | 128 |
| Data EEPROM (bytes) | 256 |
| # of I/O pins | 12 |
To program our microcontroller, we use the Microchip PicKit1 Microcontroller
Programmer Development Kit.
This package includes the MPLAB IDE v7 software which we used to interface
with the microcontroller. An additional C language library and compiler
add-on from the HI-TECH PICC compiler package enabled us to program
in C instead of native ASM.
The PIC16F684 has two main programmable ports, PORTA and PORTC. To run
the main functions of our robot, we utilized 4 pins aside from Vdd and
GND: 2 input pins to read in the voltage levels of our infrared distance
sensors, and 2 output pins to independently control, using PWM, each servo
motor-controlled wheel.
The 2 output pins (PIN11 & 13) are on PORT A. Once these pins are designated as output pins, we are able to send either a high or low signal indicated by a 1 or 0 on the PORTA register which will produce a voltage level of ~0V when low and ~4.5V when high (this value varies on Vdd).
The 2 input pins (PIN7 & 8) are on PORT C. Additionally, they are special analog input pins on register ANS. This enables them to be used as analog inputs for the onboard Analog-Digital Converter (ADC). The ADC works by comparing the input level voltage with a reference voltage (default Vdd, but this may be set to an external reference level) and then returning a 10-bit number indicating the measured voltage level, with 1111111111 (binary) indicating a reading of a voltage level equal to Vdd, 0000000000 (binary) indicating a reading of zero voltage, and any binary number in between would be a proportional indication of the measured voltage level with respect to the reference voltage.
The documentation which comes with the microcontroller is somewhere between incomplete, wrong, and childish. It is utterly useless. The time spent just figuring out how to do very simple things is vast. The below is much better documentation.
Of course, this document only contains specific information for this chip and
type of chip. If you are working with another chip with poor
documentation, then there are a few steps you can follow. First, find
code for your chip online somewhere. Search forums, the chip's name,
product name, etc. Find some example code anywhere. This will give
you an idea of some of the functions you can call and some of the settings on
your chip. Ideally, you will find some code which does what you want,
such as grab input or send an output signal. Remember to avoid assembly
language at all costs. Assembly is hard enough to debug on its
own. Debugging assembly on alien hardware is even more difficult.
Some assembly functions are translated literally from assembly into C.
So, CLRWDT becomes CLRWDT().
The 16F864 microcontroller natively takes a flavour of the Intel assembly language. Fortunately, a plugin to MPLAB allows an old C standard to be used which compiles into the requisite assembly code.
The I/O ports can be either inputs or outputs, and can be either on or off. These settings are controlled by changing the bit values of specific memory addresses on the chip. In C, you accomplish this by first importing the chip's header file. This sets some constants to point at the specific memory addresses. Then, simply set the bits of that constant to the desired values. So, for example, to set ports 11, 12, and 13 as outputs and to turn them on, call:
TRISA = 0b11111000; //Setting a bit to 1 designates a port as an input, 0 as an output
PORTA = 0b00000111; //Setting a bit to 1 raises the output to high, 0 sets output to low
where TRISA and PORTA point to the appropriate address spaces.
The Watch Dog Timer (WDT) is a feature built into the chip which will reset the program in case that a loop takes too long to complete. To prevent the WDT from resetting your program, you must continually "kick the dog" (call CLRWDT(); ) in any loop which may take too much time. This, however, significantly reduces performance.
To disable the WDT, simply call
__CONFIG (WDTDIS);
at the beginning of the file.
In addition, one can take input values from these memory addresses. To accomplish this, first set everything up. The following sets up pins 7 and 8 to receive analog input and convert it to digital:
TRISC = 0b11111111; // set register C to inputs
ANS6=1; ANS7=1; // set ANS6(RC2)-pin8 and ANS7(RC3)-pin7 to analog inputs
VCFG = 0; //set voltage reference to Vdd
ADCON1 = 0; //set A/D clock oscillator to fastest
CHS2=1; CHS1=1; //Select PIN RC2(ANS6)
ADFM = 0; //left justified of 10-bit resolution
ADON = 1; //turn on A/D module
Now, to actually read the input into an integer input_value:
unsigned int input_value;
CHS0=0; //which input to use, 0 for PIN7, 1 for PIN8
GODONE=1; //start conversion
while(GODONE==1) {} //wait for conversion to be over
input_value = ADRESH; //get digital level value
where ADRESH are the most significant 8 bits which the analog to digital converter reports from the input. The first 2 bits of ADRESL contain the remaining least significant 2 bits of the input.
The pulse width modulation controlled motors require precision control over
timing. Each pulse must be a precise width, as well as each pause
afterward. Unfortunately, the internal clock in the microcontroller is
both inaccurate and difficult to access. From the code I have seen
online, most people do not use the clock. Rather, they simply time a
loop to repeat a certain number of times. Given an X Khz clock rate, the
time it takes to complete the cycle is fairly consistent. Below is our
wait() function used for timing. On our chip, wait(60) is approximately
1.5ms.
function wait(int steps) {
int i;
for(i=0; i<steps; i++);
}
Our software design is functional and minimalist. At every opportunity, we abstracted away the ugly vestiges of assembly. All of the pins are renamed by macros (e.g. PIN8, PIN11, PIN12, PIN13...). We avoid any and all magic numbers. For example, to measure a 20ms pause after a pulse, we simply calibrate the number of steps required for a 20ms stop, then set a macro TWENTY_MS to be this number of steps. Then, simply call wait(TWENTY_MS) to wait an appropriate amount of time. Below are some of the functions in our API:
pulse();
open_inputs();
open_outputs();
wait(int time);
grab_input();
move_forward();
move_backward();
turn_left();
turn_right();
spin_left();
spin_right();
backup_left();
backup_right();
right_if_fallen();
This greatly simplifies the coding process and eliminates the possibility of most bugs.
