; ; ESC Servo Control Software ; Written 12/29/94 by Chuck McManis ; ; Copyright (c) 1994-1995, Chuck McManis all rights reserved. ; ; Modification history: ; * 12/19/94 - Initial release ; * 1/12/95 - Modified to use PORT A for output. ; * 1/18/95 - Timing constants assume a 20Mhz clock. DEVICE PIC16C54,HS_OSC,WDT_OFF,PROTECT_OFF RESET INIT ; ; Data space - F registers used ; Count_Hi EQU 8 ; High Byte of the Count Count_Lo EQU 9 ; Low Byte of the Count Temp EQU 10 ; Temporary variable PWM_Cmd EQU 11 ; PWM Constant PWM_Tmp EQU 12 ; Temporary counter for PWM count down Cmd_Reg EQU 13 ; Motor control command Cmd_Tmp EQU 14 ; Temporary copy of CMD_REGS Outer_Cnt EQU 15 ; Outer loop count Inner_Cnt EQU 16 ; Inner Loop count Glitch EQU 17 ; Our glitch count ; ; Bridge Control definitions. The output bits are connected as follows: ; Bit 3 - Left High Side Switch ; Bit 2 - Right High Side Switch ; Bit 1 - Left Low Side Switch ; Bit 0 - Right Low Side Switch Forward_Cmd EQU 89h ; Move forward (Left high + Right lower) Reverse_Cmd EQU 46h ; Move Backward (Right High + Left Lower) Coast_Cmd EQU 00h ; Coast (all off) Brake_Cmd EQU 33h ; Brakes on (Left Lower + Right Lower). Glitch_Count EQU 172 ; 860uS (172 * 5 uS, 20 Mhz clock) DeadBand EQU 6h ; Deadband range + 1 ; ; System initialization code ; ORG 0 Init mov W,#0 ; All bits as outputs. tris RA ; Output port is all outputs. Clr RA ; And all outputs OFF. mov w,#Glitch_Count mov Glitch,w ; Initialize vars mov Count_Lo,w ; Initialize count to 0x1A0 mov Count_Hi,#1 ; ; ; IDLE combines both the Idle and the Measure states. This is ; accomplished by the routine DO_MEASURE which can be called when ; measuring and when idle. ; Idle call Do_Measure ; Measure input pulse mov temp,w ; Store result in TMP test temp ; Waste time, set Z bit jnz PWM ; If non-zero, use new value. jmp $+1 ; Waste time nop ; ... jmp Idle ; Go back to measuring ; Total Clocks in Loop 25 ; ; PWM is the loop that generates the PWM output. We do this ; by turning on the H-Bridge and counting down PWM_CMD steps and ; then turning off the bridge. The loop count is effectively 100. ; This gives a direct percentage relationship between the value ; in PWM_CMD and the output. The value 1 is a 1% duty cycle square ; wave and the value 99 is a 99% duty cycle. (100 is 100%) So ; The range of legal inputs is 1 thru 100. ; ; The loop is structured to take 500 uS, the inner loop takes ; 5uS and the outer loop is designed to take 5uS as well. ; Thus 99 inner loop iterations and one outer loop pass == 100 * 5us ; which is 500uS. That yields a PWM frequency of 2000 Hz. ; The outer loop has 40 iterations so that after 20mS, if no additional ; pulse has been received the loop exits and the H-bridge turns off. ; ; When we exit due to a timeout we must make sure that we keep calling ; DO_MEASURE on schedule because we may be in the middle of measuring a ; new input pulse. So we dovetail the outer loops return to idle with ; IDLE making the transistion from the PWM+Measuring state to the ; Idle+Measuring state seamless. ; PWM mov Outer_Cnt,#40 ; Constant for 20 mS mov PWM_Tmp,PWM_Cmd ; Set up the inner loop counters mov CMD_Tmp,CMD_Reg ; Put Commmand into temporary mov RA,W ; Turn on the motors mov Inner_Cnt,#99 ; Inner loop count jmp PWM_LOOP ; Line up command PWM_Loop call Do_Measure ; Do a "measurement" ; ; If DO_MEASURE found a pulse, it will modify the control variables ; ; PWM_TMP - Set to FF to prevent swaping CMD_REG ; INNER_CNT - Set to 1 so that this loop will exit immediately ; OUTER_CND - Set to 41 so that the outer loop will run for 20mS ; PWM_CMD - Set to the new PWM constant ; CMD_REG - Set to the new H-Bridge Command ; dec PWM_Tmp ; Decrement the "On" time snz ; If not zero continue swap Cmd_Tmp ; Turn off the low side mov RA,Cmd_Tmp ; Get the Command Register mov w,PWM_Cmd ; Sort of a NOP decsz Inner_Cnt ; Decrement inner loop count jmp PWM_Loop ; Continue Looping ; Total Clocks (inner loop) 25 (5 uS) ; ; Outer loop count down, Note we've combined the MOVF PWM_CMD,W statement ; above with the MOVWF PWM_TMP statement below to accomplish ; reinitializing PWM_TMP in only 1 cycle. This works because the DECFSZ ; above doesn't change W. Without this trick there is no way for the ; outer loop to work in the required 5uS and we lose the ability to ; call do_measure on schedule. ; mov PWM_Tmp,w ; Restore PWM command (loaded above) Call Do_Measure ; This is on a 5uS boundary. mov Inner_Cnt,#99 ; Reset inner loop count mov Cmd_Tmp,Cmd_Reg ; Put command into W mov RA,W ; set motor outputs dec Outer_Cnt jnz PWM_Loop ; ; Getting here means we've timed out our 20 mS of looping. However we ; may be in the process of receiving a pulse so we make sure we call ; DO_MEASURE on schedule and essentially duplicate what the IDLE loop ; does and when we're done we loop to IDLE. ; clr RA ; Turn off the motors call Do_Measure ; mov Temp,W ; Store result in TMP test Temp ; Waste time, set Z bit jnz PWM_Loop ; If non-zero use new value. jmp $+1 ; else Waste time nop ; ... jmp Idle ; Go back to measuring ; ; DO_MEASURE is the pulse measuring routine. It is called from either ; Idle or PWM. It implements a mini-state machine the monitors the ; input pin. The code is written so that it is called every 5uS. ; When called, the input will either be high or low. Depending on the ; input pins state, it either measures or doesn't. ; If the input pin is high, we start counting down our counter. ; Each count represents 5uS. If we ever underflow our counter we ; note that as a timeout (the input pulse was too wide.) ; ; If the input pin is low, we process a potential pulse. If glitch ; is non-zero (input pulse < 860uS) we treat it as a glitch, reset ; the counters and return. (Note this happens every time when the ; input pin is low.) ; ; If Glitch is zero we check for a counter underflow (timeout). If ; the counter has timed out we reset the counters and return. (Note ; this happens every time when the input pin stays high) ; ; If we pass both of these tests (not a glitch, not a timeout) we ; process it as a valid pulse, set all of the variables ; appropriately ; and return. ; ; Given the Design, this routine _MUST_ always execute in 14 clocks. The ; The clock counts in the right margin represent the time spent in ; various "paths" through this code. ; Do_Measure jb RB.0,Inp_High ; Check for 'high' test Glitch ; Check Glitch counter jnz No_Pulse ; != 0 means Glitch. jnb Count_Hi.7,Got_Pulse ; Check for overflow mov w,#Glitch_Count ; Glitch length mov Glitch,w ; Store it in GLITCH mov Count_Lo,w ; Just a glitch, mov Count_Hi,#1 ; Load count with constant retw 0 ; Total 14 clocks. ; ; This is basically an abort, it resets the counters and state variables ; and returns. ; No_Pulse mov w,#Glitch_Count ; Just a glitch mov Glitch,w mov Count_Lo,w ; Load count with 0x100 + mov Count_Hi,#1 ; #Glitch_Count nop retw 0 ; Total 14 clocks ; ; Process a "true" input. ; Inp_High test Glitch ; Check Glitch sz ; Is it already 0? dec Glitch ; No, then decrement it snb Count_Hi.7 ; Not a timeout already jmp Inp_Timeout ; Its a timeout test Count_Lo ; Test COUNT_LO for zero snz ; If its zero, this is it dec Count_Hi ; Subtract one from COUNT_HI dec Count_Lo ; Subtract one from the count retw 0 ; Total 14 Clocks ; ; During a timeout condition we basically hold the state variables ; where they are so that when the pin goes low it will be detected and ; the variables reset. ; Inp_Timeout nop nop nop retw 0 ; Total 14 Clocks ; ; Given a valid measurement, calculate a Command and a PWM Constant. ; ; Algorithim: ; Compute Delta = Count - midpoint; ; If (Delta < 0) ; Command = Forward; ; else ; Command = Reverse; ; Delta = ABS(Delta); ; if (Delta <= DEADBAND) ; if (Delta == DEADBAND) ; Command = Coast ; else ; Command = Brake; ; PWM Constant = 100%. ; Else ; PWM Constant = Min(MAX, delta - 8) * 100/MAX. ; (for MAX == 100, this is simply (Min(MAX, delta - 8)) ; Return. ; ; Got_Pulse sub Count_Lo,#128 ; This is the midpoint. jnc Deal_With_Minus Positive_Result mov Cmd_Reg,#Reverse_Cmd New_PWM_Value sub Count_Lo,#DeadBand jnc Do_Brake cjae Count_Lo,#100,Max_PWM mov w,Count_Lo Done_Pulse mov PWM_Cmd,w ; This is the new PWM Constant inc PWM_Cmd ; Adjust 0 - 99 -> 1 to 100% mov w,#1 ; Resest all of the counters mov Inner_Cnt,w mov PWM_Tmp,w mov Count_Hi,w mov w,#Glitch_Count mov Count_Lo,w mov Glitch,w mov Outer_Cnt, #41 ; Loop count + 1 retw 1 ; Return true ; ; Various exit times depending on the path. Since the spec says that ; another pulse won't start for at least 5mS the results are not ; critical but they are useful to know. They become the true spec for ; the minimum separation between pulses. ; ; Max time 51 * .2 = 10.20uS, Min time 36 * .2 = 7.2uS ; Max_PWM mov w,#100 jmp Done_Pulse ; Finish up ; ; If the result was minus, it could be legitimately minus, or it could ; be that count was 0x100. We fix those by checking COUNT_HI. ; Deal_With_Minus jb Count_Hi.0, Adjust_Result mov Cmd_Reg,#Forward_Cmd neg Count_Lo jmp New_PWM_Value Adjust_Result mov Count_Lo,#128 ; 256 - 128 == 128 always jmp Positive_Result ; ; Now if the value is -DEADBAND thru +DEADBAND we set the command to ; "BRAKE", if it was exactly -DEADBAND or +DEADBAND we set it to ; COAST. The reason for this is that given our sample rate/accuracy ; we find that we get 'jitter' in the input that results in a COUNT ; value that can move between two values on alternate samples. Given ; that variation, for input pulse widths that are 'near' DEADBAND-1 ; or -(DEADBAND-1) we could see alternating DEADBAND and DEADBAND-1 ; on the input. Without a one value buffer, this causes the output to ; alternate between a little on (1% PWM) and BRAKE. The motor then ; sits there and stutters. However by defining the value DEADBAND ; exactly to be '0' the output is either COAST/ON or COAST/BRAKE, ; both of which have a predictable and non-harmful output. ; Do_Brake add Count_Lo,#DeadBand cje Count_Lo,#DeadBand-1,On_The_Edge mov w,#Brake_Cmd On_The_Edge mov Cmd_Reg,w ; Store it mov Pwm_Cmd,#100 ; MAX value ... jmp Done_Pulse ; Return END