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