is the pulse form signal changing properly to the servos (if not a power issue)
not sure if ths helps you out at all but here is some of the code from my HC11 project from way back when... can't say the variations in forward roll worked as i was having issues. (got the pass from making the robot do a perfect square and the follow it with a figure eight. let loose some of the magic smoke and cooked the A-to-D on the IC a few days before final test. the A-to-D was where the sensor inputs were going. instructor knew 'bot worked but had to grade me for something
)
here's the skeleton to get the jyst. (sorry its Motorola assembler) you would add some code to instigate a change to servo speed in the main before the BRA . command (branch always? been a while)
.TITLE SERVOTEST
; THIS PROGRAM IS TO TEST THE SERVO OUTPUTS ON THE E2 BOARD
; EQUATES
TCNT = 0x100E ;16 BIT TIMER COUNT REG
TOC1 = 0x1016 ;TIMER OUTPUT COMPARE 1
TOC2 = 0x1018 ;TIMER OUTPUT COMPARE 2
TOC3 = 0x101A ;TIMER OUTPUT COMPARE 3
TOC4 = 0x101C ;TIMER OUTPUT COMPARE 4
TCTL1 = 0x1020 ;TIMER CONTROL REG 1
TCTL2 = 0x1021 ;TIMER CONTROL REG 2
TMSK1 = 0x1022 ;TIMER MASK REG 1
TFLG1 = 0x1023 ;TIMER FLAG REG 1
TMSK2 = 0x1024 ;TIMER MASK REG 2
TFLG2 = 0x1025 ;TIMER FLAG REG 2
RAMHI = 0x00FF ;LAST ADDRESS OF INTERNAL RAM
REGBAS = 0x1000 ;REGISTER BASE ADDRESS
BPROT = 0x0035 ;OFFSET FROM BASE FOR BPROT
PPROG = 0x003B ;OFFSET FROM BASE FOR PROG
EESTRT = 0xF800 ;FIRST ADDRESS EEPROM ON 811E2
RSETVEC = 0xFFFE ;ADDRESS OF RESET VECTOR
ERSRAM = 0x0000 ;ADDRESS IN RAM THAT ROUTINES
;WILL BE COPIED TO
.AREA SERVOTEST(ABS)
.MODULE SERVOTEST
.ORG EESTRT
MAIN: LDS #RAMHI ;SET STACK POINTER
LDAA #0x55 ;ENABLE TOGGLE FOR 0C2, 0C3, 0C4, 0C5
STAA TCTL1
LDAA #0x78 ;ENABLE INTERRUPTS FOR OC2, OC3, OC4, OC5
STAA TMSK1
CLI ;ENABLE ALL MASKABLE INTERRUPTS.
BRA .
; OC2 CENTER SERVO SIGNAL AT 1.5ms
; @50% DUTY CYCLE -> 750us
; 750us/500ns = 1500
OC2_ISR:
LDAA #0x40 ;CLEARS OC2 FLAG
STAA TFLG1
LDD TOC2 ;SET NEW CONTROL VALUE IN TOC2
ADDD #1500
STD TOC2
RTI
; OC3 SERVO REVERSE (SLOWER SIGNAL) 3ms
; @50% DUTY CYCLE -> 1.5ms
; 1.5ms/500ns = 3000
OC3_ISR:
LDAA #0x20 ;CLEARS OC3 FLAG
STAA TFLG1
LDD TOC3 ;SET NEW CONTROL VALUE IN TOC3
ADDD #3000
STD TOC3
RTI
; OC4 SERVO FORWARD (FASTER SIGNAL) 750us
; @50% DUTY CYCLE -> 375us
; 375us/500ns = 750
OC4_ISR:
LDAA #0x10 ;CLEARS OC4 FLAG
STAA TFLG1
LDD TOC4 ;SET NEW CONTROL VALUE IN TOC4
ADDD #750
STD TOC4
RTI
; OC5 SERVO FORWARD (RUSH) 375us
; @50% DUTY CYCLE -> 187.5us
; 187.5us/500ns = 375
OC5_ISR:
LDAA #0x08 ;CLEARS OC5 FLAG
STAA TFLG1
LDD TOC1 ;SET NEW CONTROL VALUE IN TOC1
ADDD #375
STD TOC1
RTI
.ORG 0xFFE6
.DW OC2_ISR
.ORG 0xFFE4
.DW OC3_ISR
.ORG 0xFFE2
.DW OC4_ISR
.ORG 0xFFE0
.DW OC5_ISR
.ORG 0xFFFE
.DW 0xF800