;******************************************************************************* ;** ;** Basic SERVO controler ;** ;** Written by Rick Huang, Copyright (C) 2007 ;** Revision YYYY/MM/DD ;** Rev 1. 2007/11/23 - Initial release ;** ;** Command Functions ;** '1' Not used (Was PWM testing) ;** '2' Set servo 1 position ;** '3' Set servo 2 position ;** '4' Hex test - echo any hex input ;** '6' Robot control setup, set both servo at opposite direction ;** This function is designed for robot movement control, used ;** for continuous rotating servo used as wheel drive directly ;** '7' ;** ;** ;** This program is free software; you can redistribute it and/or modify ;** it under the terms of the GNU General Public License as published by ;** the Free Software Foundation; either version 2 of the License, or ;** (at your option) any later version. ;** ;** This program is distributed in the hope that it will be useful, ;** but WITHOUT ANY WARRANTY; without even the implied warranty of ;** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ;** GNU General Public License for more details. ;** ;** You should have received a copy of the GNU General Public License ;** along with this program; if not, write to the Free Software ;** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA ;******************************************************************************* ;**************************************************************************** ;** NOTE: Disable the WDT when debugging the code, however, make sure ;** the WDT is enabled when putting the system into lift time test ;** The timeout for WDT is about 2 seconds with 128:1 prescaler ;**************************************************************************** include __config _XT_OSC & _WDT_OFF & _LVP_OFF & _BODEN_OFF & _MCLRE_ON & _PWRTE_ON ;***************************************************************** ;** Variables TempW EQU 70h ; Used to save context in ISR, must be in shared TempSR EQU 71h ; Used to save context in ISR CounterB EQU 72h NextFSR EQU 73h ; CounterW EQU 74h BitCount EQU 75h LastBitVal EQU 76h Zero EQU 7bh ; Zero is 0x00 SerData EQU 7ch Temp EQU 7dh Output EQU 7eh Counter EQU 7fh TMR_h EQU 60h ; Current TMR1 read value TMR_l EQU 61h BinCnt EQU 62h Data0 EQU 63h Data1 EQU 64h Data2 EQU 65h Servo1a EQU 66h ; Servo 1 position set Servo1b EQU 67h ; LSB Servo2a EQU 68h ; Servo 2 position set Servo2b EQU 69h ; LSB InputA EQU 6ah InputB EQU 6bh #define _POAS PORTB, 7 ; Power ON Auto Start #define _DOUT PORTB, 6 ; Debug output #define _SERV1 PORTB, 5 ; Servo 1 output #define _SERV2 PORTB, 4 ; Servo 2 output #define _PWM_O PORTB, 3 ; PWM #define _TXD PORTB, 2 ; UART #define _RXD PORTB, 1 ; UART #define _DET PORTB, 0 ; Fan speed detection, interrupt trigger #define DecisionVal d'170' ;Hard decision, 0 = 159 cycle, 1 = 198 cycle ;***************************************************************** ;** Program code here goto Start ORG 4 ;waiting for <0004> ;interrupt vector goto Interrupt ORG 5 ;***************************** DATA StringStart: PWelcomeMsg: addwf PCL, F DT "Welcome!" DT H'0d', H'0a', H'00' PCommandMsg: DT "Command Error?" DT H'0d', H'0a', H'00' PParameterMsg: DT "Parameter Error?" DT H'0d', H'0a', H'00' PNoAckMsg: DT "Missing Ack" DT H'0d', H'0a', H'00' StringEnd: WelcomeMsg EQU 0 CommandMsg EQU PCommandMsg - 1 - 5 ; 5 is the offset where the ParameterMsg EQU PParameterMsg - 1 - 5 ; Strings start NoAckMsg EQU PNoAckMsg - 1 - 5 HexTable: addwf PCL, F DT "0123456789ABCDEF" ;***************************************************************** ;******************** ISR starts here Interrupt: movwf TempW ; Save context swapf STATUS, W bcf STATUS, RP0 movwf TempSR ;********************** Real interrupt handeler here bsf _DOUT ; Use Timer 1 for speed sensing movfw TMR1H ; Read from TMR1 movwf TMR_h movfw TMR1L movwf TMR_l ;********************** Create pulse for servo1 movfw Servo1a movwf Data1 movfw Servo1b movwf Data0 bsf _SERV1 call waitcust bcf _SERV1 ;********************** Create pulse for servo2 movfw Servo2a movwf Data1 movfw Servo2b movwf Data0 bsf _SERV2 call waitcust bcf _SERV2 bcf INTCON, T0IF bcf _DOUT ;********************** Restore context swapf TempSR, W movwf STATUS swapf TempW, F swapf TempW, W bcf INTCON, INTF retfie ;***************************************************************** ;******************** Program Starts here Start: ;********************** Setup default values bcf _SERV1 bcf _SERV2 movlw 01h movwf Servo1a movwf Servo2a movlw 0a0h movwf Servo1b movwf Servo2b ;********************** Setting up TMR0, used to decode FSK clrwdt bsf STATUS, RP0 movlw b'00000101' ;Prescaler to option, 1:64, 16.4mS per ISR @ 4MHz movwf OPTION_REG clrwdt ;********************* Setting up environment movlw b'10000011' ;Serial port in too 0 = output movwf TRISB ;Set port B I/O movlw b'00000111' movwf TRISA ;Set port A I/O bcf STATUS, RP0 clrf PORTA ;********************** Disable 16f628 specific features movlw b'00000111' ;Setup comparator, not used for all movwf CMCON ;********************** Setting up PWM module movlw b'00001100' ;Enable PWM module, not using the last 2 bits movwf CCP1CON ;Using source frequency = 4MHz movlw b'00000010' ;CCPRL1 contain the duty cycle, 8 bits movwf CCPR1L movlw b'00000101' ;Setting up TMR2, Prescale = 1:4 movwf T2CON bsf STATUS, RP0 movlw b'11111111' movwf PR2 ;Setting the period = 255, maximum resolution bcf STATUS, RP0 ;~1KHz PWM frequency ;********************** Setup UART bsf STATUS, RP0 movlw d'12' ;19200 for 4MHz Clock, High speed movwf SPBRG ;The only rate with low mismatch movlw b'10100111' ;Async 8-bit @ High speed movwf TXSTA bcf STATUS, RP0 movlw b'10110000' ;Enable serial port movwf RCSTA clrf Zero ;********************** Enable interrupt on TMR0 movlw b'00100000' ;NOTE GIE is not enabled at this point movwf INTCON movlw b'00000001' ;Enable Timer 1 - Not used really movwf T1CON bsf INTCON, GIE ;Enable the interrupt at this point ;********************** End of init movlw WelcomeMsg ;Display welcome string on startup call SendString goto SkipDebug ;----------------------- Debug insert ;Prototype InputA = 0xa0 - (InputB - 0xa0) movlw 0xa5 movwf InputB movlw 0xa0 subwf InputB, W sublw 0xa0 movwf InputA movlw 0x90 movwf InputB movlw 0xa0 subwf InputB, W sublw 0xa0 movwf InputA ;----------------------- End Debug insert SkipDebug: ;----------------------- Wait for command WaitMore: ; btfsc _POAS ; goto AutoStart ;Auto start not in use clrwdt btfsc RCSTA, OERR goto ClearError btfss PIR1, RCIF goto WaitMore movfw RCREG movwf SerData movwf TXREG ;Send it back out call SSpace ;--------------------------------------------- ;-- Decode command movlw '1' xorwf SerData, W btfsc STATUS, Z goto PWMtest movlw '2' xorwf SerData, W btfsc STATUS, Z goto Serv1Set movlw '3' xorwf SerData, W btfsc STATUS, Z goto Serv2Set movlw '4' xorwf SerData, W btfsc STATUS, Z goto HexTest ;Testing HEX routine movlw '5' xorwf SerData, W btfsc STATUS, Z goto BinTest movlw '6' xorwf SerData, W btfsc STATUS, Z goto ServBSet movlw '7' xorwf SerData, W btfsc STATUS, Z goto DummyNow movlw '8' xorwf SerData, W btfsc STATUS, Z goto DummyNow DummyNow: ;For all command not in use movlw CommandMsg call SendString ;Send error command message goto WaitMore ;********************* Clear receving overrun error ClearError: movfw RCREG movfw RCREG bcf RCSTA, CREN bsf RCSTA, CREN goto WaitMore ;---------------------- Commands execution ;*********************** Command 1 PWM test PWMtest: call RcvHex movfw Output movwf CCPR1L ; Set PWM duty cycle call NewLine goto WaitMore ;*********************** Command 2 Set servo speed 1 Serv1Set: call RcvHex movfw Output andlw 03h ; Set an upper end movwf InputA call RcvHex movfw Output movwf InputB call NewLine movfw InputB ; Write the new value at once movwf Servo1b movfw InputA movwf Servo1a goto WaitMore ;*********************** Command 3 Set servo speed 2 Serv2Set: call RcvHex movfw Output andlw 03h movwf InputA call RcvHex movfw Output movwf InputB call NewLine movfw InputB ; Write the new value at once movwf Servo2b movfw InputA movwf Servo2a goto WaitMore ;*********************** Command 6 Set servo speed both, opposit direction... ServBSet: call RcvHex movfw Output movwf InputB ; Input A = 0xa0 - (InputB - 0xa0) movlw 0a0h subwf InputB, W sublw 0a0h movwf InputA call NewLine movfw InputB ; Write the new value at once movwf Servo2b movfw InputA movwf Servo1b movlw 01h movwf Servo2a movwf Servo1a goto WaitMore ;*********************** Command 4,5 Serial debug HexTest: call RcvHex ;Command 4: Hex test movfw Output call SendHex call NewLine goto WaitMore BinTest: call RcvHex ;Command 5: Bin test movfw Output call SendBin call NewLine goto WaitMore ;***************************** SUBROUTINE ;----------------------------------------------------------------- ; xxx ; Input: ;***************************** SUBROUTINE - Common shared lib ;----------------------------------------------------------------- ; SendHex, Send a single byte out of serial port, hex format ; Input: data stored in W SendHex: movwf Temp swapf Temp, W andlw 00fh call HexTable HexWaitLoop1: btfss PIR1, TXIF ;Wait till buffer is empty goto HexWaitLoop1 movwf TXREG movf Temp, W andlw 00fh call HexTable HexWaitLoop2: btfss PIR1, TXIF ;Wait till buffer is empty goto HexWaitLoop2 movwf TXREG return ;----------------------------------------------------------------- ; SendBin, Send a single byte out of serial port, binary format ; Input: data stored in W SendBin: movwf Temp movlw 8h movwf BinCnt BinWaitLoop1: btfss PIR1, TXIF ;Wait till buffer is empty goto BinWaitLoop1 movlw '0' btfsc Temp, 7 movlw '1' movwf TXREG rlf Temp, F decfsz BinCnt, F goto BinWaitLoop1 return ;----------------------------------------------------------------- ; RcvHex, Receive a single byte from serial port in hex format ; Output: data stored in Output RcvHex: btfss PIR1, RCIF goto RcvHex movfw RCREG movwf SerData movwf TXREG ; Send it back out movlw 30h subwf SerData, F movlw 10h subwf SerData, W ; F - W btfsc STATUS, C ; Positive means letter input goto Letter1 RtnLetter1: swapf SerData, W movwf Output RcvHexLoop: btfss PIR1, RCIF goto RcvHexLoop movfw RCREG movwf SerData movwf TXREG ; Send it back out movlw 30h subwf SerData, F movlw 10h subwf SerData, W ; F - W btfsc STATUS, C ; Positive means letter input goto Letter2 RtnLetter2: movfw SerData iorwf Output, F call SSpace return Letter1: movlw 07h subwf SerData, F movlw 10h subwf SerData, W btfsc STATUS, C ; Positive means bad input goto Letter1E goto RtnLetter1 Letter1E: movlw d'32' subwf SerData, F goto RtnLetter1 Letter2: movlw 07h subwf SerData, F movlw 10h subwf SerData, W btfsc STATUS, C ; Positive means bad input goto Letter2E goto RtnLetter2 Letter2E: movlw d'32' subwf SerData, F goto RtnLetter2 ;----------------------------------------------------------------- ; LongDelay, 15 cycles, 3ins/cycle 22.5uS for 8Mhz clock long_delay: movlw d'15' movwf Counter long_delay_L: decfsz Counter, F goto long_delay_L return ;----------------------------------------------------------------- ; SendString, send the string out to serial port ; Input: String location in W SendString: movwf Counter SendLoop: btfss PIR1, TXIF ;Wait till buffer is empty goto SendLoop call StringStart iorwf Zero, W btfsc STATUS, Z ;Zero is the end of string return movwf TXREG incf Counter, F movfw Counter goto SendLoop ;----------------------------------------------------------------- ; NewLine, send s newline character into serial port ; Input: None NewLine: btfss PIR1, TXIF ;Wait till buffer is empty goto NewLine movlw 0dh movwf TXREG movlw 0ah NewLineLoop: btfss PIR1, TXIF ;Wait till buffer is empty goto NewLineLoop movwf TXREG return ;----------------------------------------------------------------- ; Space, send a space character into serial port ; Input: None SSpace: btfss PIR1, TXIF ;Wait till buffer is empty goto SSpace movlw ' ' movwf TXREG return ;----------------------------------------------------------------- ; SendChar, send a character into serial port ; Input: W SendChar: btfss PIR1, TXIF ;Wait till buffer is empty goto SendChar movwf TXREG return ;----------------------------------------------------------------- ; w10sec: Wait for 5 seconds ; Using: Data0, 1, 2, 3 w10sec: movlw d'75' movwf Data0 clrf Data1 clrf Data2 w10secloop: decfsz Data2, F goto w10secloop clrwdt decfsz Data1, F goto w10secloop decfsz Data0, F goto w10secloop return ;----------------------------------------------------------------- ; waitcust: User decided wait time ; Input: Data0, 1 waitcust: incf Data1, F incf Data0, F wcustloop: decfsz Data0, F goto wcustloop decf Data0, F ; Compensate delay decf Data0, F nop decfsz Data1, F goto wcustloop return ;----------------------------------------------------------------- ; End of program end