Hexapod - The Brain

My intention for this robot has always been to use the PIC chips as the computing power. Each leg was to be controlled by its own processor that knows just enough to operate a leg according to the instructions it receives from the brain. The basic idea being that when I came to implement the control algorithms I wouldn't need to worry about the mechanics of moving a leg 'safely' or correctly because the leg controller wouldn't let the brain do anything 'dangerous'. This would simplify the control algorithms but I was still concerned I wouldn't be able to express the complexity I wanted in assembler or C without a lot of time and learning.

Scott Hanselman recently published a podcast about the .net micro framework. I'm very familiar with C# (a .net language) and the development environment as I've spent the bulk of the last 6 or 7 years of my professional life working with it. Visual Studio is, almost without a doubt, the best development environment currently available. The coding support and tools, the debugging and ease of use all contribute to making it a tool that allows you to focus on the problem at hand rather than tool.

The micro framework brings a powerful language and development environment to a very small platform. There is growing support for the micro framework in the market place with a large variety of devices. The platform I'm most interested in for this project is the Netduino, a derivative of the already popular Arduino.

The most attractive feature of this piece of hardware is the plug'n'play aspect. Everything I've seen so far indicates you plug in the USB cable, power it up, start Visual Studio and you're away. As I'm more about the software than the hardware this is very attractive.

My local Arduino supplier, Mindkits, has many Arduino parts (which are compatible with the Netduino) but doesn't supply the Netduino yet (although they are considering it). I have ordered one from Sparkfun which should arrive sometime next week.

My first experiment will be to see if I can get the Netduino to talk I2C to the PIC. Once that works I'll connect another PIC and see if it can talk to both PICs. With that done I'll be well on the way to have a mobile platform.

Windows to PIC Interface - (Hexapod Part 7)

The full source code can be found here.

In my last post I mentioned that the PIC randomly stops responding to instructions from the PC, although it holds the servo in position so the interrupt was still running.

My serial port monitor showed me that I was still sending data so it probably wasn't a problem at the PC end. I'm using a Prolific USB to serial converter so that could have been failing but it seemed unlikely. The key indicator that it was my firmware was that if I hard reset the PIC everything started working again.

After a quick debugging session with MPLab I found that it was stalled waiting for more bytes to arrive in the RX buffer. The offending code looks like this

SetPositions:
    btfss PIR1, 5 ;has something been received?
    goto $-2
    movf RCREG, w
;check for servo number
    btfss PIR1, 5 ;has something been received?
    goto $-2
    movf RCREG, w
    movwf SERVO0POS
goto CommsLoop

The problem is that when a comms error occurs, in this case a buffer overrun, the receiver is disabled. This meant the processor endlessly waited for bit 5 of PIR1 to be set, which indicates when something is received. I knew this routine was too basic when I wrote it but I didn't think it was going to bite me this early on.

The solution, which is not a complete solution, is to check for comms errors and if there is one, clear the error and resume receiving. This does mean that some data is lost and this can be observed. When the scroll bar on the UI is moved continuously (causing an overrun, I assume while the processor is in the ISR) a byte may be dropped. This will cause the servo to jump to the position of whatever byte is received when reception is resumed.

I think the proper solution is to use a second, higher priority, ISR that adds bytes received into a file register buffer for later processing. This may cause some latency in the servo ISR but should be small enough to have no noticeable impact. I'll also consider running the processor at 20Mhz, instead of 4Mhz. This would mean that receiving a byte and moving it into a buffer will have 1/5th the impact it would currently have.

Anyway, the current 'solution' (stop gap?) is as follows

SetPositions:
 call ReceiveIntoW ;wait for servo number
 ;check for servo number
 call ReceiveIntoW ;wait for position
 movwf SERVO0POS
goto CommsLoop


ReceiveIntoW:
 btfsc PIR1, 5  ;has something been rx'd?
goto CompleteReceive            ;yes, go deal with it
 btfss RCSTA, OERR ;has there been an overrun error?
goto ReceiveIntoW               ;no, go check for rx'd data
 bcf  RCSTA, CREN ;clear the error by reseting the receiver
 bsf  RCSTA, CREN
goto ReceiveIntoW               ;go wait again

CompleteReceive:
 movf RCREG, w
 return

Windows to PIC Interface - (Hexapod Part 6)

The full source code can be found here.

Servo Control

I've now 'closed the loop' for communications with the PIC. Each servo has a pair of scroll bars. One showing the desired position, the other showing the actual. There are two reason why a servo may not be in the desired position.

The first is that the servo's calibration may not allow it to move to that position. If the user selects position 255 and the maximum position the servo can move to is 250 the 'Actual' position will show this. Thats the easy one. The more difficult scenario is that the servo is 'stuck' against something I.E. during a leg movement the leg has come up against an obstacle.

Each servos current will be monitored. If the current exceeds a preset limit (probably around 100ma) the servo will be considered stuck. The PIC will then back the servo off until the current becomes acceptable (probably around 50ma). Neither of these scenarios are implemented in the firmware yet but the UI has allowed for it.

The windows UI has a timer, currently set to 1 second. Each tick of the timer a single byte packet (0x02) is sent to the PIC. The PIC responds with a position packet, which consists of 4 bytes. The first byte is 'A' (0x41). This indicates the following 3 bytes will be the position of the three servos attached to this PIC. The UI receives these bytes and displays them on the scroll bar associated with the each servo. This is for the 'Actual' position.

When the user moves the 'Desired' position scroll bar the UI sends a packet to the PIC which consists of 3 bytes. The first byte (0x01) indicates this is a positioning packet. The second byte indicates which servo should be positioned and the third byte indicates the position it should be moved to.

Request: 02            - UI to PIC, Send Positions
Answer: 41 7F A0 A0    - PIC to UI, Positions are 0x7F, 0xA0 and 0xA0
Request:01 00 17       - UI to PIC, Set servo 0 to position 0x17
02                     - UI to PIC, Send Positions
Answer: 41 17 A0 A0    - PIC to UI, Positions are 0x17, 0xA0 and 0xA0
Request: 01 00 11      - UI to PIC, Set servo 0 to position 0x11
02                     - UI to PIC, Send Positions
Answer: 41 11 A0 A0    - PIC to UI, Positions are 0x11, 0xA0 and 0xA0

Under some circumstances (yet to be determined) the PIC stops responding. It is still holding the servo in position so it hasn't 'locked up' but it seems to have got out of whack somewhere. That'll be the next challenge.

After thats resolved I'll be looking at hooking the current measuring shunts up to the A/D converter and sensing overload situations.

NOTE: Big lesson learned while trying to get the RS232 interface going (specifically the PIC receiving data) don't use hyperterminal. Not sure what it was doing but after trying everything I could think of in the PIC firmware I tried using a different terminal tool and everything started working. So, if your considering using hyperterm, don't, just don't.

PIC to Windows Interface - (Hexapod Part 5)

The full source code can be found here.

One of the tasks I wanted to achieve fairly early in the piece was the communication between the PC and the PIC. This is fairly simple with any of the PICs that have a USART built in. In fact its trivial. Just configure the port and send a byte. Once its sent, send the next byte.

The following code initialises the USART to asynchronous mode, 9.6kbps

movlw B'10100100' ;initialize USART
movwf TXSTA ;8-bit, Async, High Speed
movlw .25
movwf SPBRG ;9.6kbaud @ 4MHz

movlw B'10010000' ;bit7 = SPEN, bit4 = RX Enable
movwf RCSTA

Sending the data is then simply load the byte into the TX register and wait for it to be sent

SendPositions:

;continuously send servo positions to PC

btfss SEND_STATUS, 0    ;wait until the interrupt changes something
goto $-2
BCF SEND_STATUS, 0      ;reset the 'something changed' flag

movlw "A" ;A means the next 3 bytes are servo positions
movwf TXREG ;next line
btfss TXSTA,TRMT ;wait for data TX
goto $-2

movf SERVO0POS, w ;move data into TXREG
movwf TXREG ;next line
btfss TXSTA,TRMT ;wait for data TX
goto $-2

movf SERVO1POS, w
movwf TXREG ;next line
btfss TXSTA,TRMT ;wait for data TX
goto $-2

movf SERVO2POS, w
movwf TXREG ;next line
btfss TXSTA,TRMT ;wait for data TX
goto $-2

goto SendPositions

The above code sends the ServoPosition packet to the PC. The servo position packet is identified by the first byte being an 'A'. This is just a character I chose at random, 'A' seems like a good place to start. When the software on the PC sees an 'A' it knows the next 3 bytes will be the positions of the 3 servo connected to the PIC.

The PC software, written in C#, opens the serial port and listens to all bytes coming in. These bytes are fed into a packet factory that looks for the 'A' character. When this character is seen it records the next 3 bytes and then creates a "ServoPosition" packet. This packet is then sent, via an event to anything that is listening, in this case a windows form.

The form outputs the data in two forms, a graphical form and a textual form. The software on the PIC current decrements the servos positions each time the interrupt runs so the servos are constantly moving through their full range. The next step is to set the PIC up to receive data from the PC and allow the user to move the scroll bars to position the servos.

Simplified State Machine - Hexapod (Part 4)

The full source code can be found here.

Now that I'm looking at control 3 servos in the one routine I want the routine to be as simple as possible so its easier for my brain and so the timing is more stable. In this version I have combined the servoMinimumDelay state and the servoPositionDelay state. I didn't so this intially because I wanted to be able to use one byte for the position. If I added the minimum delay to the position I would, in some cases, need two bytes. E.G. Maximum position = 0xEE and minimum position = 0x13, add those two numbers together and you get 0x100 or 256.

After thinking about it a bit more I realised the solution was quite simple. I load the timer0 high bit with 0xFF. Then add the minimum delay to the position. If the carry flag is set I decrement the timer0 high bit. The low bit is loaded with 0xFF - position.

servoPositionDelay
    movlw 0xFF
    movwf TMR0H

    ;add position to minimum to get total
    movf SERVO1POS,w
    addwf SERVO1MIN,w
    ;if the carry is set, decrement the high byte for timer 0
    btfsc STATUS, C
        decf TMR0H, f

    sublw 0xFF
    movwf TMR0L
    goto ENDISR

I did notice a bit of jitter that I'll have to track down but its not a concern at this point. The MPLab simulator is very useful for checking the timing delays so shouldn't be too difficult

Servo Position With State Machine (Hexapod Part3)

The full source code can be found here
The interesting part is the Interrupt Service Routine (ISR). In the previous example I wasn't using a prescalar but in this one I am. I'm using a 1:8 prescalar which means the timer is increased at an eighth the rate it used to be. this allows the full movement range of the servo to be expressed in one byte.

The ISR implements a 4 state state machine. The states are

• initialDelay - this state turns on the servo and pauses for 0.25ms. This is a absolute shorted pulse that will ever be needed
• servoMinimumDelay - this state pauses for the minimum position of the servo. The SERVO1MIN variable is set to 13 for the Futaba servo.  In my last post I discovered that the position I want to be the 0 position requires a pulse of 0.4ms. We have already delayed by 0.25ms in the previous state so we now need to delay by 0.15 ms to complete a 0.4ms delay. Delaying for 0x13 (19) timer ticks, with the prescalar set to 1:8 means we will delay for 19 * (0.01us * 8) = 0.152ms
• servoPositionDelay - the delay based on the required position of the servo. The position is subtracted from 0xFF because the timer triggers when it overflows (reaches 0xFF+1 or 256) . I could miss this line if I made position 0 the max position but having the subtraction means the minimum position is also the minimum delay. The only gotcha in this state is that if we are in position 0 we want to jump to the next state without waiting for the timer to expire
• operationalDelay - this state turns the pulse to the servo off and then sets the timer to delay for 18ms.

You'll also see the  the line decf    SERVO1POS, f which will make the servo move one position per ISR call so it takes a few seconds to move across its full range. The position then underflows which puts it back to it maximum position.

So far I haven't taken into account the time it takes to execute the ISR code. When the ISR is longer I'll probably have to modify the timers by a step or two to avoid jitter on the pulse but we'll see how it goes

...
...
ISR
    ;clear the interrupt
    bcf     INTCON, TMR0IF
    bcf     T0CON, TMR0ON    ;disable timer
  
    ;which state?
    movf    CONTROL_STATE, w
    addwf    PCL, f
    nop
    nop
    ;delay for 0.25ms as the minimum position of any servo
    goto     initialDelay  
    ;delay for the minimum for a selected servo
    goto     servoMinimumDelay
    ;delay for the position of the servo
    goto     servoPositionDelay
    ;delay for 18ms so we can do other stuff  
    goto    operationalDelay   

initialDelay
    movlw    0xFF
    movwf    TMR0H
    movlw    0xE1            ;256-(250us/8) = 256-31 - 225 = 0xE1
    movwf    TMR0L

    bsf        PORTB, RB1        ;turn on the servo control bit

    goto    ENDISR

servoMinimumDelay
    movlw    0xFF
    movwf    TMR0H            ;load high timer
  
    movf    SERVO1MIN , w    ;load min position
    sublw    0xFF            ;prepare low byte for timer

    movwf    TMR0L            ;load min delay into timer

    goto     ENDISR
  
servoPositionDelay
    movf    SERVO1POS,w
    bz        operationalDelay    ;if we're at pos=0 don't delay
    movf    SERVO1POS,w
    sublw    0xFF
    movwf    TMR0L
    goto     ENDISR  

operationalDelay            ;delay of 18ms that lets other things happen
    bcf        PORTB, RB1        ;turn off the servo control bit
    ;wait 18
    movlw     0xF7            ;F7 is the high byte of the timer offset
    movwf    TMR0H            ;write to the high byte first because the actual write occurs when the low byte is written
    movlw    0x2E            ;36S is the low byte of the timer offset
    movwf     TMR0L            ;The high and low bytes are now written

    movlw    0x00            ;reset state machine (0x00 because it will be incremented at the end
    movwf    CONTROL_STATE    ;
  
    decf    SERVO1POS, f    ;move the servo one step (for testing only)

ENDISR
    incf CONTROL_STATE, f   
    incf CONTROL_STATE, f   
    incf CONTROL_STATE, f   
    incf CONTROL_STATE, f    ;move to the next state
    bsf     T0CON, TMR0ON    ;enable timer, we're good to go
    RETFIE

PIC18, timers and servos (Hexapod part 2)

I've obtained a PICDEM 2 Plus demo board. It provides some standard hardware interfaced with a PIC18F542 and an in circuit programmer and debugger. The hardware includes an lcd screen, an I2C thermometer, a variable resistor wired to an analog-to-digital converter, a serial port and some buttons. There is an on board 4Mhz crystal for clocking the processor and a voltage regulator for a clean power supply.

This board saves me trying to understand the hardware and the software at the same time which is great. I'll write a basic version of the software I want and then build some hardware for it to run on.

The first task I set myself was to move a servo to a hard coded position and then maintain it. This didn't prove too difficult. I then found the limits of the servos movements using a multimeter to measure current. When the servo is at rest it consumes about 9ma. If the servo is not moving and it is consuming more than 9ma then it is up against the stop and I have sent it a pulse that is either too long or too short for its range. Once I found the limits I found the pulses required to move the servo through 180°.

A pulse of 1.5ms is the defacto standard for the center position. 1ms is 0° and 2ms is 90°. My servo, Futaba FP-S28, can move through about 190-200°. Using the method described above I found the absolute limits were 0.25ms to 2.4ms however the min and max for 180° movement were 0.4ms to 2.3ms.

The below code uses Timer0 to send a pulse of the desired length and then wait 18ms before sending the same pulse again. the servo signal wire is connected to RB1.

    list p=18f452
    INCLUDE "P18F452.inc"

;code protect disabled
    CONFIG     CP0=OFF
;Oscillator switch enabled, RC oscillator with OSC2 as I/O pin.
    CONFIG     OSCS=ON, OSC=HS
;Brown-OutReset enabled, BOR Voltage is 2.5v
    CONFIG     BOR=ON, BORV=25
;Watch Dog Timer disabled
    CONFIG     WDT=OFF


    ORG 0x00
        GOTO MAIN

       ORG 0x08 ;INTERRUPT VECTOR
           GOTO ISR ;INTERRUPT SERVICE ROUTINE
;================================================

ISR
    ;clear the interrupt
    bcf     INTCON, TMR0IF

    ;is the output already on?
    btfsc    PORTB, RB1
    ;no, it is not   
    goto    turnServoOff
    ;yes it is       
turnServoOn
    ;turn on the servo control bit                   
    bsf        PORTB, RB1           
    ;wait 0.4ms
    ;F7 is the high byte of the timer offset
    movlw     0xF7
    ;write to the high byte first because the actual write occurs when the low byte is written           
    movwf    TMR0H       
    ;00 is the low byte of the timer offset
    movlw    0x00         
    ;The high and low bytes are now written
    movwf     TMR0L          
    ;we're done, get back to what we were doing before the interrupt
    goto ENDISR

turnServoOff
    ;turn on the servo control bit
    bcf        PORTB, RB1           
    ;wait 18ms
    ;B9 is the high byte of the timer offset
    movlw     0xB9       
    ;write to the high byte first because the actual write occurs when the low byte is written
    movwf    TMR0H       
    ;B1 is the low byte of the timer offset
    movlw    0xB1       
    ;The high and low bytes are now written
    movwf     TMR0L           

ENDISR
    RETFIE
;================================================

MAIN
    ;init port b
    ;zero everything (trun it all off)
    clrf PORTB           
    ;clear bit 1 to make RB1 output
    bcf    TRISB , RB1       

    ;setup tmr0
    ;disable the timer until we have set it up
    bcf        T0CON, TMR0ON   
    ;turn the prescalar off
    bsf     T0CON, PSA       
    ;internal clock source (use the program counter clock rather than an external pin)
    bcf     T0CON, T0CS
    ;set to 16 bit mode
    bcf     T0CON, T08BIT   

    ;B9 is the high byte of the timer offset
    movlw     0xB9
    ;write to the high byte first because the actual write occurs when the low byte is written
    movwf    TMR0H           
    ;B1 is the low byte of the timer offset
    movlw    0xB1            
    ;The high and low bytes are now written
    movwf     TMR0L          

    ;enable tmr0 interrupt
    bsf     INTCON, TMR0IE   
    ;enable all interrupts
    bsf     INTCON, GIE   
    ;enable timer, we're good to go
    bsf     T0CON, TMR0ON   
    ;do nothing until interrupted
    goto $                   


;use TMR0 with no prescalar
;18ms is 18000 timer ticks so 65536-18000=47537 which is B9B1
;so B9B1 is the value to preload tmr0 with

;0.5ms is 50 timer ticks so 65536-500=65036 which is FE0C
;1ms is 1000 timer ticks so 65536-1000=64536 which is FC18
;2ms is 2000 timer ticks so 65536-2000=63536 which is F830
;2.5ms is 2500 timer ticks so 65536-2500=63036 which is F63C

    END

Changing the values in the turnServoOn routine will cause the servo to move to another position. The limit positions are laid out in the table at the end of the code.

Next step is to manage more than 1 servo, probably 3 since thats how many I have to play with.

Walking Robot Project (Hexapod Part 1)

Every since I started building robots out of technic lego and programming assembler into my C128 to make them run I've wanted to make an autonomous robot. There are many things it could do such as seek out sunlight to charge itself, move towards the loudest noise in the room (such as a clap), map a room using dead reckoning, solve a maze or just move in a straight line over a contrived obstacle course.

My current idea is to build a 6 legged insect using 18 servos, 3 servos and one low end micro per leg. This micro would be on an I2C bus with the master controller to receive instructions about where to move its leg and also to monitor the current draw on each servo so that it could report on stalls and whether the servo is in motion or stationary. This would take a bit of research as the servos will use power to maintain their position but I'm assuming this current would spike when in motion.
The PIC16F88 or (the PIC16F913-I/SP) looks like a good choice for the leg controller. It can be an I2C slave and has multiple IO pins (possibly enough for one processor to manage two legs). They look to be around $NZ9 a piece. Using a 5 way switch could provide some useful feedback.

The master controller would talk USB (or maybe RS232 for starters) to a PC  where I could write the 'personallity' in .net code. This code would just specify high level commands like 'left 15 degrees' or 'forward 5 steps'. It would be up to the master controller to pass specific instructions to each of the leg controllers and to report back to the PC any problems.

It would also be possible to use a servo as a directional control for a pair of ultrasonic sensors or light sensors or maybe even a simple CCD.

This is all way beyond my skill level at the moment but my approach would be something like this

1. Program the master controller to accept simple, byte code commands from a USB (or RS232) connection. A program to control a few LEDs via USB would be sufficient. There would also need to be some feedback loop so maybe a push button or two that can be queried via the USB.
2. Connect a leg controller to the master controller via the I2C bus and extend the above program to be able to report on the status of a pin on the leg controller and control an LED attached to the leg controller.
3. Connect another leg controller to the master controller and extend the program to be able to control both leg controllers
4. Extend the leg controllers to be able to control 3 (or 6? 2 legs) servos each.
5. Worry about the feedback later

We'll see how R&D funds go...