In this
article:
Source Code
License
Navigation:
HomeHardware
Software
Techniques
Controllers
Reviews
Index
Description
This is the PIC source code for a simple ESC based on the PIC16C54. Its pretty out of date since there are much better chips to base a controller on, I’ve used a PIC16F628 which was much nicer as it could do the pulse width measurement in hardware. However, a lot of people like this project and the source is a good example of weaving a bunch of time sensitive functions togther in a cycle counting sort of way.
The Source Code
; vim: set syntax=pic : TITLE "Electronic Speed Control SERVO SOFTWARE" LIST P=16C54A, R=HEX, C=120 include "p16C5X.inc" include "p16c5xd.inc" __FUSES _CP_OFF&_HS_OSC&_WDT_OFF ; ; 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. ; ; Data space ; CBLOCK H'0008' COUNT_HI ; High Byte of the Count COUNT_LO ; Low Byte of the Count TEMP ; Temporary variable PWM_CMD ; PWM Constant PWM_TMP ; Temporary counter for PWM count down CMD_REG ; Motor control command CMD_TMP ; Temporary copy of CMD_REGS OUTER_CNT ; Outer loop count INNER_CNT ; Inner Loop count GLITCH ; Our glitch count ENDC ; ; 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 H'89' ; Move forward (Left high + Right lower) REVERSE_CMD EQU H'46' ; Move Backward (Right High + Left Lower) COAST_CMD EQU H'00' ; Coast (all off) BRAKE_CMD EQU H'33' ; Brakes on (Left Lower + Right Lower). GLITCH_COUNT EQU D'172' ; 860uS (172 * 5 uS, 20 Mhz clock) DEADBAND EQU D'6' ; Deadband range + 1 ; The input pin #define SERVO_IN PORTB,0 ; The H-bridge ouput #define BRIDGE_OUT PORTA ; ; System initialization code ; ORG H'1FF' ; Reset Vector (PIC16C54) GOTO INIT ORG 0 INIT MOVLW 0 ; All bits as outputs. TRIS BRIDGE_OUT ; Output port is all outputs. CLRF BRIDGE_OUT ; And all outputs OFF. MOVLW GLITCH_COUNT ; Initialize vars MOVWF GLITCH ; MOVWF COUNT_LO ; Initialize count to 0x1A0 MOVLW 1 ; MOVWF COUNT_HI ; ; ; 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 16 MOVWF TEMP ; Store result in TMP 1 MOVF TEMP,F ; Waste time, set Z bit 1 BTFSS ZERO_BIT ; If zero no new value. 2 1 GOTO PWM ; If got a value, then start. - 2 GOTO $+1 ; Waste time 2 NOP ; ... 1 GOTO IDLE ; Go back to measuring 2 ; 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: MOVLW D'40' ; Constant for 20 mS MOVWF OUTER_CNT ; Outer Count MOVF PWM_CMD,W ; Set up the inner loop counters MOVWF PWM_TMP MOVF CMD_REG,W ; Get H-Bridge Command MOVWF CMD_TMP ; Put it in our temporary holder MOVWF BRIDGE_OUT ; Turn on the motors 1 MOVLW D'99' ; 1 MOVWF INNER_CNT ; 1 GOTO PWM_LOOP ; Line up command 2 (4) PWM_LOOP: CALL DO_MEASURE ; Do a "measurement" 16 ; ; 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 ; DECF PWM_TMP,F ; Decrement the "On" time 1\ BTFSC ZERO_BIT ; If not zero continue 2 1 SWAPF CMD_TMP,F ; Turn off the low side - 1 MOVF CMD_TMP,W ; Get the Command Register 1/ MOVWF BRIDGE_OUT ; Send it To the H-Bridge. 1 MOVF PWM_CMD,W ; Sort of a NOP 1 DECFSZ INNER_CNT,F ; Decrement inner loop count 1 2 GOTO PWM_LOOP ; Continue Looping 2 - ; 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. ; MOVWF PWM_TMP ; Restore PWM command (loaded above) 1 CALL DO_MEASURE ; This is on a 5uS boundary. 16 MOVLW D'99' ; Reset inner loop count 1 MOVWF INNER_CNT ; Like so 1 MOVF CMD_REG,W ; Put command into W 1 MOVWF CMD_TMP ; Re-initialized CMD_TMP 1 MOVWF BRIDGE_OUT ; Send it to the motors 1 DECF OUTER_CNT ; 1 BTFSS ZERO_BIT ; 1 2 GOTO PWM_LOOP ; 2 - ; ; 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. ; CLRF BRIDGE_OUT ; Turn off the motors 1 CALL DO_MEASURE ; 16 MOVWF TEMP ; Store result in TMP 1 MOVF TEMP,F ; Waste time, set Z bit 1 BTFSS ZERO_BIT ; If zero no new value. 2 1 GOTO PWM_LOOP ; If got a new value, then start. - 2 GOTO $+1 ; else Waste time 2 NOP ; ... 1 GOTO IDLE ; Go back to measuring 2 ; ; 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: BTFSC SERVO_IN ; Check for 'high' 2 1 GOTO INP_HIGH ; Yup, count it. - 2 (3) MOVF GLITCH,F ; Check Glitch counter 1 - BTFSS ZERO_BIT ; != 0 means Glitch. 2 - 1 GOTO NO_PULSE ; This was a glitch - - 2 (6) BTFSS COUNT_HI,7 ; Check for overflow 2 - - 1 GOTO GOT_PULSE ; Process a good count - - - 2 (8) MOVLW GLITCH_COUNT ; Glitch length 1 - - * MOVWF GLITCH ; Store it in GLITCH 1 - - MOVWF COUNT_LO ; Just a glitch, 1 - - MOVLW 1 ; Load count with constant 1 - - MOVWF COUNT_HI ; 1 - - RETLW 0 ; And return 2 - - ; -- ; Total 14 ; ; ; This is basically an abort, it resets the counters and state variables ; and returns. ; NO_PULSE: ; (6) MOVLW GLITCH_COUNT ; - - 1 MOVWF GLITCH ; - - 1 MOVWF COUNT_LO ; Just a glitch, - - 1 MOVLW 1 ; Load count with 0x1A0 - - 1 MOVWF COUNT_HI ; - - 1 NOP ; To get an even 14 clocks - - 1 RETLW 0 ; And return - - 2 ; -- ; Total 14 ; ; ; Process a "true" input. ; INP_HIGH: ; (3) MOVF GLITCH,F ; Check Glitch - 1 BTFSS ZERO_BIT ; Is it already 0? - 2 1 DECF GLITCH ; No, then decrement it - - 1 BTFSC COUNT_HI,7 ; Not a timeout already - 2 1 GOTO INP_TIMEOUT ; Its a timeout - - 2 (9) MOVF COUNT_LO,F ; Test COUNT_LO for zero - 1 BTFSC ZERO_BIT ; If its zero, this is it - 2 1 DECF COUNT_HI,F ; Subtract one from COUNT_HI - - 1 DECF COUNT_LO,F ; Subtract one from the count - 1 - RETLW 0 ; - 2 ; -- ; Total 14 ; ; ; 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: ; (9) NOP ; Filler to get 14 clocks - - - 1 NOP ; Filler to get 14 clocks - - - 1 NOP ; Filler to get 14 clocks - - - 1 RETLW 0 ; - - - 2 ; -- ; Total 14 ; ; ; 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: ; (8) MOVLW D'128' ; This is the midpoint. 1 SUBWF COUNT_LO,F ; 1 BTFSS CARRY_BIT ; Carry is set (no Borrow) 2 1 GOTO DEAL_WITH_MINUS ; Figure out what to do now. - 2 (13) POSITIVE_RESULT: ; (+7) MOVLW REVERSE_CMD ; Its probably reverse 1 MOVWF CMD_REG ; Store it in command register 1 NEW_PWM_VALUE: ; (21) MOVLW DEADBAND ; Dead band is +/- 6 (50uS) 1 SUBWF COUNT_LO,F ; Subtract DEADBAND from count. 1 BTFSS CARRY_BIT ; If no borrow, continue 2 1 GOTO DO_BRAKE ; In the dead zone do braking. - 2 (19) (26) MOVLW D'100' ; Check if its in the range 1 SUBWF COUNT_LO,W ; Subtract 99 (max range) 1 BTFSC CARRY_BIT ; Check if its Less than MAX 2 1 GOTO MAX_PWM ; Was >= 100 - 2 (23) (30) MOVF COUNT_LO,W ; Get count value. 1 DONE_PULSE: ; (26)(+12)(33) MOVWF PWM_CMD ; This is the new PWM Constant 1 1 INCF PWM_CMD ; Adjust 0 - 99 -> 1 to 100% 1 1 MOVLW 1 ; Put 1 into INNER_CNT 1 1 MOVWF INNER_CNT ; 1 1 MOVWF PWM_TMP ; 1 1 MOVWF COUNT_HI ; Reset counter 1 1 MOVLW GLITCH_COUNT ; for next time 1 1 MOVWF COUNT_LO ; 1 1 MOVWF GLITCH ; 1 1 MOVLW D'41' ; Loop count + 1 1 1 MOVWF OUTER_CNT ; 1 1 RETLW 1 ; Return true 2 2 ; Totals 36 39 ; Totals (max) 46 ; Totals (reverse) 43 46 ; Totals (do_brake) 44 51 ; ; 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: MOVLW D'100' ; Else max (does MIN(100, val)) 1 GOTO DONE_PULSE ; Fix up loop values 2 ; ; 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: BTFSC COUNT_HI,0 ; Is bit 0 a one? 2 1 GOTO ADJUST_RESULT ; Yes, so adjust - 2 MOVLW FORWARD_CMD ; Its wider than the midpoint 1 MOVWF CMD_REG ; So this is the command. 1 COMF COUNT_LO,F ; Convert to a positive value 1 INCF COUNT_LO,F ; negate COUNT_LO 1 GOTO NEW_PWM_VALUE ; Now go compute PWM constant. 2 ADJUST_RESULT: MOVLW D'128' ; 256 - 128 = 128 1 MOVWF COUNT_LO ; Fixup the low byte 1 GOTO POSITIVE_RESULT ; Now treat it normally. 2 ; ; 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: MOVLW DEADBAND ; Restore Count Value 1 ADDWF COUNT_LO,F ; Add it back to Count 1 MOVLW DEADBAND-1 ; Check for boundary condition 1 SUBWF COUNT_LO,W ; Subtrack COUNT LO 1 BTFSC ZERO_BIT ; Check to see if its zero 2 1 GOTO ON_THE_EDGE ; Its exactly DEADBAND - 1 - 2 MOVLW BRAKE_CMD ; This is the brake Command 1 - ON_THE_EDGE: MOVWF CMD_REG ; Store it 1 MOVLW D'100' ; Very large PWM Constant 1 MOVWF PWM_CMD ; So we get 100% braking action 1 GOTO DONE_PULSE ; Return indicating new value available.2 (12) END
License
This work is licensed under a Creative Commons Attribution-NonCommercial 3.0 Unported License. You are free to play around with it and modify it but you are not licensed to use it for commercial purposes. Click the link above for more details on your rights under this license.