;********************************************************************** ; This file is a for the PhilBot RC Servo Speed controler * ; It uses the PICmicro PIC12F683. * ; The Servo Control Pulse is monitored and converted into two * ; directional PWM signals for motor control. * ; * ; Refer to the MPASM User's Guide for additional information on * ; features of the assembler (Document DS33014). * ; * ; Refer to the respective PICmicro data sheet for additional * ; information on the instruction set. * ; * ;********************************************************************** ; * ; Filename: Servo.asm * ; Date: 4/1/2005 * ; File Version: * ; * ; Author: Phil Malone * ; webmaster@philbot.com * ; Details: www.philbot.com/hardware.htm * ; * ;********************************************************************** ; * ; Files required: * ; * ;********************************************************************** ; * ; Notes: * ; Servo Control Pulse is on I/O 3 (Pin 4) * ; Motor Drive FWD is on I/Os 0 & 1 (Pins 6&7) * ; Motor Drive Rev is on I/Os 4 & 5 (Pins 3&2) * ; * ;********************************************************************** list p=12F683 ; list directive to define processor #include ; processor specific variable definitions errorlevel -302 ; suppress message 302 from list file __CONFIG _FCMEN_ON & _IESO_OFF & _CP_OFF & _CPD_OFF & _BOD_OFF & _MCLRE_OFF & _WDT_OFF & _PWRTE_ON & _INTRC_OSC_NOCLKOUT ; '__CONFIG' directive is used to embed configuration word within .asm file. ; The lables following the directive are located in the respective .inc file. ; See data sheet for additional information on configuration word settings. ;***** GPIO Allocations IO_SERVO EQU 3 ; GPIO Bit 3, Servo Control Pulse Input. IO_FWD_ON EQU 0x03 ; GPIO Bits 0,1 Forward Drive Out IO_FWD_OFF EQU 0xFC IO_REV_ON EQU 0x30 ; GPIO Bits 4,5 Reverse Drive Out IO_REV_OFF EQU 0xCF ;***** VARIABLE DEFINITIONS Temp EQU 0x20 ; Flags EQU 0x21 ; variable used for context saving ICntLSB EQU 0x22 ; Timer for measuring Servo High/Low Period ICntMSB EQU 0x23 ; Timer for measuring Servo High/Low Period Length EQU 0x24 ; Pulse Length (Ticks) PulseTimer EQU 0x25 ; Pulse countdown timer CycleTimer EQU 0x26 ; Full Cycle Timer ; ***** FLAG BIT DEFINITIONS F_Forward EQU 0 ; Set = Forward. F_Reverse EQU 1 ; Set = Forward. ;**** TIMER Constants TICK EQU 0x64 ; 50uS (8MHz Clock, 20KHz Sample rate) REV_THRESH EQU 0x1D ; 50uS (8MHz Clock, 20KHz Sample rate) FWD_THRESH EQU 0x1E ; 50uS (8MHz Clock, 20KHz Sample rate) MAX_RANGE EQU 0x0A ; 10 steps per axis (plus 1) MAX_RANGEP1 EQU 0x0B ; Max Range Plus 1 ;********************************************************************** ORG 0x000 ; processor reset vector goto main ; go to beginning of program nop nop nop ;********************************************************************** ORG 0x004 ; interrupt vector location retfie ; return from interrupt ;********************************************************************** ORG 0x005 ; remaining code goes here ;********************************************************************** main bcf STATUS,RP0 ; Set file register bank to 0 clrf GPIO ; Init GPIO movlw 0x07 ; Set all Comparitors off (GPIO On) movwf CMCON0 bsf STATUS,RP0 ; Set file register bank to 1 clrf ANSEL ; Disable Analog Chanels movlw 0x70 ; Set Clock Frequency to 8MHz movwf OSCCON clrf TRISIO ; Set GPIO Directions bsf TRISIO,IO_SERVO ; Setup servo as input movlw TICK ; Load Timer 2 up with period movwf PR2 ; bsf PIE1,TMR2IE ; Turn On Timer 2 Interrupt bcf STATUS,RP0 ; Set file register bank to 0 clrf Flags ; Clear out all variables clrf ICntLSB ; clrf ICntMSB ; clrf Length ; clrf PulseTimer ; clrf CycleTimer ; bsf T2CON,TMR2ON ; Turn On Timer 2 Start ; Wait for Input to go low before continuing btfsc GPIO,IO_SERVO ; Test Servo Input goto Start ; Loop if not clear call ResetTimer2 ; Clear out all timer variables WaitingForHigh call CheckForTic ; Accumulate Ticks btfss GPIO,IO_SERVO ; Test Servo Input goto WaitingForHigh ; Loop if not Set PositiveEdge call ResetTimer2 ; Resynch timer2 & related variables WaitingForLow call CheckForTic ; Get in synch with Timer 2 btfsc GPIO,IO_SERVO ; Test Servo Input goto WaitingForLow ; Loop if not Clear NegativeEdge GetDirection ; First check to see if we are in reverse movlw REV_THRESH ; Set the Reverse threshold subwf ICntLSB,F ; Subtract Thresh from Pulse time btfss ICntLSB,7 ; Is the result negative? goto NotRev ; No, Try the next threshold InReverse ; Calculate 1 to X timer count. Current value = -1 to -X comf ICntLSB,F ; Negate Count (Complement and add 1) incf ICntLSB,F ; Val is now 1 to REV_THRESH . ; Signal drive Reverse call SetPulseLength ; Convert count to pulse length bcf Flags,F_Forward bsf Flags,F_Reverse goto DirDone NotRev ; Now check to see if we are in Neutral decf ICntLSB,F ; Decrement Pulse time for Neutral gap btfss ICntLSB,7 ; Is the result negative? goto InForward ; No, Must be forward InNeutral ; Signal Neutral bcf Flags,F_Reverse bcf Flags,F_Forward clrf Length goto DirDone InForward ; Signal drive Forward incf ICntLSB,F ; Start range at 1 not o call SetPulseLength ; Convert count to pulse length bcf Flags,F_Reverse bsf Flags,F_Forward goto DirDone DirDone call ClearTimer2 ; Clear out all timer variables PulseComplete goto WaitingForHigh ; Repeat loop looking for next pulse ; --------------------------------------------------------- SetPulseLength ; Assumes iCntLSB has a tick count of 1 to some number ; Clip the number to 10 and then set the Pulse length movf ICntLSB,W ; Make temp. copy of Count movwf Temp ; movlw MAX_RANGE ; Compare with MAX_RANGE subwf Temp,F ; btfss Temp,7 ; If Negative, do nothing movwf ICntLSB ; otherwise, Set count to Max Range ; Calculate pulse lengh by multiplying count by 25 (16+8+1) movf ICntLSB,W ; Make temp copy of Count movwf Temp ; W starts out with Count rlf Temp,F ; Multiply Count by 8 (shift 3 bits) rlf Temp,F rlf Temp,F addwf Temp,F ; Accumulate in W rlf Temp,F ; Multiply Count by 16 (one more shift) addwf Temp,W ; Accumulate in W movwf Length ; Save in pulse length return ; --------------------------------------------------------- ResetTimer2 clrf TMR2 ; Reset Timer 2 bcf PIR1,TMR2IF ; Resetr Timer Interrupt ClearTimer2 clrf ICntLSB ; Clear counters clrf ICntMSB ; return ; --------------------------------------------------------- CheckForTic btfss PIR1,TMR2IF ; Check Timer2 interrupt bit goto CFT_Done ; Exit if not set IsTick bcf PIR1,TMR2IF ; Clear Timer Interrupt incfsz ICntLSB,F ; Inc 2 byte Input Counter goto PulseCheck incf ICntMSB,F btfss ICntMSB,3 ; Have we reached 2048? (100ms) goto PulseCheck ; Nope, OK, run pulse code ; Servo Timeout bcf Flags,F_Reverse ; Set output to Neutral bcf Flags,F_Forward clrf Length PulseCheck ;Look for end of cycle decfsz CycleTimer,F ; Has Cycle timer wrapped goto PulseEndCheck ; Start of New Cycle, Start next pulse NewCycle movf Length,W ; Put pulse length in PulseTimer movwf PulseTimer CheckForward btfss Flags,F_Forward ; Going Forward? goto CheckReverse SetForward movlw IO_REV_OFF ; Set Reverse Bits off andwf GPIO,F ; movlw IO_FWD_ON ; Set Forward Bits on iorwf GPIO,F ; goto CFT_Done CheckReverse btfss Flags,F_Reverse ; Skip if going Forward goto SetNeutral SetReverse movlw IO_FWD_OFF ; Set Forward Bits off andwf GPIO,F ; movlw IO_REV_ON ; Set Reverse Bits on iorwf GPIO,F ; goto CFT_Done SetNeutral movlw IO_FWD_OFF ; Set Forward Bits off andwf GPIO,F ; movlw IO_REV_OFF ; Set Reverse Bits off andwf GPIO,F ; goto CFT_Done PulseEndCheck ; Look to see if it's time to end the pulse movf PulseTimer,F ; Get pulse time and check for zero btfsc STATUS,Z ; Is pulse still running? goto CFT_Done ; No, goto end decfsz PulseTimer,F ; Count down Pulse goto CFT_Done ; Pulse has ended so clear all drive bits movlw IO_FWD_OFF ; Set Forward Bits off andwf GPIO,F ; movlw IO_REV_OFF ; Set Reverse Bits off andwf GPIO,F ; CFT_Done return ; initialize eeprom locations ORG 0x2100 DE 0x00, 0x01, 0x02, 0x03 END ; directive 'end of program'