David Vanhorn's Servo.asm
A Servo control code for eight standard R/C servos on PORTC
A Servo control code for eight standard R/C servos on PORTC
| CODE |
;*************************************************************************** ; ; File Name :'SERVO.asm" ; Title : ; Date : ; Version : ; Support telephone :765 287 1987 David B. VanHorn ; Support fax :765 287 1989 ; Support Email :dvanhorn@cedar.net ; Target MCU :AT90S8515 ; ;***************************************************************************; ; M O D I F I C A T I O N H I S T O R Y ; ; ; rev. date who why ; ---- -------- --- ------------------------------------------ ; 0.01 98.08.20 dvh Creation ; 0.02 98.08.21 dvh Added code for a frame rate, driven by timer 0 ; (the 1ms opsys tick) This is set to XX milliseconds ; in EQUATES.INC, 20 is typical, Less seems ok, down to ; zero if 8 channels are active. If less are active, there ; will need to be some delay time set in FRAME_RATE ; Changed Frame_Delay from register to SRAM ; Regularized to XL,XH notation instead of R26.R27 etc ; 0.03 98.09.06 So much for specs. My new Cirrus servos require 400uS ; to 2mS pulses for full travel. It seems that different ; brands have different expectations. Now I have to ; calculate range and increments on the fly. Bummer. ; 0.04 98.09.27 The 16 bit servo code is working. We now have 256 bit ; resolution, over an arbitrary range. Set minimum and ; maximum widths per servo, which handles the fact that ; different servo brands have different pulse width ; requirements ; ;******************************************************************** ;DEBUGGING NOTES ; ;Problem: The servo outs are working, but only up to a value of 64-ish, then it ;appears that the top two bits are output on the subsequent channel. This is independent ;of the channel, The top 2 of 1 appear on 2, 3 appears on 4... ; ;I've tried dedicating an entire set of registers, solely to this application. ;I've tried making the width data directly, rather than bringing it in from SRAM ;I've tried dedicating a special register to the ISR ;I've tried changing the order the servos operate in. ;I've tried pushing and popping all my temps in this routine ;I've tried turning off the call to RANDOM in Main ;I've tried turning off serial buffer code in the ISRs ; ;Finally found it, RTFM bug. Page 5-39 says that you must ;load the high byte of timer 1 first because of how the system ;loads the 16 bit value. From the above, you can see that a ;simple oversight can lead to much frustration. ; ;******************************************************************** ; ;This routine drives eight standard RC servos connected to port C (easily changed) ;The number of servos could be reduced fairly easily by simply changing the test ;in the TIM1_OVF of ISR.ASM ; ;The servo control bytes are stored as SERVO_X(1-8) in SRAM, Modify as you like, ;the servo position will be updated when it's turn comes up next. ; ;Servo outputs are activated sequentially, so that no more than one servo is active ;at any time. The servos may take some time to physically slew to their new positions, ;but we are only talking to one of them at a time. This is also how standard R/C ;equipment works, so there's nothing new here. ; ;PORTC's current status tells us wether a servo is currently active, there should only ;ever be a single high bit on this port. ; ;Servo_Control determines wether any servos should be active. Every time timer1 expires, ;it shifts the Servo_Control byte. If the active bit shifts into carry, then it re-inits ;Servo_Control. Setting Servo_Control to zero will cause the system to quit outputting ;servo pulses. ; ;Frame_Delay is a timer byte, decremented by the Timer0 int every millisecond. It is ;initialized by the timer1 ISR to some value (set in EQUATES.INC). Before we output the ;first servo pulse, we must see Frame_Delay decrement to zero. This assures a constant ;frame rate, no matter what the widths of the servo pulses is, since the timers run ;concurrently. ; ;Servo_X_Reload is a base value, apparently differs by brand. Futaba wants 1mS, Cirrus ;wants about 400uS. ; Servo_Frame_Check: ; ;The Servo_Control byte tells us which servo output will be active. ;If it's zero, then no servos are active, and we don't send any output. ;Otherwise, it's just a matter of picking which one to send a signal to. ;Without any pulses, the servos go to idle and may be pushed by external forces. ;First, we check if there are any servo outs currently running, if so, ;we want to let them expire (TIM1_OVF in ISR.ASM) before doing anything else. ;This means that the vast majority of the time, we just do this test ;and bail, preserving CPU time for other things! :) in TEMP,PORTC ;Get current servo outputs and TEMP,TEMP ;Anything busy? brne Servo_Quit ;Yes, just leave them alone! ;Now, we check wether ANY servos SHOULD be active. There's only one ;bit on in here at any time, starting with bit zero. (Servo_1) ;If no bits are active, then we can just bail, since we aren't ;outputting any servos at this time (servos are free-floating then, ;and can be moved since they aren't seeking any position actively) lds TEMP,Servo_Control ;Get from SRAM and TEMP,TEMP ;Is it zero? breq Servo_Quit ;Nope, just pass ;We've determined that a servo should be active ;If it's servo 1, then we want to check if the frame delay has expired. ;FRAME_DELAY is decremented to zero by timer 0 (the 1ms opsys tick) ;If it hasn't expired, then again we bail out. If it has then we go ahead ;and start servo 1, and reload FRAME_DELAY. Note that this test is only done ;before SERVO_1 is output, it's not a delay between all channels, just between ;SERVO_8 and SERVO_1. cpi TEMP,$01 ;Is it servo 1 brne Servo_Pulse ;If not, just continue lds TEMP,Frame_Delay ;Get the frame delay byte and TEMP,TEMP ;If so, check if the servo frame delay has expired brne Servo_Quit ;If not, then don't do anything till it does. ;If this is servo 1, and FRAME_DELAY has expired, then reload FRAME_DELAY, and ;go ahead with the servo pulses ldi TEMP,FRAME_RATE ;Dec'd by Timer 0 sts Frame_Delay,TEMP ;Put it back ;DEBUG This routine fakes some different outputs on the servo channels. rcall Servo_Debug ;Some different outputs on channels 1-4 rjmp Servo_Pulse ;Go output a pulse Servo_Quit: Ret ; ;All this has to change now. I need a variable base value, and ;possibly a variable max value, and I want 8 bit resolution ;between them. ; ;Position value, 0-255 represents min to max. ;Divide the servo range by 255, to get uS per bit ;Set the timer prescaler to <1uS ; 000 = Stop ; 001 = CK 125nS/count ; 010 = CK/8 1uS ; ;So, counts = uS * 8. 400uS = 3200 counts. ; 2000uS = 16000 counts. ; Max counter range is 64000 counts (FFFF) Servo_Pulse: ;Figure out which servo time to pick up, 0-7 (in temp) ldi TEMP2,0 ;Init TEMP2 lds TEMP,Servo_Control ; ;As we shift our copy of the Servo_Control right twoard carry, Temp2 is ;incremented to form an offset, which we will add to the base of the ;servo time widths in RAM to obtain the right width for this servo ; Servo_PTR: ; lsr TEMP ;Shift the servo bit twoard carry brcs Servo_Point ;If carry, then we have our servo inc TEMP2 ;Otherwise inc, and rjmp Servo_PTR ;try again ;Now we have the index, set the main pointer in R30,31, and grab the ;proper servo width byte from SRAM ;TEMP2 has an integer offset Servo_Point: ;Point at the base of the servo times in RAM ldi ZL,low(SERVO_1) ;Point at the lowest servo ldi ZH,high(SERVO_1) ; ;Add the offset to pick SERVO_X clc ; adc ZL,TEMP2 ;Add the servo number (0-7) ;Handle carry if the location crosses a boundary brcc Servo_Point_B ;No carry, we're done inc ZH ;else inc the high byte of the pointer ;Retrieve the servo width data Servo_Point_B: ld TEMP3,Z ;Get the servo time ;At this point, TEMP2 has the servo number (0-7) ;Temp3 has the proportional width that we would like to set it to. ;At 8 mhz, /1 prescaler gives us a granularity of 0.125uS. ;The timer counts up to zero, so we need to subtract from the timer ;The servo width value is proportional, 0-255 to the particular Servo_Range ;which may be as much as 2000uS (reload value of 16000 (FFFF-3E80=C17F)) ; ;Get the base pulsewidth out of the min table ldi ZL,low(Servo_Min_Table*2) ;Point at the minimum widths table ldi ZH,high(Servo_Min_Table*2); clc ; mov TEMP,TEMP2 ;Take the servo index lsl TEMP ;Mult by 2 adc ZL,TEMP ;Add to Z low brcc Servo_Point_C ;No carry, we're done inc ZH ;else inc the high byte of the pointer ;Retrieve the servo base width data Servo_Point_C: lpm ;Table to R0 mov TEMP5,R0 ;Put it in TEMP5 (low) ld TEMP4,Z+ ;Move the pointer lpm ;Table to R0 (other half) mov TEMP4,R0 ;Put it in TEMP4 (high) Servo_Point_D: ;At this point, TEMP4,5 has FFFF - the servo base value for this servo. ;Now we have to apply the width value to servo_range, to figure out ;where to point it. ;For cirrus this is a range of 1600uS or 1000 for futaba, which works out ;to 12800 counts or 8000 ;We divide this range value by 256, then multiply by the servo value.(0-255) ;Get the servo_range for this servo, stored as a 16 bit value into TEMP6(h),TEMP7(l) ldi ZL,low(Servo_Range_Table*2);Point at the lowest servo ldi ZH,high(Servo_Range_Table*2) ; clc ; mov TEMP,TEMP2 ; lsl TEMP ; adc ZL,TEMP ;Add the servo number (0-7) brcc Servo_Point_E ;No carry, we're done inc ZH ;else inc the high byte of the pointer Servo_Point_E: lpm ;Table to R0 mov TEMP7,R0 ;Get the servo base width in microseconds ld TEMP6,Z+ ;Move the pointer lpm ;Table to R0 mov TEMP6,R0 ;Low ;Divide the servo_range by 256, result in ??? ldi TEMP,8 ; mov LOOP,TEMP ; Servo_Point_EA: lsr TEMP6 ;/2 ror TEMP7 ; dec LOOP ;8 times brne Servo_Point_EA ; ;129 is as big as it gets, so it's an 8 bit quantity now, in TEMP7 ;Multiply by the servo width value, result in TEMP4,5 mov LOOP,TEMP3 ; ;Subtract the 16 bit result from the timer Servo_Point_EB: clc ; sub TEMP5,TEMP7 ; brcc Servo_Point_EC ; dec TEMP4 ; Servo_Point_EC: dec LOOP ; brne Servo_Point_EB ;Now TEMP4,5 contains a 16 bit reload value representing the base ;width, plus the proportional part ;Load timer 1, Timer high byte MUST be loaded first, ;per databook 5-39, else wierdness will ensue. ; out TCNT1H,TEMP4 ;Timer high byte out TCNT1L,TEMP5 ;Timer low byte ;Start timer 1 in TEMP,TCCR1B ;Get the timer control register andi TEMP,$F8 ;Mask out the timer bits ori TEMP,$01 ;Set the prescaler to "/1" (8MHz at 8.0 MHz) out TCCR1B,TEMP ;Make it so. ;Start the servo output active lds TEMP,Servo_Control ;Get the active servo bit out PORTC,TEMP ;Start the output ret ; ; ;******************************************************************* ; ;Mix, take a servo width, and apply some amount of it to another channel. ; Servo_Mix: ;First I need to know the source channel ;then the destination ;Then I read the source and the destination. ; ;Servo outs are generally seen as + or - around zero (128) ;So, I need to look at it as signed data. ;Ex: 175 would be +50, and 75 would be -50 ;A mix ratio of 50% would give +25 and -25 respectively, ;and then I need to add or subtract these amounts to the ;destination channel, regardless of it's original position, ;but if the operation causes rollover (carry) then I need to ;set the channel to maximum travel in that direction, (0 or 255) ;rather than allowing it to wrap across to the opposite side! ; ;Then apply the mix ratio to the source, ;and to the destination. Both could be positive or negative. ; ; ret ; ;******************************************************************* ; ;Add a bit of randomness to the servo outputs ; ;From observing the servos, it seems that they only resolve to about 6 bits ;of real precision. Any changes smaller than that don't seem to produce shaft ;output. This is called "deadband", and keeps the servo from drawing excessive ;power seeking around for a precise position. ; ;I thought it might be interesting to have the servo take up positions that are ;slightly different from the signalled position, so I made this little quicky, which ;substitutes some bits of the random bytes into the servo byte. ; ;Input, servo width in temp, Output, altered servo width in temp. ; Servo_Dither: andi TEMP,$F8 ;Mask out the lowest 3 bits mov TEMP2,RAND2;Get some randomness ori TEMP2,$07 ;We'll use these bits or TEMP,TEMP2 ;Replace the low 3 bits with the random ones. ret ; ;******************************************************************* ; ;Just a debug routine to fake in some default data ; ;For the demo of two different servos (with different min and range) ;on channels 1,2, make sure to start them with the same width. ; Servo_Set: ldi TEMP,$FF ; sts Servo_1,TEMP ;Set a full wide pulse ldi TEMP,$FF; sts Servo_2,TEMP ;Set a 1/2 wide pulse ldi TEMP,$40 ; sts Servo_3,TEMP ;Set a 1/4 wide pulse ldi TEMP,$20 ; sts Servo_4,TEMP ;Set a 1/8 wide pulse ldi TEMP,$10 ; sts Servo_5,TEMP ;Set a 1/16 wide pulse ldi TEMP,$08 ; sts Servo_6,TEMP ;Set a 1/32 wide pulse ldi TEMP,$04 ; sts Servo_7,TEMP ;Set a 1/64 wide pulse ldi TEMP,$00 ; sts Servo_8,TEMP ;Set a minimum pulse ret ; ; ;Just some code to make the servos twitch, it's called before servo1 each time ;Frame_Delay rolls to zero ; Servo_Debug: ; ;DEBUG This code will smoothly ramp a given servo up to FF ;Normally, this code will not be used ; ;Ramps both servo 1 and servo 2, which are set as different types. ;They should run at the same rate, and to the same positions. ; lds TEMP,Servo_1 ;Change this to SERVO_X as desired inc TEMP ; sts SERVO_1,TEMP ; lds TEMP,Servo_2 ;Change this to SERVO_X as desired inc TEMP ; sts SERVO_2,TEMP ; ;or for manual control.. in TEMP,PIND ;Input the switches ;sts SERVO_1,TEMP ;Set servo 1 from the buttons! ;sts SERVO_2,TEMP ;Set servo 2 from the buttons! sts SERVO_3,TEMP ;Set servo 3 from the buttons! ;sts SERVO_4,TEMP ;Set servo 4 from the buttons! ;sts SERVO_5,TEMP ;Set servo 5 from the buttons! ;sts SERVO_6,TEMP ;Set servo 6 from the buttons! ;sts SERVO_7,TEMP ;Set servo 7 from the buttons! ;sts SERVO_8,TEMP ;Set servo 8 from the buttons! ;Normally, this code will not be here sts SERVO_4,RAND1 ;Pick a new random position ret ; |