Tag Archives: Servo

Driving a Raspberry Robot

DSCF3039

This article chronicles the path I took in creating a small mobile robot using a Raspberry Pi. In this post I will exhaustively describe what I did to get things working. The main purpose of the article is to support a lower-division undergraduate robotics course and project that I am part of as an instructor at the Department of Computer Engineering of Inholland University of Applied Sciences at Alkmaar, The Netherlands. It should provide for a nice learning experience if you’re up to the challenge. If you choose to attempt any of the experiments or instructions below, you are doing so at your own risk, your mileage may vary and success is not guaranteed, although I’ll try to clarify my work as conscientiously as I can.

These are my assumptions about you, the reader:

  • You are familiar with digital electronics and you know how to read datasheets, circuit schematics and timing diagrams
  • You have wrestled with microcontrollers before and you’re aware of the purpose of an Interrupt Service Routine (ISR)
  • You know how to program using the C language and are fluent in the binary, decimal and hexadecimal number systems
  • You are able to flash a PIC microcontroller
  • You have some aptitude for using the Linux shell
  • You have at least some experience in practical matters like soldering, sawing, using a dremel tool, drilling, etc.
  • You have access to the tools necessary for doing all of the above

Having said that, let’s take a peek at what I cooked up until now:

DSCF3041

Figure 1 – The glorious raspberry robot

1 Parts

The robot consists of the following parts and components:

  • The chassis, which is just a slice of MDF
  • Two continuous rotation servo motors with accompanying mounting brackets and wheels, harvested from a broken robot I had lying around (the ominous and easy-to-break ‘Jobot Nano 2′, see here)
  • Plastic ball caster, as pictured below (figure 2), for a static free rolling ‘nose wheel’, also reaped from the Jobot cadaver.
    BallCaster

    Figure 2 – A ball caster kit

  • Battery holder for 6 rechargeable batteries size AA, most of it from Jobot, but some of it from my inventory.
  • Six rechargeable NiMH batteries, size AA, 1.2V, > 2000 mAh, also taken from the Jobot corpse.
  • Model A Raspberry Pi (RPi or Pi for short) + Cyntech Case
  • Class 4 Samsung SD Card (8 Gb), preloaded with Raspbian by the guys from modmypi.com
  • Nano WiFi USB dongle for SSH-ing to the Pi from my laptop
  • Nano Bluetooth USB dongle for driving my robot using a Wiimote
  • Homebrew RPi – PIC SPI controller mounted on a eurocard perfboard that allows me to easily and accurately use hardware PWM (HPWM) for servo / motor control and helps me interpret sensor data. It also doubles as a regulated PSU for the RPi and motors. It uses SPI for communicating with the RPi. It consists of a number of components, including a PIC16F1827. See the parts list below for details

For reference, I included an exhaustive parts list, that I kept as generic as possible (i.e. I took some shortcuts with my specific implementation that might not be easily reproducible, so I listed replacement parts, but more about that later). The prices mentioned in the list are current at the time this article was written (May 2013), so they are just for reference. The ‘My Source’ column indicates where I found the part and the ‘NL Store’ column gives readers from my general area on the planet some idea of where to buy it.

Part URL My Source NL Store Amount Price € Total €
Servo Futaba S3003 (or any '360 modifiable' analog servo) datasheet Harvested from Jobot RS 2 10.00 20.00
Servo mounting brackets - Harvested from Jobot RS 4 0.20 0.80
MDF 4mm thickness (piece of 122×61 cm) - DIY/Hardware Store - 1 4.00 4.00
Plastic Ball Caster Kit (w/mounting materials) product Harvested from Jobot Floris.cc 1 3.00 3.00
Battery Holder for 6xAA wired product alternative Harvested from Jobot / Inventory EOO 1 5.00 5.00
Rechargeble batteries NiMH > 2000 mAh (Sanyo/Eneloop) product Harvested from Jobot NKON 6 3.00 18.00
Raspberry Pi (Model A 256MB RAM) product ModMyPi.com - 1 25.00 25.00
Cyntech Case product ModMyPi.com - 1 5.00 5.00
Nano WiFi adaptor product ModMyPi.com - 1 9.00 9.00
8GB SD Card product ModMyPi.com - 1 12.00 12.00
4 Port USB 2.0 Hub product ModMyPi.com - 1 14.00 14.00
5V 1500mA Power Supply product ModMyPi.com - 1 7.00 7.00
Microchip PIC 16F1827 datasheet Inventory Farnell 1 2.12 2.12
Linear voltage regulator 7805 TO-220 datasheet Inventory EOO 2 0.50 1.00
4 channel bidirectional level shifter product Inventory (homebrew) Floris.cc 1 4.50 4.50
Capacitor (ELCO) 47 or 100 uF 16V - Inventory EOO 2 0.15 0.30
Capacitor (Ceramic) 100 nF - Inventory EOO 1 0.10 0.10
PCB header 36 poles divisible - Inventory EOO 1 0.50 0.50
IC socket 18 pin - Inventory EOO 1 0.50 0.50
Eurocard 3 holes-per-isle perfboard product Inventory EOO 1 2.50 2.50
Switch (on/off) - Harvested from Jobot EOO 2 1.00 2.00
2×5 double pin header (may be PCB mount shrouded IDC-receptacle) + 10 wire flat cable - Inventory EOO 1 3.00 3.00
QRE1113 digital line sensor breakout OR separate reflective sensors w/hex inverter product alternative Inventory Floris.cc or EOO 3 2.54 7.62
Misc. wire / screws / nuts / bolts / mounts / brackets / spacers / tape / solderwire / expendables - Inventory - 1 10.00 10.00
Total - - - - - 156.94

I originally wrote the parts list in Excel, so for those of you who prefer it that way:

Parts list and budget for RPi Robot in Excel

As you can see, I looted a broken Jobot Nano, used some components from my inventory and ordered the rest (ca. € 75.00) from modmypi.com.

2 Chassis

First I cut a piece of MDF (thickness 4 mm) of about 16 * 9 cm. You can easily place and mount 2 servos sideways with the battery pack on top and just in front of that the Raspberry Pi in its case. This left about 2 cm in the front, which I wasted with the ball caster (front free-rolling wheel). I then placed my sensor array at the back just behind my servos (I had some room to maneuver because of the servo mounting brackets). This was a mistake, because having the sensor array behind the wheels introduced a host of problems when correcting a path based on sensor information.

The reason was that the line-feedback from the sensor array came too late to correct the vehicle’s path properly. I corrected my mistake by mounting an extra MDF bumper onto my bot, which gave me an extra 3 cm. So what I *should* have done, is create the original chassis base a little longer and place the sensor array in the very front to begin with. As far as I can tell, 18 or 19 cm would do the trick, but your mileage may vary depending on the particulars of the sensor array you’ll end up with (more about that later).

Finally I cut another piece of 6 * 9 cm as a base to stick my improvised battery holder onto. I think you’ll only need this if your battery holder consists of more than one unit, like mine did because I ripped mine from a Jobot, which has one holder for 4 batteries, and one holder for 1. So I added another holder for 1 battery totalling 3 holders for 6 batteries. I then wired the holders together and stuck them to said piece of MDF. I then taped this on top of the servos:

DSCF3042

Figure 3 – Servo and battery holder placement

3 Servo Internals

The average servo can only turn between 0 and 180 degrees, so to be able to use them for driving wheels, they need to be modified. If you only have unmodified servos lying around, there’s a good chance you can modify them yourself. There’s plenty of tutorials out there, detailing how to go about doing this. Such tutorials usually describe one of two ways:

  1. Rip out the entire feedback mechanism and just use the mini-DC motor inside the servo as you would any normal DC motor, as per these instructions. Keep in mind that if you do it this way, you will need a normal DC motor driver (like an L293D H-driver) or something else that eats PWM for speed control and has the ability to reverse motor polarity, so that it can turn in both directions. Luckily the selected PIC (16F1827) has hardware peripherals that are capable of generating HPWM signals of the necessary frequency. Make sure you know how to properly solder / desolder before attempting this.
  2. The other way of doing things is a little simpler (my servos were modified this way): just remove the servo’s main gear tab and disconnect the feedback mechanism from the servo’s rotation by hollowing out the main gear so that it does not turn the potentiometer when rotating. The potentiometer can then be used for calibration. Check out the youtube tutorial below to get some inspiration. If you find this to be too risky, you can also remove the potentiometer altogether and replace it with a pair of resistors that calibrate the servo statically. To do this, check here.

Here’s a video that details ‘option 2′ (I did not make this video, I just found it on YouTube):

There are many kinds of analog servo motors and the steps will vary subtly based on their type, however the basic steps are always the same:

  1. Remove any mechanical rotation limitations
  2. Fool the position sensing circuitry into thinking that the servo motor is in the middle position using a voltage divider (by either leaving the potentiometer in place, but disconnected from the gear, or by replacing the potentiometer with two resistors of equal value).

After these steps control of the servo motor becomes relative not absolute – that is, positional information that varies from the middle position is converted into velocity information.

The controller circuitry of an unmodified analog servo compares the desired position (set by the user by a form of Low Frequency [50 Hz] Pulse Width Modulation) with the current position (by reading voltage at the center pin of the potentiometer) of the motor to turn the motor in the direction that minimizes the error. So, telling the servo to go to 90° will place it in the middle position. With a continuous rotation motor, telling the motor to go to 90° equates to telling it to not move at all. Telling it to move to 91° equates to telling it to move clockwise by a small amount, and telling it to move to 89° equates to telling it to move counter-clockwise by small amount.

In general the formula is the difference between the specified angle of 90° equates to the velocity at which you wish your motor to turn. This is because in an unmodified servo motor, it ‘wants’ to get to its specified position as quickly as possible, so the bigger the difference, the faster it will get there. It also means that it ‘eases out’, to use some animation parlance, and this means smoother motion. You can have that in a continuous rotation motor too, but you’ll need to manage that in the way you control it.

The last couple of paragraphs are taken from this blog post. I did not include this post as a source, because some of the steps in the accompanying tutorial are redundant and might therefore confuse you. Aside from that, it was the most informative article I could find, so you might want to check it out anyway.

4 PWM to a Servo

Figure 4 below shows the relationship between the PWM input signal and the position feedback of a servo motor (image taken from here).

Pulse-Width-Modulation-Servo-Motor

Figure 4 – Relationship between PWM and positional feedback for an analog servo

Picture what happens when you remove the position feedback arrow and substitute a static value for the exact 90 degree position. The ‘electronic circuit’ will always want the motor to move as fast as is proportionate to this static 90 degree setting. It will do this based on the PWM signal that you see in the image. The way this PWM signal is interpreted by analog servos, is shown in figure 5 (taken from here):

TimelineServo

Figure 5 – Timing diagram for an analog servo modified for continuous rotation

As you can see the servo should hold still when a pulse width of 1500 microseconds (give or take 30 μs) is applied to the signal wire. The Futaba S3003 specs tell us that for our particular model, this pulse width should be 1520 μs. Maximum reverse rotation is attained at about a 1000 μs pulse width and maximum forward rotation at about 2000 μs. Apart from the pulse width, the frequency at which these pulses are applied is also important. Most analog servos need to have these pulses applied to them at about 50 Hz, so the timing diagram will look something like this (image again taken from here):

Servo-Angular-Rotation-Pulse-Width-Modulation

Figure 6 – Timing diagram revealing frequency and pulse width for analog servo control

Some more introductory information can be found here.

5 Circuit for controller and PSU

I drew up the circuit diagram (see figure 7 below) for my homebrew controller with the freeware version of DIPTrace. Notice the BS170 MOSfets I used for level shifting. I borrowed the MOSfet trick from this application note. Because the PIC16F1827 operates at 5V TTL logic level and the GPIO pins of the RPi operate at 3V3 CMOS logic level whilst not being very 5V tolerant, level shifting is mandatory for interfacing the two! Reference here. Focus on what this page declares in boldface:

GPIO voltage levels are 3.3 V and are not 5 V tolerant. There is no over-voltage protection on the board – the intention is that people interested in serious interfacing will use an external board with buffers, level conversion and analog I/O rather than soldering directly onto the main board.

As far as I can tell, all lines are unidirectional push-pull stage output signals, so I omitted the resistors at the output circuit sides (see paragraph 2.3.3 on page 11 of the app note). This effectively means that I did not use a 3K3 to pull up the lower voltage side (3V3). In practice, this assumption seems to work out, since SPI is working as it should, also when under scrutiny of my logic analyzer. But your mileage may vary and if you think this is risky, you should include the lower voltage side pull-ups as well (use four 3K3 resistors between your BS170’s ‘source’ lines and +3V3).

If you want to know more about level shifting in general and the different solutions people have come up with over time, check this little nugget of info.

If you decide to buy the entire 4-channel level shifting solution as a separate unit (see the parts list for details), you should be substituting both the MOSfets and resistors with the unit of your choice. Here’s the circuit then:

Figure 7 - Circuit diagram for the homebrew RPi SPI controller

Figure 7 – Circuit diagram for the homebrew RPi SPI controller

The pin headers are connected to the environment as follows:

Name in diagram pin 1|6 pin 2|7 pin 3|8 pin 4|9 pin 5|10
3V3 IN FROM PI RPi pin 1 (3V3) - - - -
SPI RASPI PIC SDI to RPi 19 (MOSI) PIC SDO to RPi 21 (MISO) PIC SCK to RPi 23 (SCLK) PIC !SS to RPi 24 (CE0) -
SENSORARRAY PIC RA2 to LEFT sensor +5V sensor PIC RA3 to CENTER sensor GND sensor PIC RA4 to RIGHT sensor
PROGRAM HEADER PIC !MCLR GND +5V PIC ICSPDAT PIC ICSPCLK
SERVOx GND +5V SIGNAL - -
NiMH 7V2 IN +7V2 GND - - -
5V OUT TO PI +5V GND - - -

Pinout for RPi rev. 2 below (got it from here):

gpio-pinout-rev2

Figure 8 – GPIO pinout for RPi rev. 2

I did not etch my own PCB for this project, I just manually placed the components on a 3 isle eurocard perfboard and soldered everything together, this is what it looks like (shabby, I know):

DSCF3045

Figure 9 – Homebrew RPi SPI controller on perfboard

5.1 PSU remarks

The power supply unit (PSU) is based on using two separate 5V linear regulators. One supplies the servos, the other supplies all other subsystems. The minimum input voltage for a 7805CT is 7.0V. Six rechargeable NiMH AA batteries provide 7.2V nominally, so this is actually cutting things closely. In practice though, I noticed after measuring that my Sanyo batteries supply just over 1.4V when fully loaded, so I start off at 8.4V. For me this is acceptable as far as educational / hobby projects go, but it would be better to use an LDO (Low Drop-Out) regulator, which accepts a lower input voltage, like for example an LM2940. Tip: when working with LDO’s, the output capacitor’s ESR (Equivalent Series Resistance) is critical in preventing the regulator from oscillating, so make sure you choose carefully using the datasheet as your guide.

Alternatively, one could opt for building an SMPS (Switched Mode Power Supply), because *in general* it is far more efficient, so you’ll get more fun out of your battery power. I did not do this, because a linear regulator or LDO works out well in this case (the difference between the regulator’s Vin and Vout is very small) and on top of that, building a proper SMPS on a perfboard is a challenge in and of itself. Most buck and/or boost converters require an inductor that needs to be chosen carefully. Many of them also need a voltage divider that creates the feedback voltage which needs to be spot on. Also, these IC’s are generally much more expensive than linear regulators. So I try to avoid this if and when I can, but ultimately, the choice is yours. The LM2575 (5V or ADJ) buck regulator would be a good candidate for this project. You could also take a look at this article.

6 Phototransistor array / line sensor

Most low-cost line sensors are created using an IR-LED / IR-Phototransistor combo. They are housed or placed in such a way that together they make a sort of open, albeit indirect, optocoupler, like so:

Figure 10 - Circuit diagram for a reflective sensor

Figure 10 – Circuit diagram for a reflective sensor

The general idea behind this setup is that when more IR light from the LED is reflected off a surface, more current is allowed to flow through the transistor. The transistor’s current amplification combined with the properties of the circuit mean that small swings in the transistors’ base voltage produce large(r) changes in Vout. This property could be used to feed Vout to an MCU’s ADC input, so the amount of light that is reflected can be discretely quantified.

As can be inferred from figure 7, my controller has a 2×5 header (I use a shrouded IDC receptacle with a flatcable) that connects to the sensor array. You will probably not need this, since I only used it to connect specifically to a ready-made sensor array that is no longer in production anywhere. I found it doing nothing on some old robot I had lying around at the workplace, so I just ‘borrowed’ it for my project. It’s a Lynxmotion Tracker V3.0. This is what the mounted version of my sensor array looks like:

DSCF3043

Figure 11 – Mounted Lynxmotion Tracker V.3.0

If you insist on using something similar to this unit, creating one of your own should be doable if you take a good look at the unit’s schematic. This unit uses a Schmitt-Trigger / Hex-Inverter to ‘digitize’ the analog values read by the phototransistors. I find this to be convenient, because in the PIC I only need to check for ‘reflective’ or ‘non-reflective’, without the hassle of doing ADC-measurements. The downside is that it can now only be used to differentiate between black and white, so your track should be designed accordingly…

For your own project I advise you to go with something more contemporary, like this breakout from Sparkfun. It uses a QRE1113 reflective sensor, for which the datasheet and even the breakout schematic are available at the Sparkfun page. Homebrewing a sensor array with 3 or more of these babies should be relatively painless. I included 3 of these in the parts list.

7 Code for 16F1827

My compiler of choice is MikroElektronika’s MikroC Pro for PIC, so some of what I’m about to divulge will be specific for this compiler. However, if you know how to program PIC microcontrollers by any other method, this should not be hard to adapt to your particular situation. I do realise that this compiler is not within budget for most hobbyists, but MikroElektronika provides for a free version of their compiler which is a “fully functional demo license with up to 2K of program words of output code size which can be just enough for simple applications”. This version should suffice for the needs of this particular project.

Alternatively, one could opt for the open source Small Device C Compiler (SDCC), which can incidentally be built from source code under Raspbian, for those who are interested.

7.1 Servo control

First order of business is to create the 50 Hz pulse needed by the servo motors. My initial idea was to use the HPWM peripherals on pins CCP1 (RB3) and CCP2 (RA7) of the PIC16F1827. However, this does not work out, because for any given clock speed (FOSC) of the PIC, there is a minimum associated PWM frequency for the CCPx peripherals. You can verify this by looking at paragraph ‘23.3.4 – PWM Period’ of the datasheet: apply the equation to your desired FOSC and the lowest prescaler value possible. It boils down to a minimum HPWM frequency of 245 Hz with a 4 MHz clock (FOSC). I run mine at 16 MHz, which has its minimum at 977 Hz, way too fast for sluggish 50 Hz ‘servo PWM’!

If you decide to use DC motors for your project, this minimum HPWM frequency business is of no consequence, so you can just use the PWM peripherals. But for my project, I had to think of something different! I’m a big fan of doing as many things in hardware as possible, so I decided to go for a solution which still uses the CCPx pins. Instead of HPWM I just use the comparator modules in tandem with the timer 1 module by having timer 1 generate an interrupt every 20 ms (50 Hz) and then setting the duty cycle for a servo by comparing the value of timer 1 with presets in the comparator modules. I’ll explain this in greater detail shortly.

7.1.1 Register handling

We’ll be using timer 1 and CCP modules 1 and 2, all three of which have a low byte and high byte register associated with them. Since 8 bit PICs deal in byte-size registers and MikroC does not come with 16 bit convenience registers, I create my own. A PIC’s data memory contains both general purpose RAM and special function registers in the same addressable space for variables in your program. This means that you can use the linker directive absolute to bind a variable of your program to a memory location that’s used by special function registers.

This is what my compiler manual says about linker directive absolute: ‘if variable is multi-byte, higher bytes are stored at consecutive locations’. This is a direct consequence of the way a PIC’s memory is accessed. The significant implication however is that if we want to pinpoint the start address of a multi-byte register in memory, we should use the low byte address. You’ll have to check the datasheet to see at which absolute address these registers are located in memory. Paragraphs 3.2.5 and 3.2.6 of the 16F1827’s datasheet deal with the device’s memory map, and you can find there that the low byte timer 1 register TMR1L is located at 0x0016:

memmapds1827tmr1

Figure 12 – Timer 1 register location in PIC16F1827 data memory

This eventually leads us to declare some globals and use 16 bit unsigned integer datatypes for them (in MikroC you can use unsigned int), like this:

unsigned int TMR1 absolute 0x0016;
unsigned int CCPR1 absolute 0x0291;
unsigned int CCPR2 absolute 0x0298;

7.1.2 PIC initialization

In order to use the peripherals at the pins designated in my circuit diagram (figure 7), you’ll have to remap the CCP2 module from its default place at pin RB6 to its alternative location at RA7. CCP2 pin designation can be adjusted with the APFCON0 register:

// Bit 3 high, means CCP2 (Right-side servo 2) on
// RA7 rather than on RB6
APFCON0 = 0x08;

I always set all unused pins to be inputs, with internal weak pull-ups enabled, so they read high. See the circuit diagram’s pinout to see whether pins should be set as input or output: RA7 is servo 2 output (CCP2). RA2, 3 and 4 should be inputs for the reflection sensor array. RB3 is servo 1 output (CCP1). RB2 (SDO1) is output to SPI MISO of the Raspberry Pi. This amounts to the following TRISx and WPUx settings:

TRISA = 0x7F;
TRISB = 0xF3;
WPUA  = 0x7F;
WPUB  = 0xC1;

Next up is deciding which interrupts to enable. I enabled the SPI interrupt (SSP1IE) and both comparator interrupts (CCP1IE and CCP2IE). Finally, I enable the appropriate interrupt groups in the INTCON register. The accompanying code:

PIE1 = 0x0C;
PIE2.CCP2IE = 1;
INTCON = 0x48;

Some readers who are still awake after all this will notice that INTCON.GIE is still 0. This is because I only enable interrupts globally just before my actual program loop starts and all initialization has been taken care of.

7.1.3 Servo timing

I run timer 1 at 1 Mhz because it’s easy to deal in units of exactly 1 μs. Since I run my PIC at 16 MHz FOSC, the instruction cycle clock runs at FOSC / 4, which is 4 MHz. So if I want to run my timer 1 module at 1 MHz, I need to use a 1:4 prescaler option. After that, I need to generate an interrupt every 20 ms, which is every 20000 μs. Since this 16 bit timer has its interrupt triggered at the moment of overflow between 0xFFFF (65535) and 0x0000, I set its starting point to 65535 – 20000 = 45535. Then, timer 1 interrupt still needs to be enabled in the PIE1 register.

Furthermore, the CCPxCON registers should be set to using Compare Mode. In this mode, I want CCPx to generate interrupts on compare matches to control the servo dutycycles, but without messing with the I/O state of the accompanying CCPx pins. Because the pulse width will be dictated by the master (Rpi) over SPI, I’ll start the controller off in servo neutral mode, so both comparators need to check for a match at exactly 1520 μs after the timer 1 starting point: set both comparator’s matching triggers to 45535 + 1520 = 47055. Final step is to start counting with timer 1 (set TMR1ON bit). If you do not understand what I’m trying to accomplish here, I urge you to check all these settings in the PIC’s datasheet. This paragraph translates to the following code:

T1CON = 0x24;
TMR1 = 45535;
PIE1.TMR1IE = 1;
CCP1CON = 0x0A;
CCP2CON = 0x0A;
CCPR1 = 47055;
CCPR2 = 47055;
T1CON.TMR1ON = 1;

Next up is the ISR (interrupt service routine). For now, it should:

  • Handle timer 1 overflow by setting the servo I/O pins (effectively creating a 50 Hz frequency pulse) and resetting the timer counter to 65535 – 20000 = 45535.
  • Handle individual CCPx matches by clearing the appropriate servo I/O pin (effectively creating the individual pulse width per servo set by the RPi)

The ISR code:

void interrupt()
{
    if (PIR1.TMR1IF)
    {
        // TMR1 overflow between 0xFFFF (65535) and 0x0000
        // 20 ms has passed, set servo 'PWM' signal(s) to HIGH again
        LATB3_bit = 1; // Servo 1 = CCP1 = left servo
        LATA7_bit = 1; // Servo 2 = CCP2 = right servo
        // Reset timer1 to '20.000' ticks before overflow...
        TMR1 = 45535;
        PIR1.TMR1IF = 0;
    }
    if (PIR1.CCP1IF) // Servo 1 = CCP1 = LATB3
    {
        LATB3_bit = 0;
        PIR1.CCP1IF = 0;
    }
    if (PIR2.CCP2IF) // Servo 2 = CCP2 = LATA7
    {
        LATA7_bit = 0;
        PIR2.CCP2IF = 0;
    }
}

Now, if you have the bot already built, you could run all of these initialization settings in your main() routine followed by enabling all interrupts and then an endless program loop. You would already be able to check if the servos are performing properly:

unsigned int TMR1 absolute 0x0016;
unsigned int CCPR1 absolute 0x0291;
unsigned int CCPR2 absolute 0x0298;

void interrupt()
{
    if (PIR1.TMR1IF)
    {
        // TMR1 overflow between 0xFFFF (65535) and 0x0000
        // 20 ms has passed, set servo 'PWM' signal(s) to HIGH again
        LATB3_bit = 1; // Servo 1 = CCP1 = left servo
        LATA7_bit = 1; // Servo 2 = CCP2 = right servo
        // Reset timer1 to '20.000' ticks before overflow...
        TMR1 = 45535;
        PIR1.TMR1IF = 0;
    }
    if (PIR1.CCP1IF) // Servo 1 = CCP1 = LATB3
    {
        LATB3_bit = 0;
        PIR1.CCP1IF = 0;
    }
    if (PIR2.CCP2IF) // Servo 2 = CCP2 = LATA7
    {
        LATA7_bit = 0;
        PIR2.CCP2IF = 0;
    }
}

void main()
{
    APFCON0 = 0x08;
    TRISA = 0x7F;
    TRISB = 0xF3;
    WPUA  = 0x7F;
    WPUB  = 0xC1;
    PIE1 = 0x0C;
    PIE2.CCP2IE = 1;
    INTCON = 0x48;
    T1CON = 0x24;
    TMR1 = 45535;
    PIE1.TMR1IE = 1;
    CCP1CON = 0x0A;
    CCP2CON = 0x0A;
    CCPR1 = 47055;
    CCPR2 = 47055;
    T1CON.TMR1ON = 1;
    INTCON.GIE = 1;
    while (1);
}

When I ran the above test program and listened in on the CCPx pins with my logic analyzer, I got a nice 50 Hz PWM pulse with an exact 1500 μs duty cycle (I recorded this when I did not yet know that Futaba S3003 servos have their neutral pulse width at 1520 μs, so I actually programmed with 1500 μs).

50HzFreq

Figure 13 – Measurement of the 50 Hz frequency generated by timer 1 (rising edge every 20 ms)

1500usPW

Figure 14 – Measurement of the 1500 μs pulse width as a result of a comparator match (falling edge 1500 μs after rising edge)

Please note that with this program, the servos should be actively held in the neutral position. This means the wheels should not be moving and the servo motors are powered, making them a lot harder to turn by hand (don’t force them, just feel it out). If by any chance one or both wheels are turning, then the respective servo is not calibrated properly. This is a good opportunity to do so, so make it happen: with the setup powered, turn the potentiometer inside the wheel shaft with a small flathead screwdriver so that the wheel stops turning altogether. This tuning process is very sensitive, so it’ll take a steady hand!

7.2 SPI Slave Configuration

Next up is proper SPI slave configuration of the PIC. As you might have noticed in the circuit diagram earlier, I connected all 4 SPI pins, including slave select (active low). When using one slave to one master, slave select can be considered optional, but the PIC’s datasheet warns us (paragraph 24.2.5):

If the Slave Select line is not used, there is a risk that the slave will eventually become out of sync with the master. If the slave misses a bit, it will always be one bit off in future transmissions. Use of the Slave Select line allows the slave and master to align themselves at the beginning of each transmission.

This is why I used all 4 SPI wires. As for the master (RPi) side of things, I’ll be using Gordon Henderson’s wiringPi. Although I’ll be discussing this at length later on, for now it’s important to notice that in wiringPi, SPI is setup in mode 0. Read up on SPI modes here if necessary. SPI mode 0 means the clock polarity is low (CPOL = 0) when the clock line is idle and data is sampled on the leading clock edge, so clock phase is also low (CPHA = 0). Microchip uses their own terminology for these settings, and to add fuel to the flame they have ‘their’ CPHA (which is called CKE) mean the exact opposite of CPHA. So to get mode 0 on a PIC, we need to set CKP to 0 and CKE to 1 (reference here).

MikroC has a built in library for setting up and using SPI, but it was written entirely for master mode. So when you want to use your PIC as an SPI slave, you’re on your own! The main idea is following the relevant instructions found in paragraph 24.2 of the datasheet, so I will not be explaining every line of code for my SPI slave setup. Verification is left as an excercise for the reader. Code:

// SPI defines                           // See datasheet p.283:
#define MST_OSC_DIV_16              0x01 // 0001
#define MST_OSC_DIV_64              0x02 // 0010
#define SLV_SS_EN                   0x04 // 0100
#define SLV_SS_DIS                  0x05 // 0101
#define SMP_SDO_MIDDLE              false
#define SMP_SDO_END                 true
#define CKP_CPOL_IDLE_LOW           false
#define CKP_CPOL_IDLE_HIGH          true
#define CKE_NOTCPHA_TRSMIT_FALLING  true
#define CKE_NOTCPHA_TRSMIT_RISING   false

void HWSpiInit(unsigned short mst_slv_oscdiv, bool smp, bool ckp,
               bool cke)
{
    unsigned short sspconfig = 0x00;
    if (ckp == true)
        sspconfig = 0x10;
    SSP1CON1 = sspconfig | mst_slv_oscdiv;

    // While operated in SPI slave mode the SMP bit of the SSPxSTAT
    // register must remain clear (datasheet p.240 and p.282).
    if (mst_slv_oscdiv == SLV_SS_EN || mst_slv_oscdiv == SLV_SS_DIS)
        SMP_bit = 0;
    else
        SMP_bit = (smp == false) ? 0 : 1;

    CKE_bit = (cke == false) ? 0 : 1;
    // Finally, enable the 'Synchronous Serial Port'...
    SSPEN_bit = 1;
}

void main()
{
    HWSpiInit(SLV_SS_EN, SMP_SDO_MIDDLE, CKP_CPOL_IDLE_LOW,
              CKE_NOTCPHA_TRSMIT_FALLING);

    // ... more init here

    while (1)
    {
        // ... main program loop
    }
}

You’ll need a communication scheme or some set of conventions for receiving servo commands and transmitting sensor data on request. Eventually, I settled for sending one byte at a time from the RPi, because whenever a byte comes in, I need some time to handle it (I did not yet want to build a FIFO for buffering incoming data, although I might do this later on), and wiringPi only leaves 1 μs between bytes when transmitting multiple bytes. This means there’d be only 9 μs between interrupts, which limits the amount of time to 36 instructions on the PIC side, which is cutting things closely when ultimately relying on some flag in the main program loop to be handled. I do use a global command array buffer of two bytes, because I only want to handle servo commands if they’ve come in for both servos. For my motor commands I concocted the following 8-bit scheme:

Bit 7 6 5 – 0
Meaning Servo (0 = right and 1 = left) Direction (0 = reverse and 1 = forward) Velocity (0 – 50)

Note the velocity limits: somewhere between 0 and 50 are valid values (meaning we’ll allow for 10 μs resolution when setting values between 1000 and 1500 or 1500 and 2000), but the 6 bit encoding allows for values between 0 and 63. This implies that bytes like 0xFF and 0x7F can occur, but do not contain valid servo commands. I will use this to distinguish between servo commands and sensor requests by looking at what byte came in. If it’s 0xFF, I take that as a request for sensor data, so I put the latest sensor readings into the SPI buffer. When 0x7F comes in, this means the latest sensor data just barrel shifted to the master side, so I ignore that one. When it’s any other byte, I consider it to be a servo command. Relevant code:

unsigned char glob_command[2];
unsigned char glob_tmp;
volatile bit glob_SPI_transmission_complete_flag;
bit glob_first_byte_flag;

void interrupt()
{
    // ... extend existing ISR code with the following SPI
    // interrupt handler:

    if (PIR1.SSP1IF) // when a byte has been received by the master:
    {
        glob_tmp = SSP1BUF;

        if (glob_tmp == 0xFF) // dummy byte sensor request!
        {
            SSP1BUF = glob_reflection;
        }
        else if (glob_tmp == 0x7F) // dummy byte sensor data SDO!
        {
            glob_first_byte_flag = 0;
        }
        else
        {
            if (!glob_first_byte_flag)
            {
                glob_command[0] = glob_tmp;
                glob_first_byte_flag = 1;
            }
            else
            {
                glob_command[1] = glob_tmp;
                glob_first_byte_flag = 0;
                glob_SPI_transmission_complete_flag = 1;
            }
        }
        PIR1.SSP1IF = 0;
    }
}

Take note that the glob_SPI_transmission_complete_flag is only set when both motor commands have been received. I also keep track of whether the byte received is the first of a series of two with glob_first_byte_flag. When it’s low, I write to command index 0, when it’s high, I write to command index 1. I also reset it when I’m certain I’ve just sent sensor data back to the RPi, since the sensordata request-response pair also happens to be a two-parter.

Now for the command logic, which makes up the meat and potatoes of the main program loop. I declare the sensordata as global ( glob_reflection), since the ISR needs to be able to always access the latest changes to it. In the program loop, I always update this value to the latest reading on PORTA. Then I handle servo commands if there’s a complete set available. First I copy the global command buffer to a local buffer. I do this because if during the command handling another SPI interrupt comes in, the value of one of the commands could be changed, which is undesirable. Per command (so for two iterations) I then extract the velocity, which I cap to 50. After that, I examine bit 6 for direction and set the servo pulse width accordingly. Finally, based on bit 7, I write the pulse width compare value to the appropriate comparator and close the handler until a new transmission is successfully received by clearing glob_SPI_transmission_complete_flag.

unsigned char glob_reflection;

void main()
{
    unsigned char local_command[2];
    unsigned short velocity = 0, i = 0;
    unsigned int servovalue;

    // global bits init:
    glob_SPI_transmission_complete_flag = 0;
    glob_first_byte_flag = 0;

    // ... PIC init and SPI init are placed here

    glob_reflection = 0;

    while (1)
    {
        glob_reflection = (PORTA >> 2) & 0x07;

        // Handle a completed SPI transmission:
        if (glob_SPI_transmission_complete_flag)
        {
            local_command[0] = glob_command[0];
            local_command[1] = glob_command[1];

            for (i = 0; i < 2; i++)
            {
                velocity = local_command[i] & 0x3F;
                if (velocity > 50)
                    velocity = 50;

                // direction = forward, between 1500 - 2000
                // direction = reverse, between 1000 - 1500
                // speed divided in 50 discrete 10 us steps
                if (local_command[i].B6)    
                    servovalue = 45535 + 1520 + (velocity * 10);
                else
                    servovalue = 45535 + 1520 - (velocity * 10);

                if (local_command[i].B7) // left servo, CCP1
                    CCPR1 = servovalue;
                else //if (!glob_command.B7) // right servo, CCP2
                    CCPR2 = servovalue;
            }

            glob_SPI_transmission_complete_flag = 0;
        }
    }
}

If and when the RPi side of things is ready to rumble, this program should now be able to handle servo commands and sensor data requests. The entire C program we’ve built so far can be downloaded here:

RasPiIOController.c

Please note this C code is MikroC-specific, so you might need to port parts of it to a flavor of C that’s common in your part of the world :) !

7.3 PIC programming

I programmed the PIC using my trusty Wisp648 from voti.nl. The accompanying software (XWisp) also supports the PIC16F1827. As for the programming header, I use this homebrew thing that has a pinout that matches the program header of the circuit in figure 7:

SAMSUNG

Figure 15 – The program header for my Wisp648

For the color coding scheme, looky-look here (page 2). I also own an ICD3 from MicroChip, but for non-production projects, it’s almost always faster to use the Wisp648 (otherwise, I’d have to install MPLab X, set project parameters, import Hex, have the ICD flash itself to working with mid-range MCU’s, etc., it’s a drag!). The driver that comes with my RS232 to USB converter (ProLific PL2303HXD based) does not like Windows 8, so I run Windows XP in a virtual machine using VMWarePlayer to flash my PICs. If all went as it should, you should have your controller ready by now!

8 Raspberry Pi configuration

I used the official and unadulterated Raspbian image found here (in fact, ‘2013-02-09-wheezy-raspbian.zip’ came preinstalled with my SD card). There’s no use for LXDE or any other X-server based desktop environment (and a host of other packages) on this project, so Raspbian might not be appropriate for an embedded OS in the eyes of many. I readily admit it would be better to use something like Arch or BSQuask, or whatever else you can find out there.

Another, and entirely different approach, would be to use hard real-time extensions, like Xenomai Linux, for which a pre-built RPi image is available by the way. The advantage of using a real-time kernel is that you can do soft-PWM straight from the GPIO-header without having to worry about the OS messing up your precious PWM timing by having the scheduler preemptively prioritize some other task or having a garbage collector hogging CPU cycles whilst trolling through your heap at the most inconvenient of moments. You’d need servos of which the signal logic is 3V3 compatible, though! Also, a working knowledge of real-time operating systems wouldn’t hurt :)

But to get things going quickly driver-wise and package-wise I tend to stick with something that I can get up and running in no time flat, so that’s why I decided to use Raspbian anyway.

8.1 WiFi

One of the application examples I’ll work out will be a line follower. Although I don’t mind nano or vim, I’m far more comfortable coding with Notepad++, so I’ll edit my code with N++ and then copy it over to the Pi via SFTP using FileZilla. To do this on a model A RPi, you need to get WiFi up and running, so here goes! I should mention that the drivers for the chipset my wifi adaptor uses are already present in Raspbian, so that’s why I will not include driver installation instructions. If you need to install drivers for your adaptor, now would be a good time to do so.

If you also have a model A RPi, you’ll probably need to copy your drivers to the SD card’s boot directory via another machine that has internet access and then install them manually from there. When you’re done installing the drivers, fire up your Pi and make sure to connect a properly powered USB hub, like the one on my parts list and connect a keyboard and your verified WiFi nano adapter to it. Obviously, you’d also have to hook your Pi up to a monitor. When logged in, edit the network interfaces file:

I made mine look like this, adding allow-hotplug for wlan0 and 2 iface profiles:

The default profile is for my router at home, the mobile profile is for a small router I carry around for when there’s no WiFi available. CTRL-O to save the file, CTRL-X to quit nano. Then edit the wpa_supplicant configuration file, as follows:

In it, make sure you add the profiles you promised in the network interfaces file:

The id_str parameter is used to distinguish between profiles. With the priority parameter I tell the RPi that whenever these wireless networks are both available, it should go for the mobile profile (i.e. my travelling router). Save the file and exit nano. Reboot your Pi by using the reboot command:

Alternatively, you could use the shutdown command, after which you turn your Pi off and then on again:

When you’re booted up again, your WiFi connection should come online. You can check your connection by using:

or:

If the connection does not come online by itself, edit the /etc/rc.local file:

and add the following line:

If you want your DHCP assigned IP address to be printed when you log in, also add the following to /etc/rc.local:

For changes in /etc/rc.local to take effect, you need to reboot. When afterward you need to scan the Pi’s wireless environment to find a WiFi network, use:

To conclude this section, I’d advise Windows users to download PuTTy. With it, you can SSH into your Pi from your workstation, so you can work on it remotely without the need for a(n extra) keyboard or monitor. You can then also remove the USB hub and plug the WiFi nano adaptor directly into the Pi. Now that you’ve got WiFi running, let’s add bluetooth capabilities. That is, if you own a bluetooth (nano) dongle…

8.2 Bluetooth

Since I got mine from modmypi.com, I followed their steps. Make sure you use a verified bluetooth dongle, to prevent problems. Since you just got a WiFi connection running you should do yourself a favor and let the package manager do its thing by letting it update and upgrade, just like the modmypi.com steps suggest.

Update the package list:

Upgrade your packages to the latest and greatest:

Clean up your package repository by removing unused and obsolete packages:

Updating and upgrading can take a while. For my dongle, the driver is already present in Raspbian, so all I have to do after upgrading is install the packages to create a bluetooth stack:

The blueman package is not mandatory for our purposes, because this is the bluetooth manager GUI you can use under the desktop environment, which I won’t be using. But I’ll leave that up to you. After this has finished, turn off your Pi, insert the bluetooth dongle and turn the Pi back on. Check whether your dongle has been recognized by listing the currently connected USB devices:

You should see it listed here. Alternatively, you could use:

This will also give you the bluetooth address. More importantly, check whether the bluetooth stack is running properly by issueing:

or alternatively:

If all is well, this should yield:

If bluetooth does not seem to be running, try this:

When you want to scan the environment of your dongle for other bluetooth devices, issue this command:

When you have a Wiimote lying around, press 1 + 2 just after issueing the previous command, to put the controller into discovery mode. The scanning process should produce something like this:

When a connection has been established between your dongle and your wiimote, you can check up on the connection by issueing:

This will output something like this:

With bluetooth and WiFi up and running, the last thing to do is to install the necessary libraries to be able to use all the hardware peripherals from the C programs we’ll be writing. Let’s start by opening up the low-level perpiherals on the Pi in the next section!

8.3 Low-level peripherals (GPIO & SPI)

I took a major shortcut here by using Gordon Henderson’s wiringPi libraries. This section describes how to install the libraries and enabling control of all necessary peripherals by program code. First off, install wiringPi (I installed it in /home/pi, the directory you’re in when you’ve just logged on). To be able to clone wiringPi from the Git repository it resides in, you need to install git:

Then clone the wiringPi repository:

Install the library by running the build script that comes with it:

These instructions are taken from the wiringPi install guide. It also contains a plan B for those who cannot or will not use git, so check it out if anything goes wrong!

Next on the list is un-blacklisting the BCM2835’s SPI kernel module, since we need to use SPI to communicate with our homebrew PIC PWM / Sensor controller. First open the blacklist configuration file:

Now place a hashtag (#) at the beginning of the line that says:

This line should now read:

Save, exit and then reboot the Pi for changes to take effect. After this is done, you’ll be able to control your Pi’s peripherals from program code. It’s almost time for an application example!

8.4 Raspberry Pi convenience config options

Here’s a few short tips that might make working with your system a little less annoying. For example, I always hate that nano has a default tabsize of 8 spaces, it makes my code look horrible. To change config settings in nano:

In this file, change the following:

As for another tip, you might want your RPi to automatically log in when it boots up. Do it like this:

Replace the following line:

with this line:

Then save, exit and reboot. The ‘pi’ user should now login automatically. Reference here.

Whenever you’re ready configuring and programming your Pi, you might want to make a sector-by-sector backup of your SD card, so you can easily revive it when the unimaginable happens (sudden and spontaneous data corruption). Do yourself a favor and use HDD Raw Copy Tool. It’s fast and compresses an 8 Gb Raspbian card to take up about 1 Gb of disk space in my experience.

There’s one more trick I’ll share: a safe shutdown button using a momentary switch. But since this needs its own hardware and program code, I’ll list it with the other application examples below. Next up is the home stretch, in which we’ll finally tie things up by creating some useful application examples.

9 Raspberry Pi code

Being a fan of the C language, I decided to keep things simple and stick to C. It enabled me to use wiringPi (and libcwiid, as you’ll see in a bit) natively. So here goes!

9.1 Line follower example

To create a line follower example, we’ll need to read the sensor values from the PIC and write servo commands to the PIC using the SPI bus. This means we’ll have to create a C program that adheres to the conventions I conjured up when writing the PIC code. Remember that we agreed to send both servo commands and sensor requests as two-byte units, but on a per-byte transmission basis. Also recollect that 0xFF and 0x7F are bytes with special meaning for the sensor request pair. Lastly, we need to remember the 8 bit encoding scheme for servo commands designed earlier. Keeping this in the back of your head, let’s get started. First we need to include the necessary libraries:

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>

Next up we’re going to need a function that translates user-friendly servo settings to encoded bytes that will be sent to the PIC over SPI. For that we need a number of defines that help with the encoding scheme plus a servo command buffer. Although in my scheme I use this buffer with one byte commands at a time, I wanted to create this in such a way it can be extended for other schemes. For this reason I put the encoded command in a one-byte command buffer array. Please note that the servo motors are mounted in such a way (mirrored) that if you would set them to the same duty cycle, they would turn in opposite directions relative to each other. That’s why I negate the Left Servo Reverse (LREVERSE) value to create the Right Servo Reverse (RREVERSE) value. Same goes for LFORWARD / RFORWARD. I then immediately shift the commands out over SPI as a pair, the way the PIC expects it.

#define NUMBYTES	1
#define BYTEINDEX	0
#define RSERVO		0x00
#define LSERVO		0x80
#define LREVERSE	0x00
#define LFORWARD	0x40
#define RREVERSE	LFORWARD
#define RFORWARD	LREVERSE
#define SPI_CH_0	0
#define SPI_CH_1	1

unsigned char cmdBuff[NUMBYTES];

void setMotors(signed char right, signed char left)
{
	unsigned char rdir, ldir, rspeed, lspeed;
	
	if (right < 0)
	{
		if (right < -50)
			right = -50;
		rdir = RREVERSE;
		rspeed = right * -1;
	}
	else // if (right >= 0)
	{
		if (right > 50)
			right = 50;
		rdir = RFORWARD;
		rspeed = right;
	}
	
	if (left < 0)
	{
		if (left < -50)
			left = -50;
		ldir = LREVERSE;
		lspeed = left * -1;
	}
	else // if (left >= 0)
	{
		if (left > 50)
			left = 50;
		ldir = LFORWARD;
		lspeed = left;
	}
	
	cmdBuff[BYTEINDEX] = RSERVO | rdir | rspeed;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
	
	cmdBuff[BYTEINDEX] = LSERVO | ldir | lspeed;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
}

Take a look at wiringPi’s SPI reference. Take note of the functions available for setting up and using the SPI module. Drogon created a full-duplex SPI function for us mortals: in my opinion this is the way things should be done, so we’re in luck! Many library builders create separate read() and write() functions for using SPI, and this usually impairs or hinders full-duplex operation, which is a shame! But moving on…

We also need a function to request sensor data from the SPI slave. We agreed that special byte 0xFF meant sending the request for queueing the latest sensor data to the PIC and special byte 0x7F was meant as a dummy byte to force the slave to cough it up. In between the request and the ‘data pull’ transmissions, I wait 1 millisecond to make sure the PIC has had enough time to retrieve the latest sensor values and queue them up in the SPI buffer register. I did it like this:

#define DUMMYBYTE1	0xFF
#define DUMMYBYTE2	0x7F

void msleep(int ms)
{
	usleep(ms * 1000); // convert to microseconds
}

unsigned char getSensorData()
{
	cmdBuff[BYTEINDEX] = DUMMYBYTE1;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
	msleep(1);
	cmdBuff[BYTEINDEX] = DUMMYBYTE2;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
	return (cmdBuff[BYTEINDEX] & 0x07); // whatever came back over SPI
}

Furthermore, I created a number of convenience functions that set the servo motors to execute meaningful moves, that I’ll later need to handle all possible sensor values. For ease of use I set the speed for all moves uniformly, to a value of 10. Note that we agreed to allow values between 0 and 50, so this might seem slow. However, when testing with this scheme, I noticed that the maximum speed is already reached around a value of 15.

Apparently, the servo controller does not set the motor speed as a linear function of the position error. Another clue for this is the extreme sensitivity of the servo calibration process (see paragraph 7.1.3). This means that servo motors are actually not a very optimal choice when it comes to continuous rotation motor control. This was to be expected when we modified the servos beyond recognition, so we’ll just have to live with it, or use H-bridge controlled DC motors, as mentioned earlier. What it comes down to for now, is that a speed setting of 10 is fine for my testing purposes, but you might need something else.

#define SPEED		10
#define OFF			0

void forward()
{
	setMotors(SPEED, SPEED);
}

void reverse()
{
	setMotors(-SPEED, -SPEED);
}

void stop()
{
	setMotors(OFF, OFF);
}

void turnLeftHard()
{
	setMotors(SPEED, -SPEED);
}

void turnRightHard()
{
	setMotors(-SPEED, SPEED);
}

void turnLeftSoft()
{
	setMotors(SPEED, OFF);
}

void turnRightSoft()
{
	setMotors(OFF, SPEED);
}

The actual line follower code is the simplest I could come up with, so there’s a lot of room for improvement and tuning. But it gets the job done (kinda), so it should be fine for testing. I setup SPI with a 1 MHz clock, because I know the PIC can easily handle that. Before entering the main program loop, I actively halt the servos for safety reasons. The main program loop has a switch that checks the state of the sensors with a certain resolution of change. I used 50 ms for my example, but this is arbitrary and should be optimized by testing. If anything has changed since the last time we checked, the movement of the servos can be adjusted. I think you get the point:

int main(void)
{
	int i;
	unsigned char currentreflection, previousreflection;
	
	if (wiringPiSetup() == -1)
	{
		printf("wiringPi setup failed somehow!\n");
		exit(1);
	}

	if (wiringPiSPISetup(0, 1000000) < 0)
	{
		printf("SPI setup failed somehow!\n");
		exit(1);
	}

	stop();
	currentreflection = 0;
	previousreflection = 99;
	
	// Main program loop
	while (1)
	{
		// Resolution of change:
		msleep(50);
		currentreflection = getSensorData();
		// If something's changed:
		if (currentreflection != previousreflection)
		{
			switch (currentreflection)
			{
				// WHITE = 1, BLACK = 0!
				//         ...432
				//         ...RCL
				case 0: // ...000 - all black, ignore
					break;
				case 1: // ...001
					turnRightSoft();
					break;
				case 2: // ...010 - should not occur
					forward();
					break;
				case 3: // ...011
					turnRightHard();
					break;
				case 4: // ...100
					turnLeftSoft();
					break;
				case 5: // ...101 - line in middle, straight forward
					forward();
					break;
				case 6: // ...110
					turnLeftHard();
					break;
				case 7: // ...111 - all white, line lost, keep going
					break;
				default:
					forward();
			}
			previousreflection = currentreflection;
		}
	}
	return 0;
}

The complete line follower program can be downloaded here:

linefollower.c

Copy the file over to your Pi using FileZilla (I just copied it to my /home/pi directory), open up an SSH session to your Pi and whilst in the same directory as the source file, compile like so:

Put the robot on a track of your choosing (hint: use white background and black track) and run the program:

I also created a ‘stop’ program, so that when my linefollower goes haywire, I can just CTRL-C to send a SIGINT to the program to terminate it and then issue

to bring the motors to a controlled halt. I can then reprogram the Pi without having to disconnect the motors on the board. This is convenient when testing. Download the stop.c program and compile using:

To top things off, here’s a little demonstration of my bot navigating the Lego Mindstorms Testpad, which proves to be difficult because of all the additional dark lines:

9.2 Wiimote RC example

To be able to control the robot remotely using a Wiimote, you’ll need a library called libcwiid. First install the necessary library and dependencies:

This library is not very well documented, but when you study their usage example together with the main header file for the library, you’ll be able to use it with some practice.

Let’s start off with the necessary includes. Aside from wiringPi, you’ll also need bluetooth and libcwiid:

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>
#include <bluetooth/bluetooth.h>
#include <cwiid.h>

You’ll need the same setMotors() routine used in the line follower example, so I’ll refer to the previous section for the associated code and explanation. I’ll be using the Wiimote’s d-pad while holding the controller sideways to drive the robot around. Expanding this code to use the Wiimote’s accelerometer to steer the robot is left as an exercise for the reader. Most convenience functions for movement stay the same as with the line follower, but I created a couple of extras to provide for dual button presses, which are common and usable on the Wiimote’s d-pad. Look at the comments for the how and why:

#define SPEED		10
#define SLOW		4
#define OFF			0

void forwardRight() // d-up + d-right
{
	setMotors(SLOW, SPEED);
}

void forwardLeft() // d-up + d-left
{
	setMotors(SPEED, SLOW);
}

void reverseRight() // d-down + d-right
{
	setMotors(-SLOW, -SPEED);
}

void reverseLeft() // d-down + d-left
{
	setMotors(-SPEED, -SLOW);
}

Next we need to initialize the bluetooth connection between the Pi and the Wiimote. As a result of studying the cwiid library and the usage example, it occurred to me that you’d need a wiimote handle, a bluetooth address, which should be left as tolerant as possible (any address can connect), a call to cwiid_open() and commands to turn on an LED on the Wiimote and to set reporting mode for the buttons, so we can listen to them later on. This is what I came up with:

cwiid_wiimote_t *wiimote;	// wiimote handle
struct cwiid_state state;	// wiimote state
bdaddr_t bdaddr;	// bluetooth device address

bdaddr = *BDADDR_ANY;
	
// Connect to the wiimote
printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
if (!(wiimote = cwiid_open(&bdaddr, 0)))
{
	fprintf(stderr, "Unable to connect to wiimote\n");
	return -1;
}
	
// Turn on LED1 if the connection was successfully established
cwiid_command(wiimote, CWIID_CMD_LED, CWIID_LED1_ON);

// Enabled reporting mode for the wiimote's buttons
cwiid_command(wiimote, CWIID_CMD_RPT_MODE, CWIID_RPT_BTN);

The main program loop should then periodically force the buttons to report their state and according to this state information, servo movement functions should be called. The library contains these convenient button flags:

#define CWIID_BTN_2		0x0001
#define CWIID_BTN_1		0x0002
#define CWIID_BTN_B		0x0004
#define CWIID_BTN_A		0x0008
#define CWIID_BTN_MINUS	0x0010
#define CWIID_BTN_HOME	0x0080
#define CWIID_BTN_LEFT	0x0100
#define CWIID_BTN_RIGHT	0x0200
#define CWIID_BTN_DOWN	0x0400
#define CWIID_BTN_UP	0x0800
#define CWIID_BTN_PLUS	0x1000

Please note that the button orientation for the d-pad in this scheme is based on the Wiimote being held upright, not sideways as I will be holding it. This means that if I read CWIID_BTN_UP, I actually need to move left, etc. You can play around with the resolution of change, I chose 250 ms rather arbitrarily, but you can probably get smoother operation if you optimize things. For dual button presses I just added the values associated with the buttons and then used the newer convenience functions for movement to do things like ‘go slightly to the right while also moving forward’. Code:

while (1)
{
	// Resolution of change:
	msleep(250);
	if (cwiid_get_state(wiimote, &state))
	{
		fprintf(stderr, "Error getting state\n");
	}

	switch (state.buttons)
	{
		case 0:
			stop();
			break;
		case (CWIID_BTN_RIGHT):
			forward();
			break;
		case (CWIID_BTN_LEFT):
			reverse();
			break;
		case (CWIID_BTN_UP):
			turnLeftSoft();
			break;
		case (CWIID_BTN_DOWN):
			turnRightSoft();
			break;
		case (CWIID_BTN_RIGHT + CWIID_BTN_DOWN):
			forwardRight();
			break;
		case (CWIID_BTN_RIGHT + CWIID_BTN_UP):
			forwardLeft();
			break;
		case (CWIID_BTN_LEFT + CWIID_BTN_DOWN):
			reverseRight();
			break;
		case (CWIID_BTN_LEFT + CWIID_BTN_UP):
			reverseLeft();
			break;
		case (CWIID_BTN_HOME):
			homebuttonstate = WAS_PRESSED;
			break;
		default:
			stop();
	}
}

The complete wiimote rc program can be downloaded here:

wiimotedrive.c

If you look at it closely, you’ll notice that I also listen for home button presses. When the user presses the Wiimote’s home button, I break out of the main program loop to properly close the wiimote connection and then shut the system down. I did this because the alternative was to just immediately shutdown whenever the shutdown button is pushed (see below). The bluetooth connection is then never properly closed, which might lead to reconnection problems. This is a way to shut down properly, while still taking the time to neatly close the bluetooth connection.

Copy the file over to your Pi using FileZilla (I just copied it to my /home/pi directory), open up an SSH session to your Pi and whilst in the same directory as the source file, compile as follows:

Now to run this program poses somewhat of a problem, because when you remove the WiFi dongle to exchange it for the bluetooth dongle, you won’t be able to SSH into the Pi to run the program. We’ll solve this in a bit, but for now you can just run the program with USB hub, keyboard, WiFi, bluetooth and monitor connected and see if the robot reacts to the Wiimote:

The program will tell you to press 1 + 2 to put the wiimote in discovery mode. When it has connected, you should be able to get it to move when pressing d-pad buttons! The final step would be to have it run stand-alone. The simplest way to do this is to start the program as a service when the Pi boots. To do this, open up /etc/rc.local:

Now add the following line just before the one at the very end that says exit 0:

Save and exit, then turn off your Pi. Disconnect the USB hub and monitor and connect only the bluetooth (nano) dongle directly to the Pi. When you fire it up, the LED of your bluetooth dongle will probably start blinking steadily when the program is auto-started and the message to press 1 + 2 would be printed to stdout. At that moment you press 1 + 2 on the Wiimote and with any luck, LED 1 on the Wiimote comes on and you’ll be able to drive the bot like I did mine.

Now I have to make a small remark about my bluetooth setup, which is not functioning perfectly just yet. Whenever I plug the dongle into the USB hub, it always works. Even when the USB hub is bus-powered by the Pi! But when I plug the dongle into the Pi directly, 4 out of 5 times, it remains unrecognized up to the moment of pairing with the Wiimote, so it does not work properly yet. It seems to work better if I plug it into the Pi a few seconds after firing it up, so I think it has something to do with the (root)hub the dongle is connected to. It does not seem to be a power issue. If anyone can shed any light on this, please share! With a little fidgetting I can get it to work most of the time however, so I can give you a little demo:

9.3 Shutdown button

To top things off, I implemented a contraption of convenience, that enables me to properly shut down the Pi with a momentary switch. The way to safely connect a momentary switch to a microcontroller is with a simple pull-up configuration, like this:

Figure 16 – A simple and safe pull-up configuration

Figure 16 – A simple and safe pull-up configuration

This configuration has a number of advantages:

  • Many controllers have the pull-up resistor built in for GPIO pins. This also applies to the Pi, so we won’t need the actual pull-up resistor to 3.3V. That’s one less component…
  • It’s a safe solution, because of the resistor in series with the switch: in case the input is accidentally set to output, it will limit the output current from the output pin, since we’re not directly shorting to ground.
  • It’s what they call active-low: by default it’s pulled up to VDD (+3V3 in our case), making it less sensitive to spikes that could lead us to misread data.

I had a momentary switch lying around with a molex connector put onto the leads, so for me it was easy to ‘plug and play’. Note that by connecting it directly to the Pi, I left out the safety resistor from figure 16, which could be unsafe. In any case, you may just as well solder a momentary switch to your controller board, put in the safety resistor and connect the leads to the same pins as I did on the Pi. I stuck mine onto pins 7 and 9. For a rev. 2 RPi this is fine, because the pinout from figure 8 tells us that pin 9 is ground. Rev. 1 boards have this pin designated as DNC (do not connect) on most pin diagrams I’ve come across, so in that case the safe decision would be to connect to pin 6, the only ‘official’ ground pin for rev. 1 boards.

Pin 7 of the header is also pin 7 in the wiringPi scheme. WiringPi allows you to set the internal weak pull-up and pull-down resistors of the GPIO pins, so if you configure the pull-up for pin 7, you’ll have yourself a safe, active-low momentary switch. We’ll program it so that it stays in a loop while the pin voltage reads high. When the pin voltage reads low long enough and consistently enough (ignoring switch debouncing and accidental presses), we’ll break out of the main program loop and shut down the entire system. This is my code, read the comments to get a sense of how it works:

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>

enum btnstates
{
	FAKEPRESS,
	VALIDPRESS
} buttonstate;

void msleep(int ms)
{
	usleep(ms * 1000); // convert to microseconds
}

int main(void)
{
	unsigned short counter;
	
	if (wiringPiSetup() == -1)
	{
		printf("wiringPi setup failed somehow!\n");
		exit(1);
	}
	
	printf("Shutdown button listener program started!\n");
	
	buttonstate = VALIDPRESS;
	
	// Setup pin 7 as input with internal pull-up enabled.
	// Thus it becomes active-low and it should physically
	// be connected to ground via a momentary switch.
	pinMode(7, INPUT);
	pullUpDnControl(7, PUD_UP);
	
	// Main program loop
	while (1)
	{
		// Stay in loop as long as pin 7 is held HIGH, and
		// if at some point it becomes LOW ...
		while (digitalRead(7) == HIGH);
		// ... then wait 25 ms for contact to fully debounce ...
		msleep(25);
		// ... then assume that the button press is legit,
		// or let the code below prove otherwise:
		buttonstate = VALIDPRESS;
		// During 1 second, sample 10 times to see if the
		// pin became high:
		for (counter = 0; counter < 10; counter++)
		{
			msleep(100);
			if (digitalRead(7) == HIGH)
			{
				// Button was let go during this one
				// second, so we're dealing with a
				// FAKEPRESS after all...
				buttonstate = FAKEPRESS;
				break;
			}
		}
		
		if (buttonstate == VALIDPRESS)
		{
			break;
		}
		
		// Apparently, we misread, because we're reading
		// high again after debouncing, so we're dealing
		// with a FAKEPRESS: give the user (1/2) a 
		// second and then start waiting for another
		// button press...
		msleep(500);
	}
	
	// If we ever break out of the main program
	// loop, shut the system down:
	printf ("Shutdown button pressed - shutting down now!\n");
	system ("sudo shutdown -h now");
	
	return 0;
}

You can download the code here:

shutdownbutton.c

As you can probably tell from the code, much effort goes into differentiating between switch bounces or accidental presses and valid switch presses. Note that instead of all these checks it might be a good idea to apply a small capacitor as switch debounce. The trade-off is between code and components. The choice is yours!

Compile:

And last but not least, open up /etc/rc.local to add the shutdownbutton program as a startup service:

Now add the following line just before the one at the very end that says exit 0:

Save, exit, reboot and you’ll be good to go!

10 Conclusion

Whatever portion(s) of this article you used, I hope you enjoyed using it. Feedback and comments are welcome! There’s still much work to be done on the software (like using concurrency for doing sensor reading and motor actuation in parallel), but that may be the subject of another article. As for the hardware side of things: in order to have a prototype of which I literally control every bit, these are things that still need some work:

  • Servo motors: I experience the full range of speed between settings -15 through 15 (pulse widths between 1370 and 1670 μs), although a range of -50 to 50 is available to me (pulse widths between 1020 and 2020 μs). I’d like to more accurately approximate the actual servo control function so as to have better control over the servo speed setting.
  • I am stumped by the bluetooth dongle problem I described: it always works when connected via a (bus-powered) USB hub, but it has difficulties being recognized when I plug it in directly.

If anyone has any enlightning comments, experiments or results, please share!