; ******************************************* 
; ** 64 Channel Serial Servo Controller    **
; **           For ATMega168               **
; **            Version 6.0                **
; **                                       **
; **     Copyright (c) September 2009      **
; **             Len Holgate               **
; **                                       **
; ** See http://www.lhexapod.com           **
; **                                       **
; ** Note that this controller assumes     **
; ** that we have CD74HCT238E or equivalent**
; ** demultiplexor chips connected to pins **
; ** 0-4 of Ports B and C and that the     **
; ** required address lines for these MUXs **
; ** are run from pins 2-4 of PortD.       **
; *******************************************
;

.nolist
.include "m168def.inc"
.list


.def resl = r0                  ; serial + PWM setup (used by mul)           
.def resh = r1                  ; serial + PWM setup (used by mul)

.def currentPos = r2            ; PWM setup ONLY
.def stepEvery = r3             ; PWM setup ONLY
.def targetPos = r4             ; PWM setup ONLY
.def stepCount = r5             ; PWM setup ONLY
.def stepSize = r6              ; PWM setup ONLY

.def bankMask = r7              ; init and PWM setup

.def changed = r8               ; PWM setup ONLY

.def sCommandLength = r9        ; serial ONLY

.def pulseStopTemp1 = r10       ; PWM switch off ONLY
.def pulseStopTemp2 = r11       ; PWM switch off ONLY

.def sExpectedBytes = r12       ; serial ONLY

.def sCurrentPos = r13          ; serial ONLY
.def sStepEvery = r14           ; serial ONLY
.def sTargetPos = r15           ; serial ONLY               
.def servoIndex = r16           ; serial ONLY
.def serialChar = r17           ; serial ONLY
.def movesComplete = r18        ; serial + PWM setup

.def index = r19                ; PWM setup ONLY
.def bankIndex = r20            ; init and PWM setup
.def muxAddress = r21           ; init and PWM setup


.def temp1 = r22                ; serial + PWM 
.def temp2 = r23                ; serial + PWM
.def temp3 = r24                ; PWM ONLY
.def temp4 = r25                ; PWM ONLY


.equ POSITION_DATA_START  = SRAM_START
.equ NUM_SERVOS           = 64              ; Max 64!

; Each servo has 5 bytes of configuration/working data. These are as follows:

; 0 - current position
; 1 - step every
; 2 - target position
; 3 - step size
; 4 - step counter

; TODO...
; min
; max
; centre adjust
; reset value


.equ CURRENT_POS_OFFSET   = 0
.equ STEP_EVERY_OFFSET    = 1
.equ TARGET_POS_OFFSET    = 2
.equ STEP_SIZE_OFFSET     = 3
.equ STEP_COUNT_OFFSET    = 4

.equ BYTES_PER_SERVO      = 5           
.equ POSITION_DATA_END    = POSITION_DATA_START + (NUM_SERVOS * BYTES_PER_SERVO)

.equ PWM_DATA_START       = POSITION_DATA_END
.equ PWM_SERVOS_PER_CYCLE = 8
.equ PWM_BYTES_PER_SERVO  = 3
.equ PWM_DATA_SIZE        = (PWM_SERVOS_PER_CYCLE + 1) * PWM_BYTES_PER_SERVO
.equ PWM_DATA_END         = PWM_DATA_START + PWM_DATA_SIZE

.equ SERIAL_DATA_START    = PWM_DATA_END
.equ SERIAL_DATA_LENGTH   = 20
.equ SERIAL_DATA_END      = SERIAL_DATA_START + SERIAL_DATA_LENGTH

.equ STACK_START          = RAMEND
;.equ STACK_SIZE           = 19
;.equ STACK_END            = STACK_START - STACK_SIZE + 1        ; inclusive


.equ JUMP_TABLE_START    = INT_VECTORS_SIZE


.equ clock        = 7372800
.equ baudrate     = 9600
.equ baudconstant = (clock/(16*baudrate))-1


.ORG $0000
    rjmp Init       ; Reset
.ORG OC1Aaddr
    rjmp TC1CmpA    ; Timer/Counter1 Compare Match A


.ORG JUMP_TABLE_START
rjmp PWMSetup 
rjmp PWMPulseStop

; Timer 1 interrupt for CTC timer 

TC1CmpA:

; Each timer interrup handler deals with its own stack maintenance...


    ijmp                ; Z is set up to point into our jump table (above) so we jump to Z then
                        ; jump to the correct state for our timer handling...


; Program start

Init:

    ; Set stack pointer - we use the stack for the return address of the interrupt handler...

    ldi temp1, LOW(STACK_START)
    out SPL, temp1
    ldi temp1, HIGH(STACK_START)
    out SPH, temp1

    ; Set up the serial port

    ldi temp1, HIGH(baudconstant)          ; Set the baud rate
    sts UBRR0H, temp1
    ldi temp1, LOW(baudconstant)
    sts UBRR0L, temp1

    ldi temp1, (1 << RXEN0) | (1 << TXEN0)    ; enable rx and tx
    sts UCSR0B, temp1

    ldi temp1, (3 << UCSZ00)                  ; 8N1 

    ; Initialise Timer1 in CTC mode

    clr temp1                       ; Controlword A    0x0000
    sts TCCR1A, temp1

    ldi temp1, (1<<WGM12) | (1<<CS11) ; CTC mode (WGM12) with the timer running at clock/8
    sts TCCR1B, temp1

    ldi temp1, 1 << OCIE1A          ; Enable Timer1 interrupt
    sts TIMSK1, temp1

    ; Initialise our PWM output pins    

    ldi temp1, $0F                  ; set pins 0-4 on port B as output 
    out DDRB, temp1

    ldi temp1, $0F                  ; set pins 0-4 on port C as output 
    out DDRC, temp1

    ; Initialise our MUX address select output pins         

    ldi temp1, $1C                  ; Init MUX address line outputs; we use pins 2-4 on port D
    out DDRD, temp1

    clr muxAddress                  ; address channel 0 on the mux's
    out PORTD, muxAddress

    clr bankIndex                   ; start from bank 0

    ldi temp1, 1                    ; set the bank mask to bit 1
    mov bankMask, temp1 

    ; We have two different states for our timer interrupt handling, a setup phase and a 
    ; 'pulse switch off' phase. We use an indirect jump via the value of the Z pointer to deal 
    ; with this. Initially we set Z to point to the start of our jump table, which jumps to 
    ; the PWM setup routine, at the end of the setup routine we change Z to point to the next 
    ; entry in our jump table which is the PWM pulse switch off routine. Once all pulses have 
    ; been switched off we reset Z to the PWM setup routine and the next interrupt starts the 
    ; PWM generation for the next bank of servo pins.

    ldi ZL, LOW(JUMP_TABLE_START)   ; set up our first jump table entry for timer handling
    ldi ZH, HIGH(JUMP_TABLE_START)

; Not needed in ATMega168 simulator as that resets contents of SRAM to 0 on start...
;    ; Clear down the SRAM area so that it is slightly easier to debug in the simulator
;   ; Note that this code can be removed without causing any harm...
;
;    ldi XL, LOW(SRAM_START)         ; clear down the area we'll be working in
;    ldi XH, HIGH(SRAM_START)        ; to make it easier to debug
;
;    clr temp1                      ; usually it's easier if all of SRAM is 0x00 in the simulator
;    ;ldi temp1, 0xFF               ; but sometimes 0xFF helps too...
;
;InitFillMemoryLoop:
;
;    st X+, temp1
;    
;    cpi XH, HIGH(RAMEND + 1)
;    brne InitFillMemoryLoop
;
;    cpi XL, LOW(RAMEND + 1)     
;    brne InitFillMemoryLoop   

    
    ; End of SRAM memory fill loop

;    ldi XL, LOW(STACK_END)         ; Fill the stack space we're allowed to use with 0xFF to aid
;    ldi XH, HIGH(STACK_END)        ; debugging in the simulator...
;
;    ldi temp1, 0xFF                
;
;InitFillStackLoop:
;
;    st X+, temp1
;
;    cpi XH, HIGH(STACK_START + 1)
;    brne InitFillStackLoop
;
;    cpi XL, LOW(STACK_START + 1)     
;    brne InitFillStackLoop   

    ; Now set all of our servos to their initial start positions...

    ldi XL, LOW(POSITION_DATA_START)
    ldi XH, HIGH(POSITION_DATA_START)

    ldi temp1, 0x7F     ; all servos start in the centre ... 

    clr temp2

InitLoadInitialPositionsLoop:


    st X+, temp1        ; current position
    st X+, temp2        ; step every
    st X+, temp2        ; target position
    st X+, temp2        ; step size
    st X+, temp2        ; step counter

    cpi XH, HIGH(POSITION_DATA_END)
    brne InitLoadInitialPositionsLoop


    cpi XL, LOW(POSITION_DATA_END)     
    brne InitLoadInitialPositionsLoop

    ; Kick off the PWM code...

    clr temp1                  ; Reset Timer 1...
    sts TCNT1H, temp1          
    sts TCNT1L, temp1          


    ldi temp1, HIGH(10)        ; CTC-value - something low to kick us off... 
    sts OCR1AH, temp1          
    ldi temp1, LOW(10)         ; Note that the 16-bit timer control registers MUST be written
    sts OCR1AL, temp1          ; high byte first then low byte as the low write triggers the
                               ; 16-bit atomic write.

    ; Initialisation is done...

    sei                        ; enable interrupts    

    rjmp    SerialStart


; Timer interrupts


; PWM Setup - timer interrupt handler for phase 1 of the PWM generation cycle


PWMSetup : 

; timer interrupt needs 5 (data) + 2 (return address) bytes of stack for itself.

    nop
    push temp1         ; save the state of these as they are used by the serial protocol code
    push temp2
    push resl
    push resh

    in temp1, SREG     ; save status register
    push temp1

    ; Set the timer to 400ms which is much longer than the PWM setup phase takes to run. We 
    ; will adjust this to the correct value when we complete the setup phase.

    ldi temp1,HIGH(400) 
    sts OCR1AH,temp1
    ldi temp1, LOW(400)        ; Note that the 16-bit timer control registers MUST be written
    sts OCR1AL, temp1          ; high byte first then low byte as the low write triggers the
                               ; 16-bit atomic write.


    ; set the port d bits to control the mux address selection...

    out PORTD, muxAddress     ; mux address is 4 * bank index


    ; Start the PWM signal generation for this bank. All pwm pins on port b and c are on...

    ldi temp1, $0F     
    out PORTB, temp1
    out PORTC, temp1

    ; We generate at most 8 PWM signals at a time (PWM_SERVOS_PER_CYCLE) and repeat these 
    ; signals every 20ms, since we can support at most 64 servos in total this means we 
    ; need to generate 8 batches of 8 signals and each batch can take at most 2.5ms to 
    ; generate the signals for all of the servos in that batch. The signals should be between 
    ; 600us and 2.4ms long. 

    ; The data we use to generate the PWM signals is based on the data that the serial I/O
    ; code stores at POSITION_DATA_START. The serial code stores 5 bytes per servo. These bytes
    ; are as follows:
    ; 0 - current position
    ; 1 - step every
    ; 2 - target position
    ; 3 - step size
    ; 4 - step counter
    ; The current and target position values are control values of between 0 and 254 with 127 
    ; being in the centre with a pulse length of 1.5ms
    ; If 'step every' is not zero and the current position is not equal to the target position
    ; then the PWM setup code will decrement the step counter by one. When the step counter 
    ; reaches zero we step the current position towards the target position by the step size.
    ; If the current position goes past the target position during this move then the current
    ; position is set to the target position. When a step means that the current position
    ; becomes the target position we set a bit in the movesComplete register that represents
    ; the bank in which the servo that has completed is present. The serial I/O code can check,
    ; but not change, this register and send a 'incremental move complete' message back to 
    ; the controlling computer.
    ; Note that PWM code has read access to control bytes 1, 2 and 3 and read/write access
    ; to control bytes 0 and 4. The serial code has the opposite access. 

    ; The PWM generation code uses two byte servo control values that represent the actual
    ; timeout values required to turn off the servo pin, we can have up to 8 off times, as
    ; we merge duplicate off times together. The final off time (which could be number 9 if
    ; we have 8 unique servo control values) is the time remaining between the last servo 
    ; pin being turned off and the start of the next PWM generation cycle. I.e. it is the 
    ; time remaining until the 2.5ms time period has passed.

    ; First we copy our position data from the source buffers that the serial I/O code
    ; can alter into the working buffers that we'll use during PWM generation.
    ; The soruce data is arranged in 8 banks of 8 bytes, we use the eex to index
    ; into the start of the bank that we're working on.

    ; To make it easier to create a board for versions of this controller that don't provide
    ; all 64 channels we reorder the servo data during the copy so that the servo channels 
    ; appear sequentially on the pins of the MUX chips. That is servo 0 is pin 0 on mux 1, 
    ; servo 1 is pin 1 on mux 1, servo 9 is pin 0 on mux 2, etc.

    ; To achieve this we need to step the source pointer of our copy by 8 servo control data 
    ; structures each time, starting at the bankIndex offset address. So for the first cycle 
    ; we copy starting from our position data start address plus an offset of 0, for the next 
    ; we start at an offset of 1 * the number of bytes of control data per servo (currently 5), 
    ; for the next at an offset of 2 * the number of bytes of control data per servo, etc.
    

    ldi ZL, LOW(POSITION_DATA_START)   ; Load memory location to copy from
    ldi ZH, HIGH(POSITION_DATA_START)

    ; Multiply the bank index by the number of bytes per servo to get the offset from the 
    ; start of the data

    ldi temp1, BYTES_PER_SERVO

    mul bankIndex, temp1

    add ZL, r0
    adc ZH, r1

    ; Clear the 'moves are complete' bit for this bank

    mov temp1, bankMask
    com temp1

    and movesComplete, temp1

PWMSetupSetDestination:

    ldi YL, LOW(PWM_DATA_START)
    ldi YH, HIGH(PWM_DATA_START)

    ; The position data consists of a 1 byte value for each servo. The PWM code needs to 
    ; also have the bit that corresponds to the pin that controls this servo. As we copy
    ; from source to destination we expand the data per servo from a 1 byte position value 
    ; to a two byte position and bit combination.

    ldi index,1                         ; the first pin is at position 1


PWMSetupCopy:

    ; We can be configured to support fewer than 64 servos, in that case the servos that we 
    ; don't support are hardcoded to an 0x7F value in case the hardware is such that someone
    ; can connect a servo to the pin! We still generate the signals correctly for all 64 
    ; servos, you just cant configure more than the maximum specified in NUM_SERVOS.


    cpi ZH, HIGH(POSITION_DATA_END - 1)
    brne PWMSetupCopyFromSource

    cpi ZL, LOW(POSITION_DATA_END - 1)          ; then check low byte....
    brne PWMSetupCopyFromSource


PWMSetupUnsupportedServo:

    ; Z is out of range. We are attempting to access a servo index that we don't support.
    ; Hard code the value to 0x7F...

    ldi temp1, 0x7F
    mov currentPos, temp1                    
    
    adiw ZL:ZH, (PWM_SERVOS_PER_CYCLE * BYTES_PER_SERVO); increment the source pointer to the next set of servo data

    rjmp PWMSetupCopyToDest

PWMSetupCopyFromSource:

    ; Servo index is valid for control.

    ld currentPos,Z+                        ; load current position from the position data

    ld stepEvery, Z+                        ; load 'step every' from the position data

    ld targetPos, Z+                        ; load the target position

    ld stepSize, Z+                         ; load step size

    ld stepCount, Z+                        ; load the step count position

    sbiw ZL:ZH, BYTES_PER_SERVO             ; move Z back to the start of this structure

    tst stepEvery                           ; if we're not stepping...
    breq PWMSetupIncrementSource

    ; We only ever refer to the target position and the step size when step every is non zero.

    cp currentPos, targetPos                ; if current position != target position
    brne PWMSetupStep1                          

    ; Although we signal the serial code when a move actually completes we also signal it here to say that this
    ; move has completed and the serial code hasn't processed it yet. 

    or movesComplete, bankMask              ; add this bank of servos to the servos with moves completed register

    rjmp PWMSetupIncrementSource

PWMSetupStep1:

    ; current position is not equal to target position and step every is not equal to zero...

    tst stepCount                           ; if count is not zero
    brne PWMSetupStep2

    mov stepCount, stepEvery

PWMSetupStep2:    

    dec stepCount                           ; reduce our counter...
 
                                            ; save the counted back to the control data...
                                              
    adiw ZL:ZH, STEP_COUNT_OFFSET           ; increment Z to point to the step count

    st Z, stepCount                         ; save the value

    sbiw ZL:ZH, STEP_COUNT_OFFSET           ; move Z back to the start of this structure

   
    tst stepCount                           ; if count is not at zero...
    brne PWMSetupIncrementSource

    cp currentPos, targetPos                ; if we're incrementing towards the target position
    brlo PWMSetupIncrementCurrentPos

                                            ; else we're decrementing towards the target position...

    cp currentPos, stepSize                 ; check for underflow in the currentPos - stepSize calc
    brlo PWMSetupSetCurrentToTarget

    sub currentPos, stepSize                ; decrement towards the target...

    cp targetPos, currentPos                ; if the target position is still less than the current position
    brlo PWMSetupUpdateCurrentPos

    rjmp PWMSetupSetCurrentToTarget         ; else set the current position to the target position

PWMSetupIncrementCurrentPos:

    ldi temp1, 0xFF                         ; check for overflow in the currentPos + stepSize calc
    sub temp1, stepSize
    
    cp temp1, currentPos
    brlo PWMSetupSetCurrentToTarget

    add currentPos, stepSize                ; increment towards target position

    cp currentPos, targetPos                ; if the target position is still larger than the current position
    brlo PWMSetupUpdateCurrentPos

                                            ; else set the current position to the target position

PWMSetupSetCurrentToTarget:

    mov currentPos,  targetPos              ; set to the target position

    or movesComplete, bankMask              ; add this bank of servos to the servos with moves completed register

PWMSetupUpdateCurrentPos:
    
    st Z, currentPos                        ; update the current position

PWMSetupIncrementSource:

    adiw ZL:ZH, (PWM_SERVOS_PER_CYCLE * BYTES_PER_SERVO); increment the source pointer to the next set of servo data
    
    rjmp PWMSetupCopyToDest

PWMSetupCopyToDest:

    ; Copy the current position value (which has possibly been moved closer to the target position value)
    ; to the PWM working data.

    st Y+,currentPos        ; store to the pwm data

    ldi temp1, 0xFF
    eor temp1, index        ; servos are ON when the pin is set, we need to know the unset, OFF 
                            ; value for this pin
    st Y+, temp1            ; store the pwm OFF pin value

    lsl index               ; next pin
    tst index
    brne PWMSetupCopy


    ; Now that we have the servo data copied into our working space and grouped with the 
    ; related pin information we need to sort the data into order from low to high. This
    ; will allow us to set a timer for the next servo control value change, adjust the pins
    ; when the timer fires and then step to the next later servo control value.

    ; We use a simple bubble sort to compare the values of this servo control byte with the
    ; next servo control byte and if the next value is less than this value we swap both the
    ; values and the pins.

    ; During the sort we merge the pin values of servo control values that are equal as we
    ; only need to set the timer once to turn off multiple servos. This leaves us with a 
    ; servo control value that we no longer need, we set this to 0xFF so that it automatically
    ; bubbles up to the end of the list.

    ; set up before sort...

 .undef temp1
 .undef temp2
 .undef temp3
 .def unusedServoValue = r22
 .def thisValue = r23
 .def nextValue = r24

    ldi unusedServoValue, 0xFF        ; control value for 'unused pwm control value'

PWMSetupSortLoop:

    clr changed                         ; changed is a flag that we set if we need to swap
                                        ; any values this time through the loop, we keep
                                        ; looping until we manage to get through the whole
                                        ; loop without swapping anything, and then we know that
                                        ; the values are in order.

    ldi ZL, LOW(PWM_DATA_START)         ; 'this' value, start at the begining of the PWM data
    ldi ZH, HIGH(PWM_DATA_START)
    ldi YL, LOW(PWM_DATA_START + 2)     ; 'next' value, note we compare values and ignore the pins
    ldi YH, HIGH(PWM_DATA_START + 2)

PWMSetupSort1:

    ld thisValue, Z                     ; load this value
    ld nextValue, Y                     ; load next value

    cp nextValue, unusedServoValue      ; if the next value is the 'unused pwm control value' 
    breq PWMSetupSortLoopEnd            ; we can skip the rest of this loop

    cp nextValue, thisValue             ; if next < this then swap them
    brlo PWMSetupSwap

    brne PWMSetupSortIncrement1         ; if they are not the same, increment the indices and move
                                        ; on to the next value

    cp nextValue, unusedServoValue      ; if they are the same but the are not UnusedServoValue, merge them
    brne PWMSetupMerge

PWMSetupSortIncrement1:

    adiw ZL:ZH, 1                       ; step on to this pin
    adiw YL:YH, 1                       ; step on to next pin

PWMSetupSortIncrement2:

    cpi YL, LOW(PWM_DATA_START + 0x0F)  ; check to see if we're at the last value...        
    breq PWMSetupSortLoopEnd            ; if so see if we need to restart the loop

    adiw ZL:ZH, 1                       ; step on to this pin
    adiw YL:YH, 1                       ; step on to next pin

    rjmp PWMSetupSort1                  ; sort the next two values...
    
PWMSetupSwap:

    st Z+, nextValue                    ; store the 'next' value in the 'this' position
    st Y+, thisValue                    ; store the 'this' value in the 'next' position
    ld thisValue, Z                     ; load the 'this' pin data
    ld nextValue, Y                     ; load the 'next' pin data         
    st Z, nextValue                     ; and swap those two
    st Y, thisValue 

    inc changed                         ; note that we changed something so that we run the whole 
                                        ; sort loop again

    rjmp PWMSetupSortIncrement2         ; since we swapped the values the indices are at the pin
                                        ; positions (value + 1) therefore we jump to the second 
                                        ; stage of our index increment code...

PWMSetupMerge:

    ; If the servo values are equal then we merge the pin values together and set the later servo
    ; value to UnusedServoValue which is 0xFF and will automatically bubble up to the end of the 
    ; list...

    st Y+, unusedServoValue             ; store UnusedServoValue to the 'next' value, we don't use that 
                                        ; value anymore

    adiw ZL:ZH, 1                       ; step on to this pin

    ld thisValue, Z                     ; load 'this' pin
    ld nextValue, Y                     ; load 'next' pin
    and thisValue, nextValue            ; and the pin values together to merge the bits set in into temp1
    st Z, thisValue                     ; and store it as the 'this' pin value

    inc changed                         ; note that we changed something so that we run the whole 
                                        ; sort loop again

    rjmp PWMSetupSortIncrement2         ; since we merged the values the indices are at the pin
                                        ; positions (value + 1) therefore we jump to the second 
                                        ; stage of our index increment code...

PWMSetupSortLoopEnd :

    tst changed                         ; did we change anything this time through the loop?
    brne PWMSetupSortLoop               ; if so we run the whole sort again...

    ; Now that the servo control data has been sorted into ascending pulse length order and the 
    ; duplicates have been merged we need to run through the pin control values and merge the later ones
    ; with the earlier ones so that each ascending pulse turn off value includes all of the earlier turn
    ; off signals... That is, if we have a sequence of 0x01 0xFE, 0x02 0xEF then we should and the first 
    ; off signal with the second to give a sequence of 0x01 0xFE, 0x02 0xEE. This means that as we work
    ; our way through the sequences we turn off progressively more pulses.

PWMSetupBitMergeSetup:

    ldi ZL, LOW(PWM_DATA_START + 1)     ; 'this' pin data, start at the first pin data element in the PWM
    ldi ZH, HIGH(PWM_DATA_START + 1)    ; data area.
    ldi YL, LOW(PWM_DATA_START + 3)     ; 'next' pin data
    ldi YH, HIGH(PWM_DATA_START + 3)

PWMSetupBitMerge:

    ld thisValue, Z+                    ; load 'this' value
    ld nextValue, Y                     ; load 'next' value

    and nextValue, thisValue            ; merge the two values so that pins that were off for 'this' are
                                        ; also off for 'next'
    st Y, nextValue                     ; store the new 'next' value.

    tst nextValue                       ; check to see if all pins are now off, if so, we're done
    breq PWMSetupBitMergeDone

    cpi ZL, LOW(PWM_DATA_START + 0x0E)  ; check for last value...        
    breq PWMSetupBitMergeDone

    adiw ZL:ZH, 1                       ; increment our indices, we already incremented Z once during our load
    adiw YL:YH, 2

    rjmp PWMSetupBitMerge

PWMSetupBitMergeDone:


    ; We now have a seqence of servo control values and pin control values sorted in ascending time order and
    ; our pin control values are such that as we step through them we progressively turn off more pins.

    ; Now we need to make the servo control values relative to each other so that the sequential values are 
    ; incremental rather than absolute; for example, 120 127 129 130 becomes 120 7 2 1


    ldi ZL,     LOW(PWM_DATA_START)    ; first servo control value
    ldi ZH,    HIGH(PWM_DATA_START)

    ld thisValue, Z+

    adiw ZL:ZH, 1

PWMSetupSequentialLoop :

    ld nextValue, Z

    cp nextValue, unusedServoValue      ; if we are at the end of the valid data...
    breq PWMSetupSequentialLoopEnd

    sub nextValue, thisValue
    st Z+, nextValue
    add thisValue, nextValue

    adiw ZL:ZH, 1

    rjmp PWMSetupSequentialLoop

PWMSetupSequentialLoopEnd :


    ; Now we need to convert the single byte servo control values into values suitable for our timer control.
    ; This requires us to multiply by 7 to give us a range of 0-1778 with the centre position as 889 for
    ; a servo control value of 127


    ; This changes the shape of the data from a list of two byte structures (servo control byte, pin control byte)
    ; to a list of three byte structres (servo high, servo low, pin control).

    ; At this point we zero out the entries that were merged and are now extra, i.e. the 0xFFs


    ldi ZL, LOW(PWM_DATA_START + 0x0E)      ; source data, we're working backwards along our list so that we can 
    ldi ZH, HIGH(PWM_DATA_START + 0x0E)     ; overwrite the data in place...
    ldi YL, LOW(PWM_DATA_START + 0x15)      ; our list is now 8 bytes longer than it was before as all the elements
    ldi YH, HIGH(PWM_DATA_START + 0x15)     ; are a byte bigger...

    ldi temp4, 7                            ; we multiply by 7 to get a range of 0-1778

PWMSetupMultiplyLoop:

    ld thisValue, Z+                        ; load the source data into temp1 ready for the multiply
    cp thisValue, unusedServoValue          ; if it's 0xFF (our invalid pwm data marker) we skip it
    breq PWMSetupSkipMultiply                   

    mul thisValue, temp4                       

    rjmp PWMSetupAfterMultiply

PWMSetupSkipMultiply:

    clr resl                                ; we need these to be zero so that we can write zeroes for the skipped
    clr resh                                ; elements...

PWMSetupAfterMultiply:

    ld thisValue, Z                         ; load the pin control data
    st Y+, resh                             ; store the new servo control value; low byte
    st Y+, resl                             ; and high byte
    st Y, thisValue                         ; store the pin control data


    cpi ZL, LOW(PWM_DATA_START + 1)         ; check for last value...        
    breq PWMSetupMultplyDone

    sbiw ZL:ZH, 3                           ; step our source index back 3 (we'd stepped forward 1 to get to the pin data)

    sbiw YL:YH, 5                           ; step our destination index back 5 (we'd stepped forward two whilst writing this data)

    rjmp PWMSetupMultiplyLoop               ; multiply the next value...

PWMSetupMultplyDone:

.undef unusedServoValue
.undef thisValue
.undef nextValue
.def temp1 = r22
.def temp2 = r23
.def temp3 = r24

    ; Now that we have the expanded relative timeout values in order we need to add our base time to the first
    ; timeout

    ; To get from the new control values to the absolute time in us we need to add 493, which gives a
    ; value of 1500 for 127, and a range of 611us - 2389us (493-2271 clock ticks)

.equ timerBase = 493                        ; if you're running with a 7.3728mhz clock - wider range

    ; Since the AVR doesn't have an add adci we subtract a negative number; see the examples with AVR202 application note
    ; adiw can only add a max of 63...

    mov temp1, resl
    mov temp2, resh

    subi temp1, low(-timerBase)
    sbci temp2, high(-timerBase)    

    sbiw YL:YH, 2                           ; step our destination index back to allow us to overwrite the value

    st Y+, temp2
    st Y+, temp1

    ; Now that we have our timeouts in order as relative times we need to work out the absolute time for
    ; the final timeout. Once we have this we can work out how much time we have left in our timeslot and 
    ; set a timeout for that time which brings us to the end of this batch of PWM signals and starts us on
    ; the next batch...

    clr resl
    clr resh

    ldi ZL, LOW(PWM_DATA_START)             ; source
    ldi ZH, HIGH(PWM_DATA_START)

PWMSetupAccumulateTotalTimeLoop:

    ld temp1, Z+        ; high
    ld temp2, Z+        ; low

    add resl, temp2
    adc resh, temp1
 
    ld temp1, Z+
    tst temp1
    brne PWMSetupAccumulateTotalTimeLoop

.equ maxTime = 2304                         ; if you're running with a 7.3728mhz clock 

    ldi temp1, low(maxTime)
    ldi temp2, high(maxTime)

    sub temp1, resl
    sbc temp2, resh

    st Z+, temp2                            ; store the final timer value that takes us to the end of our 2.5ms timeslot...
    st Z+, temp1

    ldi temp1, 0xFF                         ; final sentinel pin control value for the next loop...
    st Z+, temp1

    ; And finally.... Our interrupt processing from the point where the timer goes off to the point where we
    ; have turned off the PWM generation for the pins concerned takes up 1us, so we need to subtract that from the 
    ; times...

    ldi ZL, LOW(PWM_DATA_START)             ; source 
    ldi ZH, HIGH(PWM_DATA_START)
    ldi YL, LOW(PWM_DATA_START)             ; destination 
    ldi YH, HIGH(PWM_DATA_START)

PWMSetupIntTimeAdjustLoop:

    ld temp1, Z+
    ld temp2, Z+

    subi temp2, LOW(1)
    sbci temp1, HIGH(1)

    st Y+, temp1
    st Y+, temp2
    adiw YL:YH, 1

    ld temp1, Z+
    cpi temp1, 0xFF
    brne PWMSetupIntTimeAdjustLoop


    ; Now we set things up for the next time that the PWMSetup code is called, we increment the
    ; mux address by 4, and increment the bank address and rotate the bank mask...

    ldi temp1, 4
    add muxAddress, temp1

    lsl bankMask                ; adjust the bank mask for this bank 

    inc bankIndex               ; select next bank of servos, wrap to 0 at 8..

    cpi bankIndex, 8
    brne PWMSetupComplete

    clr bankIndex               ; reset the bank address
    clr muxAddress              ; reset the mux address

    ldi temp1, 1                ; reset the bank mask to bit 1 
    mov bankMask, temp1 

PWMSetupComplete:

    ; Now set up our Y pointer for use by the PWM pulse stop code and 
    ; adjust the timer that we set so that it goes off at the first PWM stop time

    ldi YL, LOW(PWM_DATA_START)        ; source 
    ldi YH, HIGH(PWM_DATA_START)

    ld temp1, Y+
    sts OCR1AH,temp1
    ld temp1,Y+
    sts OCR1AL,temp1

    ; Finally set our Z pointer back into the correct place in our jump table so that
    ; we're processing PWM pulse stops when the next interrupt goes off...

    ldi ZL, LOW(JUMP_TABLE_START + 1)       ; set up Z for the next timer interrupt
    ldi ZH, HIGH(JUMP_TABLE_START + 1)      ; we jump to the second entry in the jump table

    ; Setup is complete

    pop temp1          ; restore the state prior to the interrupt
    out SREG, temp1

    pop resh
    pop resl
    pop temp2
    pop temp1

    reti


; PWM pulse stop - timer interrupt handler for phase 2 of the PWM generation cycle
;
; PWM pulse stop phase. This timer interrupt handler turns off some pins on PORTB and then sets the
; next stop time and returns, once all pins are off the final timeout uses up the remaining part
; of the 2.5ms slot and sets the Z pointer to call the PWMSetup code to start the next batch of servos
;


PWMPulseStop :

    in pulseStopTemp1, SREG     ; save status register
    push pulseStopTemp1
    push temp1
    push temp2

    ld pulseStopTemp1, Y+

    ; load the next time...

    ld pulseStopTemp2, Y+
    sts OCR1AH, pulseStopTemp2
    ld pulseStopTemp2,Y+            ; Note that the 16-bit timer control registers MUST be written
    sts OCR1AL, pulseStopTemp2      ; high byte first then low byte as the low write triggers the
                                    ; 16-bit atomic write.

    ; this needs tuning...

    nop                
    nop



    mov pulseStopTemp2, pulseStopTemp1
    mov temp2, pulseStopTemp1

    ldi temp1, 0x0F
    and pulseStopTemp1, temp1                   ; we need the low nibble for port b
    swap pulseStopTemp2
    and pulseStopTemp2, temp1                   ; we need the high nibble for port c

    out PORTB, pulseStopTemp1                   ; Turn off servo pins...
    out PORTC, pulseStopTemp2                   ; Turn off servo pins...

    tst temp2                                   ; If all the pins are now off then the next timeout is our last
    brne PWMPulseSetNextTime                    ; and we switch back to setup phase when it expires...

    ldi ZL, LOW(JUMP_TABLE_START)               ; set up our first jump table entry for timer handling
    ldi ZH, HIGH(JUMP_TABLE_START)      

PWMPulseSetNextTime:

    pop temp2
    pop temp1
    pop pulseStopTemp1                          ; restore the state prior to the interrupt
    out SREG, pulseStopTemp1

    reti


; Serial protocol handling..

; The serial handling code can be interrupted at any time by the PWM generation code.
; We never turn off interrupts, so any two instructions in the serial handling code 
; could be separated by the PWM code running. Because of this we need to be careful
; about how we update the PWM configuration data. The PWM code will ONLY act on the 
; PWM configuration data for a servo if the stepEvery value is non zero. If the
; stepEvery value IS zero then only the currentPos will be used - and the PWM code
; will generate a signal for the specified current position. The servo is stationary.
; Before changing anything else the serial code should always set the stepEvery
; value to 0. This means that it can safely change the currentPos, targetPos and 
; stepSize and then change the stepEvery value to a non zero value for these other
; values to take affect. Immediate moves only involve changing the currentPos and
; these take effect immediately, delayed moves require the currentPos, targetPos, 
; stepSize and stepEvery to be changed and by observing the protocol detailed above
; these four values can be changed atomically with respect to what the PWM generation
; code will see.

; The serial code loops and accumulates a number of bytes into a complete command.
; Once a command is complete we process it, send any results and loop back to 
; accumulate another command. Each time through the byte accumulation loop that
; begins at SerialLoop we check the movesComplete register which is set by the PWM
; code. If this is non-zero then we send any delayed move completion notifications
; that need sending.

; Each command starts with a known byte. Most commands are of a fixed length though
; some can be variable length. Even variable length commands are treated like fixed
; length commands to start with; the fixed portion of their length being the part up
; to the length indicator embedded in the command. All commands consist of an initial,
; unique, command byte. Once we have this byte we can determine the length of the
; fixed part of the command. If we don't recognise the command code then we return
; an error.

; Note that all successful commands are echoed back to the caller. Invalid parameters
; are indicated by an error in the form 0xFF <bad param index> <command echo> where
; bad param index is a 1 based index into the parameters in the echoed command and
; indicates the first parameter that failed validation.

; Some commands, Stop and Query commands for example, also cause a notification to be
; returned once the command has been executed, these should be treated as asynchronous
; messages, though, in fact, they are guarenteed to occur immediately after the command
; echo completes.

SerialStart :

    ; we start with a zero command length, we're waiting for a command byte to work
    ; out how long the command will be...

    clr sCommandLength
    clr sExpectedBytes

SerialLoop :

    tst movesComplete                           ; check to see if we have any incremental
    breq SerialLoop1                            ; move completion notifications to send

    rcall SerialSendMoveCompleteNotification    ; send them...

SerialLoop1 : 

    lds temp1, UCSR0A
    sbrs temp1, RXC0
    rjmp SerialLoop

    lds serialChar, UDR0                        ; read the character

    tst sExpectedBytes                          ; are we already accumulating a command?
    brne SerialDataCharacter

    ; new command to accumulate?

    ldi XL, LOW(SERIAL_DATA_START)              ; new command, set pointer to buffer start
    ldi XH, HIGH(SERIAL_DATA_START)

    ; is the command byte valid??

    ; calculate the length of data required for valid commands...

    cpi serialChar, 0x00
    breq SerialSetCommandLengthGetInfo

    cpi serialChar, 0x41
    breq SerialSetCommandLengthSetPosn

    cpi serialChar, 0x42
    breq SerialSetCommandLengthSetDelayPosn

    cpi serialChar, 0x43
    breq SerialSetCommandLengthSetDelayPosn2

    cpi serialChar, 0x44
    breq SerialSetCommandLengthStopServo

    cpi serialChar, 0x45
    breq SerialSetCommandLengthStopServos

    cpi serialChar, 0x46
    breq SerialSetCommandLengthStopAll

    cpi serialChar, 0x47
    breq SerialSetCommandLengthQueryServo

    cpi serialChar, 0x48
    breq SerialSetCommandLengthQueryServos

    cpi serialChar, 0x49
    breq SerialSetCommandLengthQueryAll

    ; return an error for unrecognised commands...

    rjmp SerialError

SerialDataCharacter :

    ldi temp1, HIGH(SERIAL_DATA_END)            ; check for buffer overruns...
    cp temp1, XH
    brge PC+2
    rjmp SerialError

    cpi XL, LOW(SERIAL_DATA_END)                ; if we have already filled our buffer space 
    brne PC+2
    rjmp SerialError                            ; that's an error

    st X+, serialChar

    dec sExpectedBytes
    
    tst sExpectedBytes                          ; if we have accumulated the expected number of bytes
    breq SerialProcessCommand                   ; process the command

    rjmp SerialLoop


; Determine the length of the fixed portion of the commands.

SerialSetCommandLengthSetDelayPosn2 :

    ldi temp1, 5
    mov sExpectedBytes, temp1
    mov sCommandLength, temp1

    rjmp SerialDataCharacter

SerialSetCommandLengthSetDelayPosn :

    ldi temp1, 4
    mov sExpectedBytes, temp1
    mov sCommandLength, temp1

    rjmp SerialDataCharacter

SerialSetCommandLengthSetPosn :

    ldi temp1, 3
    mov sExpectedBytes, temp1
    mov sCommandLength, temp1

    rjmp SerialDataCharacter

SerialSetCommandLengthStopServo :
SerialSetCommandLengthQueryServo :
SerialSetCommandLengthStopServos :              ; This is a variable length command with a two byte header
SerialSetCommandLengthQueryServos :             ; This is a variable length command with a two byte header      

    ldi temp1, 2
    mov sExpectedBytes, temp1
    mov sCommandLength, temp1

    rjmp SerialDataCharacter

SerialSetCommandLengthGetInfo :
SerialSetCommandLengthStopAll :
SerialSetCommandLengthQueryAll :

    ldi temp1, 1
    mov sExpectedBytes, temp1
    mov sCommandLength, temp1

    rjmp SerialDataCharacter


; process the command...

SerialProcessCommand :

    ; we have an 'x' byte serial command...

    ldi XL, LOW(SERIAL_DATA_START)       
    ldi XH, HIGH(SERIAL_DATA_START)

    ld temp1, X+                                ; get the command byte...

    cpi temp1, 0x00
    brne PC+2
    rjmp SerialProcessCommandGetInfo

    cpi temp1, 0x41
    brne PC+2
    rjmp SerialProcessCommandSetPosn   

    cpi temp1, 0x42
    brne PC+2
    rjmp SerialProcessCommandSetDelayPosn

    cpi temp1, 0x43
    brne PC+2
    rjmp SerialProcessCommandSetDelayPosn2

    cpi temp1, 0x44
    brne PC+2
    rjmp SerialProcessCommandStopServo

    cpi temp1, 0x45
    brne PC+2
    rjmp SerialProcessCommandStopServos

    cpi temp1, 0x46
    brne PC+2
    rjmp SerialProcessCommandStopAll

    cpi temp1, 0x47
    brne PC+2
    rjmp SerialProcessCommandQueryServo

    cpi temp1, 0x48
    brne PC+2
    rjmp SerialProcessCommandQueryServos

    cpi temp1, 0x49
    brne PC+2
    rjmp SerialProcessCommandQueryAll

    rjmp SerialError


SerialProcessCommandGetInfo : 

    ; send back version major/minor then number of servos supported

    ldi serialChar, 0x00
    rcall SendSerial

    ldi serialChar, 0x06
    rcall SendSerial

    ldi serialChar, 0x00
    rcall SendSerial

    ldi serialChar, NUM_SERVOS
    rcall SendSerial

    rjmp SerialStart


; Set Servo Position.
; 0x41 <servo> <posn>
; This command is the same as the SSC 0xFF <servo> <posn> command. We set the
; specified servo to the specified position. The servo must be in the range 
; 0 - NUM_SERVOS and the position must be in the range 0 - 0xFE. We validate
; the parameters and send an error message if any are out of bounds.
; Since we could be setting a position when a delayed move is in progress we
; MUST set stepEvery to 0 before updating currentPos. Once stepEvery is 0
; we can update currentPos to the new desired servo position and then set 
; the other delayed move parameters to zero.

SerialProcessCommandSetPosn :

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                  ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoOutOfRange

    ld temp1, X+
    cpi temp1, 0xFF                             ; check the servo position is valid
    brne PC+2
    rjmp SerialPosnOutOfRange
    
    ; we have a servo index 0-NUM_SERVOS in servoIndex
    ; we have a control value 0-254 in temp1

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    ldi temp2, BYTES_PER_SERVO
    
    mul servoIndex, temp2

    add XL, resl
    adc XH, resh

    adiw XL:XH, 1       ; we must update the 'step every' value to 0 before changing anything else
                        ; or the PWM generation may get confused and use this data as part of a 
                        ; delayed move...
    clr temp2           

    st X, temp2        ; step every...

    sbiw XL:XH, 1

    ; now update the whole data structure, we reset unused values to zero, we need only really set
    ; current position and step every...

    st X+, temp1        ; current position
    st X+, temp2        ; step every...
    st X+, temp2        ; target position
    st X+, temp2        ; step size

    rcall SerialEchoCommand

    rjmp SerialStart


; Delayed Set Servo Position
; 0x42 <servo> <posn> <stepSize>
; Sets the servo to move from the current position to the specified position with the specified
; step size which increments each refresh of the PWM signal (that is, 20 times per second). 
; The servo must be in the range 0 - NUM_SERVOS and the position must be in the range 0 - 0xFE,
; the step size must be non zero. We validate the parameters and send an error message if any are 
; out of bounds.
; Since we're updating the multiple byte part of the servo control data we MUST set the stepEvery
; value to zero before we update anything else. Once that's done we update the targetPos and the
; stepSize. We then set the stepEvery value to 1 so that we step each cycle.

SerialProcessCommandSetDelayPosn :

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                  ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoOutOfRange

    ld temp2, X+
    cpi temp2, 0xFF                             ; check the servo position is valid
    brne PC+2
    rjmp SerialPosnOutOfRange

    mov sTargetPos, temp2

    ld temp1, X+
    tst temp1                                   ; check the step size is valid
    brne PC+2
    rjmp SerialStepSizeOutOfRange

    ; we have a servo index 0-NUM_SERVOS in servoIndex
    ; we have a control value 0-254 in sTargetPos
    ; we have a step size in temp1

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    ldi temp2, BYTES_PER_SERVO
    
    mul servoIndex, temp2

    add XL, resl
    adc XH, resh

    adiw XL:XH, 1       ; we must update the 'step every' value to 0 before changing anything else
                        ; or the PWM generation may get confused and use this data as part of a 
                        ; delayed move...
    clr temp2           

    st X+, temp2         ; step every...

    ; now update the rest of the data structure for a delayed move
    
    st X+, sTargetPos   ; target position (where we want to move to)
    st X, temp1         ; step size

    sbiw XL:XH, 2       ; move back to the 'step every' part of the data structure

    ldi temp2, 1

    st X, temp2         ; step every cycle...

    rcall SerialEchoCommand

    rjmp SerialStart


; Delayed Set Servo Position with step frequency
; 0x42 <servo> <posn> <stepSize> <stepEvery>
; Sets the servo to move from the current position to the specified position with the specified
; step size which increments at the specified frequency of the PWM signal (that is a value of 1
; steps 20 times per second, a value of 2 steps 10 times per second, a value of 20 once per second, 
; etc). The servo must be in the range 0 - NUM_SERVOS and the position must be in the range 
; 0 - 0xFE, the step size and the step frequency must be non zero. We validate the parameters and 
; send an error message if any are out of bounds.
; Since we're updating the multiple byte part of the servo control data we MUST set the stepEvery
; value to zero before we update anything else. Once that's done we update the targetPos and the
; stepSize. We then set the stepEvery value to the desired step frequency.

SerialProcessCommandSetDelayPosn2 :

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                  ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoOutOfRange

    ld temp2, X+
    cpi temp2, 0xFF                             ; check the servo position is valid
    brne PC+2
    rjmp SerialPosnOutOfRange


    mov sTargetPos, temp2

    ld temp1, X+
    tst temp1                                   ; check the step size is valid
    brne PC+2
    rjmp SerialStepSizeOutOfRange

    ld sStepEvery, X+
    tst sStepEvery                              ; check the step frequency is valid
    brne PC+2
    rjmp SerialStepEveryOutOfRange

    ; we have a servo index 0-NUM_SERVOS in servoIndex
    ; we have a control value 0-254 in sTargetPos
    ; we have a step size in temp1
    ; we have a step frequency in sStepEvery

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    ldi temp2, BYTES_PER_SERVO
    
    mul servoIndex, temp2

    add XL, resl
    adc XH, resh

    adiw XL:XH, 1       ; we must update the 'step every' value to 0 before changing anything else
                        ; or the PWM generation may get confused and use this data as part of a 
                        ; delayed move...
    clr temp2           

    st X+, temp2         ; step every...

    ; now update the rest of the data structure for a delayed move
    
    st X+, sTargetPos   ; target position (where we want to move to)
    st X, temp1         ; step size

    sbiw XL:XH, 2       ; move back to the 'step every' part of the data structure

    st X, sStepEvery    ; step every cycle...

    rcall SerialEchoCommand

    rjmp SerialStart


; Servo Stop functions

; Stop Servo
; 0x43 <servo>
; Stops the specific servo and causes a servo stopped response to be returned (after the command echo). 
; The servo must be in the range 0 - NUM_SERVOS. We validate the parameters and send an error message 
; if any are out of bounds. Note that in addition to the command echo this command also generates a
; single stop notificiation message in the form: 
; 0xFD <servo> <currentPos> <stepEvery> <targetPos> <stepSize> 
; If the servo was not moving when the stop command was sent then the <stepEvery> value in the 
; notification will be 0.

SerialProcessCommandStopServo :

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                  ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoOutOfRange

    rcall SerialEchoCommand

    rcall SerialStopServo

    rjmp SerialStart

; Stop Servos
; 0x44 <numservos> <servo1> <servo2> ... <servoN>
; Stops all of the specified servos. This is a variable length command.
; The first thing we do is check to see if we've been called once already. The initial
; command accumulation code works with a fixed length of 2 bytes, enough for us to
; accumulate the length of the actual command. If the command length is currently 2 then
; we go off to calculate the real length and continue to accumulate more data until we
; have a complete command.
; Once we have a complete command we loop over all of the servos and validate them. 
; this allows us to send the command echo, or error before we start to send 
; servo stop notifications.
; Finally we loop over the servos again and stop each one and send the appropriate
; notification

SerialProcessCommandStopServos :

    ; sCommandLength == 2 then our command length is actually
    ; 2 + value of length byte, else we're complete...

    ldi temp1, 2

    cp sCommandLength, temp1
    breq SerialProcessCommandStopServosSetRealLength

    ld temp1, X+                                            ; number of servos to process...

SerialProcessCommandStopServosValidationLoop :

    tst temp1                                               ; it's valid, though unusual, to have 0 servos...
    breq SerialProcessCommandStopServosValidationLoopEnd

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                              ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoListElementOutOfRange

    dec temp1

    rjmp SerialProcessCommandStopServosValidationLoop

SerialProcessCommandStopServosValidationLoopEnd :

    rcall SerialEchoCommand

    ; now process the command... 

    ldi XL, LOW(SERIAL_DATA_START)                          ; back to the start of the buffer
    ldi XH, HIGH(SERIAL_DATA_START)

    ld temp1, X+                                            ; throw away the command code...

    ld temp2, X+                                            ; number of servos to process...
    
SerialProcessCommandStopServosActionLoop :

    tst temp2                                               ; it's valid, though unusual, to have 0 servos...
    breq SerialProcessCommandStopServosActionLoopEnd

    ld servoIndex, X+

    rcall SerialStopServo

    dec temp2

    rjmp SerialProcessCommandStopServosActionLoop

SerialProcessCommandStopServosActionLoopEnd :

    rjmp SerialStart

SerialProcessCommandStopServosSetRealLength : 

    ; the stop servos command is in the form <cmd><length><servo><servo><servo>....
    ; where there are 'length' servo indexes after the length...
    ; the initial command length is set to 2, once we have 2 bytes we can work out
    ; the complete length...

    ld temp1, X+

    tst temp1                                           ; 0 is valid, though pointless!
    breq SerialProcessCommandStopServosValidationLoop

    mov sExpectedBytes, temp1

    ldi temp2, 2
    add temp1, temp2

    mov sCommandLength, temp1

    rjmp SerialLoop                                     ; back to the command accumulation loop!



; This function does the actual work of stopping a servo, also called from serial stop servos....
; Note that we first read the stepEvery value, then set it to zero to stop any further movement
; of the servo, we must then (and only then) read the currentPos of the servo as the PWM code
; will not change it once stepEvery is zero. 

SerialStopServo : 

    push XL
    push XH

    ; servo to stop is in servoIndex

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    ldi temp1, BYTES_PER_SERVO
    
    mul servoIndex, temp1

    add XL, resl
    adc XH, resh

    ld sCurrentPos, X+
    ld sStepEvery, X

    tst sStepEvery
    breq SerialStopServoAlreadyStopped

    clr temp1                               ; zero the step every value to stop the servo
    st X, temp1

SerialStopServoAlreadyStopped : 

    sbiw XL:XH, 1                           ; move back to the current pos as this is used
                                            ; in SerialSendStopResponse

    rcall SerialSendStopResponse

    pop XH
    pop XL

    ret


; Stop All Servos
; 0x46 
; Stops all of the servos that the controller controls.

SerialProcessCommandStopAll :

    rcall SerialEchoCommand

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    clr servoIndex

SerialStopAllServosLoop :

    ld sCurrentPos, X+
    ld sStepEvery, X

    tst sStepEvery
    breq SerialStopAllServosLoopServoAlreadyStopped

    clr temp1                   ; zero the step every value to stop the servo
    st X, temp1

SerialStopAllServosLoopServoAlreadyStopped : 

    sbiw XL:XH, 1               ; move back to the current pos...

    rcall SerialSendStopResponse      

    adiw XL:XH, 1               ; step over the final member of the structure 
                                ; to the next record       

    inc servoIndex

    cpi servoIndex, NUM_SERVOS
    brne SerialStopAllServosLoop

    rjmp SerialStart


; Sends a servo stopped notification

SerialSendStopResponse :

    ld sCurrentPos, X+              ; load current pos after we've set 'step every' to 0

    ldi serialChar, 0xFD
    rcall SendSerial
    
    mov serialChar, servoIndex      ; servo we stopped
    rcall SendSerial
    
    mov serialChar, sCurrentPos     ; position it was in
    rcall SendSerial
    
    
    adiw XL:XH, 1                   ; step the pointer over the 'step every' value

    mov serialChar, sStepEvery      ; how often it steped before we stopped it
    rcall SendSerial

    ld temp1, X+                    ; target position

    mov serialChar, temp1           ; where it's moving too
    rcall SendSerial

    ld temp1, X+                    ; step size

    mov serialChar, temp1           ; the size of the step
    rcall SendSerial

    ret


; Query Servo Position
; 0x47 <servo>
; Queries the position of the specific servo and causes a servo query response to be returned 
; (after the command echo). 
; The servo must be in the range 0 - NUM_SERVOS. We validate the parameters and send an error message 
; if any are out of bounds. Note that in addition to the command echo this command also generates a
; single query notificiation message in the form: 
; 0xFC <servo> <currentPos> <stepEvery> <targetPos> <stepSize> 
; If the servo was not moving when the query command was sent then the <stepEvery> value in the 
; notification will be 0. If the <currentPos> and the <targetPos> are not equal and the <stepEvery> value
; is non-zero then the servo is moving.

SerialProcessCommandQueryServo :

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                  ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoOutOfRange

    rcall SerialEchoCommand

    rcall SerialQueryServo

    rjmp SerialStart


; Query Servos
; 0x48 <numservos> <servo1> <servo2> ... <servoN>
; Queries all of the specified servos. This is a variable length command.
; The first thing we do is check to see if we've been called once already. The initial
; command accumulation code works with a fixed length of 2 bytes, enough for us to
; accumulate the length of the actual command. If the command length is currently 2 then
; we go off to calculate the real length and continue to accumulate more data until we
; have a complete command.
; Once we have a complete command we loop over all of the servos and validate them. 
; this allows us to send the command echo, or error before we start to send 
; servo query notifications.
; Finally we loop over the servos again and query each one and send the appropriate
; notification

SerialProcessCommandQueryServos :

    ; sCommandLength == 2 then our command length is actually
    ; 2 + value of length byte, else we're complete...

    ldi temp1, 2

    cp sCommandLength, temp1
    breq SerialProcessCommandQueryServosSetRealLength

    ld temp1, X+                                            ; number of servos to process...

SerialProcessCommandQueryServosValidationLoop :

    tst temp1                                               ; it's valid, though unusual, to have 0 servos...
    breq SerialProcessCommandQueryServosValidationLoopEnd

    ld servoIndex, X+

    cpi servoIndex, NUM_SERVOS                              ; check the servo index is valid
    brlt PC+2
    rjmp SerialServoListElementOutOfRange

    dec temp1

    rjmp SerialProcessCommandQueryServosValidationLoop

SerialProcessCommandQueryServosValidationLoopEnd :

    rcall SerialEchoCommand

    ; now process the command... 

    ldi XL, LOW(SERIAL_DATA_START)                          ; back to the start of the buffer
    ldi XH, HIGH(SERIAL_DATA_START)

    ld temp1, X+                                            ; throw away the command code...

    ld temp2, X+                                            ; number of servos to process...
    
SerialProcessCommandQueryServosActionLoop :

    tst temp2                                               ; it's valid, though unusual, to have 0 servos...
    breq SerialProcessCommandQueryServosActionLoopEnd

    ld servoIndex, X+

    rcall SerialQueryServo

    dec temp2

    rjmp SerialProcessCommandQueryServosActionLoop

SerialProcessCommandQueryServosActionLoopEnd :

    rjmp SerialStart

SerialProcessCommandQueryServosSetRealLength : 

    ; the query servos command is in the form <cmd><length><servo><servo><servo>....
    ; where there are 'length' servo indexes after the length...
    ; the initial command length is set to 2, once we have 2 bytes we can work out
    ; the complete length...

    ld temp1, X+

    tst temp1                                           ; 0 is valid, though pointless!
    breq SerialProcessCommandQueryServosValidationLoop

    mov sExpectedBytes, temp1

    ldi temp2, 2
    add temp1, temp2

    mov sCommandLength, temp1

    rjmp SerialLoop                                     ; back to the command accumulation loop!


; This function does the actual work of querying a servo, also called from serial query servos....

SerialQueryServo :

    ; servo to query is in servoIndex

    push XL
    push XH


    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    ldi temp1, BYTES_PER_SERVO
    
    mul servoIndex, temp1

    add XL, resl
    adc XH, resh

    rcall SendQueryResponse

    pop XH
    pop XL

    ret

; Query All Servos
; 0x49
; Queries all of the servos that the controller controls.

SerialProcessCommandQueryAll :

    rcall SerialEchoCommand

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    clr servoIndex

SerialQueryAllServosLoop :

    rcall SendQueryResponse      

    adiw XL:XH, BYTES_PER_SERVO     ; next servo...

    inc servoIndex

    cpi servoIndex, NUM_SERVOS
    brne SerialQueryAllServosLoop

    rjmp SerialStart


; Sends a query response

SendQueryResponse : 

    ldi serialChar, 0xFC
    rcall SendSerial

    mov serialChar, servoIndex      ; servo we queried
    rcall SendSerial

    ld sCurrentPos, X+
    
    mov serialChar, sCurrentPos     ; position it is in
    rcall SendSerial
    
    ld temp1, X+                    ; step every

    mov serialChar, temp1           ; how often it steps
    rcall SendSerial

    ld temp1, X+                    ; target position

    mov serialChar, temp1           ; where it's moving too
    rcall SendSerial

    ld temp1, X+                    ; step size

    mov serialChar, temp1           ; the size of the step
    rcall SendSerial

    sbiw XL:XH, 4                   ; move X back to where we started

    ret


; When the PWM code completes some delayed servo moves, that is, when
; the servo reaches the target position, it signals the serial code to
; send notifications. The siginally is based on the PWM code setting the
; movesComplete register (it sets the bit for the bank in which the 
; servo that has completed its move). Right now we ignore the bits and
; always check all servos if the movesComplete register is non zero.

; Note that to ensure that we don't miss any move completions we MUST 
; set the movesComplete register to 0 before we begin to process the
; servo data and send notifications. This is because the PWM code can
; interrupt us at any point and may complete more servo moves.

; Completion messages are in the form: 0xFE <servo> <posn>.

SerialSendMoveCompleteNotification :

    ; we could optimise this so that we only check the bank that is
    ; set in movesComplete...

    ; note that we've processed these completions...

    ldi movesComplete, 0x00

    ; loop over all the config data...

    push XL
    push XH

    ldi XL, LOW(POSITION_DATA_START)       
    ldi XH, HIGH(POSITION_DATA_START)

    clr servoIndex

SerialSendMoveCompleteNotificationLoop :

    ld sCurrentPos, X+
    ld sStepEvery, X+
    ld sTargetPos, X

    cp sCurrentPos, sTargetPos
    brne SerialSendMoveCompleteNotificationLoopIncrement

    tst sStepEvery
    breq SerialSendMoveCompleteNotificationLoopIncrement    

    sbiw XL:XH, 1

    clr sStepEvery
    st X+, sStepEvery

    ldi serialChar, 0xFE
    rcall SendSerial
    
    mov serialChar, servoIndex 
    rcall SendSerial

    mov serialChar, sCurrentPos
    rcall SendSerial

SerialSendMoveCompleteNotificationLoopIncrement :

    adiw XL:XH, BYTES_PER_SERVO - 2

    inc servoIndex

    cpi servoIndex, NUM_SERVOS
    breq SerialSendMoveCompleteNotificationLoopEnd

    rjmp SerialSendMoveCompleteNotificationLoop          

SerialSendMoveCompleteNotificationLoopEnd :

    pop XH
    pop XL

    ret 


; Echoes the command in the serial command accumulation buffer
; back to the client.

SerialEchoCommand :

    ; echo the command back to the sender...

    mov sExpectedBytes, sCommandLength

    ldi XL, LOW(SERIAL_DATA_START)        
    ldi XH, HIGH(SERIAL_DATA_START)

SerialEchoCommandSendNextByte :

    ld serialChar, X+ 
    rcall SendSerial

    dec sExpectedBytes

    tst sExpectedBytes
    brne SerialEchoCommandSendNextByte

    ret


; Sets up temp2 for parameter out of range errors.

SerialServoListElementOutOfRange : 

    ldi temp2, 1                    ; add one for the command length...
    add temp2, temp1 

    rjmp SerialParamOutOfRange 

SerialServoOutOfRange :   

    ldi temp2, 1                    ; index of the bad parameter

    rjmp SerialParamOutOfRange 

SerialPosnOutOfRange:   

    ldi temp2, 2                    ; index of the bad parameter

    rjmp SerialParamOutOfRange 

SerialStepSizeOutOfRange : 

    ldi temp2, 3                    ; index of the bad parameter

    rjmp SerialParamOutOfRange 

SerialStepEveryOutOfRange : 

    ldi temp2, 4                    ; index of the bad parameter

    rjmp SerialParamOutOfRange 


; Sends a parameter out of range error; 0xFF <param> <command echo>
; where <param> is a 1 based index into the parameters in the command
; that indicates which parameter is invalid.

SerialParamOutOfRange : 

    ; send the error message back to the sender...

    ldi serialChar, 0xFF            ; command error response
    rcall SendSerial

    mov serialChar, temp2           ; index of parameter that is out of range
    rcall SendSerial

    rcall SerialEchoCommand         ; the command...

    rjmp SerialStart


; Sends a serial error, which is simply a parameter out of range error
; with a parameter index of 0. 

SerialError :

    ; we could light a led and only unlight it on valid data...


    ldi temp2, 0                    ; index of the bad parameter

    rjmp SerialParamOutOfRange 


; Sends a byte out of the serial port.

SendSerial : 

    lds temp1, UCSR0A
    sbrs temp1, UDRE0
    rjmp SendSerial

    sts UDR0, serialChar

    ret