; MicroMouse PID control
; Code fragment for PID control of MicroMouse lateral postion, central
; between maze walls.
; DFR and DFL are 7-bit unsigned numbers from side-looking IR distance
; sensors.
; ADD16, DIV16, MUL8, DIV8 are maths subroutines.
; W.G.Marshall 2002

            clr    C
            mov    A, DFR           ; Subtract lateral sensor readings
            subb   A, DFL           ; to form lateral position error (P)
            mov    POSTEMP, A       ; Save new lateral position error

            mov    r0, A            ; Convert 8-bit signed number
            clr    A                ; to 16-bit signed number.
            jnc    PosNo
            cpl    A
PosNo:      mov    r1, A

            mov    r2, INTEG        ; Get current integral value
            mov    r3, INTEG+1
            lcall  ADD16            ; Form new Integral error (I)
            mov    INTEG, r0
            mov    INTEG+1, r1
            mov    r2, KI           ; Scale integral by Ki factor
            mov    r3, #0
            lcall  DIV16
            mov    INTL, r0
            mov    A, POSTEMP       ; Get new lateral position error
            clr    C
            subb   A, OLDPOS        ; Form lateral rate error (D)
            mov    r0, A
            mov    r1, KD           ; Scale rate error by Kd factor
            lcall  MUL8
            mov    RATE, r0         ; Save scaled rate error
            mov    r0, POSTEMP
            mov    r1, KP           ; Scale position error by Kp factor
            lcall  DIV8
            mov    A, r0            ; Get scaled position error
            add    A, RATE          ; Add scaled rate error
            add    A, INTL          ; Add scaled integral error
            add    A, #70H          ; Add control factor to reference PWM
            mov    CCAP1H, A        ; Update PWM value for LH motor
            mov    OLDPOS, POSTEMP  ; Replace old with new position error
            ret

            end