Sensing and Control with FORTHdsPIC
My last two blog posts on FORTHdsPIC featured its ability to provide some basic control for a simple 4-servo ‘humanoid’ robot, and its transfer to a Microchip MicroStick II development board. I wanted to add sensor capabilities and felt these would be easier to demonstrate on a conventional wheeled mobile robot based on the Parallax BoE-Bot hardware. The result is version 0.6 of the software and the need for additions to the user guide.
The trusty WIDE.HK dsPIC Arduino-format board came back into play, plugged into the underside of the Parallax prototyping shield as before. This shield is designed to work with the BoE-Bot chassis and the robot’s battery pack plugs straight into the WIDE board. Well that was pretty easy, particularly as the BoE-Bot has continuous-rotation servo motors that use the same sockets as the standard servo motors of the humanoid robot. The choice of sensors wasn’t difficult – the robot was to be a ‘Photovore’ and lock onto a torch beam aimed along the floor in its direction. For a control system to lock on to anything requires an error signal to be produced and minimised. That means at least two light sensors must be used. The sensors are mounted on the front of the robot and the steering system aims to keep the difference in light signal between them (the error), close to zero. In theory I needed a good old-fashioned PID control algorithm to run on the dsPIC. In practice, I found that the dynamics of the robot were such that the Proportional error feedback component would suffice and the Integral and Differential parts were not necessary for smooth operation. For the light sensor I used a CdS LDR (Light Dependent Resistor) – an ORP12 – as I happen to have a drawer-full of the things. The LDR is connected as the lower half of a potential divider between the 3.3V power rail and ground, with a 4K7ohm resistor as the upper half. An ADC channel on the dsPIC is used to measure the voltage across the LDR: nearly 3.3V dark and close to 0V when brightly lit. So that determined the first addition to FORTHdsPIC software: the ADC driver.
No self-respecting mobile robot would be complete without some form of obstacle avoidance system. The easy way is to use a mechanical contact approach with simple switches connected to a GPIO pin, preferably an interrupt input. This seems very crude however, with the robot charging around in a rather undignified manner crashing into things before changing direction. No, that’s no good, a non-contact method is required so an object can be detected before impact. There are two ways of achieving this; Lidar and Sonar. Lidar uses the intensity of infra-red light reflected off a surface to indicate distance. Sonar measures the time taken for a pulse of ultrasound to travel to the obstacle and back in order to determine its distance away or range. Generally Lidar is used for very short range work up to a few centimetres, sonar for up to a few metres. I went for sonar as I had a very nice device to hand – an SRF08 rangefinder. You can see the SRF08 in the picture: it’s the red module with the two silver transducers. This configuration is used by many similar commercial units of this type which are mostly very simple and rely on the host microcontroller to do all the donkey work. In this case, what you can’t see in the picture is the PIC microcontroller on the back of the red PCB handling all the pulse transmitting, receiving and calculation of distance. The SRF08 is clever, receiving commands and sending back data to the dsPIC via an I2C serial bus. Hence the second addition to FORTHdsPIC is an I2C serial bus driver.
I said earlier that the wheels are driven by continuous-rotation servomotors. These are like the standard variety but the feedback pot linked to the output shaft has been replaced by a simple trimmer pot allowing the shaft to rotate through 360°. The PWM signal now determines the speed and direction of rotation rather than angular position. The pulse period is still 20ms with a pulse width of 1.3ms giving full ‘reverse’, 1.5ms is stop and 1.7ms full ‘forward’. The trimmer pot is accessible to allow calibration of the stop position while a 1.5ms signal is applied. As these PWM values are different from the standard motor I added a new word CSERVO to the FORTHdsPIC vocabulary.
I have mentioned the use of a control system to link the input from the light sensors to the motor signals. As this is a digital system, light samples are taken at regular intervals, processed into numbers which then update the servo speed settings. I thought about using the spare Timer 1 for the task of producing sampling interrupts but then realised that Timer 2 was already generating 20ms interrupts for the benefit of the servomotor PWM signals. This is really a no-brainer because there is no point in taking and processing the sensor samples at a faster rate than the motor speeds are updated. Hence the Timer 2 interrupt service routine (ISR) in addition to its original task of updating the PWM registers, now samples the analogue channels AN0 and AN1, and saves the results in two system variables ADC0 and ADC1. How does the user program know when an interrupt has occurred and new sensor samples are ready for processing? For this purpose the ISR sets Bit 15 of the ADC0 sample as a ‘new data’ flag. The main program waits in a loop reading ADC0 and exiting the loop when a negative number appears. This is possible because the ADC resolution is only 12 bits, the top four normally remaining zero. Hence Bit 15 can be used as a flag. No need to flag ADC1 as both variables are always set and read together. The flag bit is cleared prior to the value being used by the control program.
That’s it for the new peripheral device driver definitions for the ADC, I2C serial bus and a version of the PWM driver for continuous-rotation servos. Next comes some application programming in Forth with test programs to demonstrate the obstacle detection system, the light follower and finally a combination of the two yielding a photovore mobile robot that avoids running into things!
If you're stuck for something to do, follow my posts on Twitter. I link to interesting new electronics components and development kits (some of them available from my employer!) and retweet posts I spot about robot, space exploration and other issues.