; TITLE 'Serial Servo Controller-Accuracy change 7/29/99' ;Copyright Dennis Clark and TTT Enterprises. This code is free to use and ;distribute according to the Free Software Copyleft rules and conventions. ;You may not publish or sell it without written permission from me, the ;author. ; ; I included the "include" file for the 508 for reference and changed it a bit ; LIST P = 12C508, F = INHX8M ; P12C508.INC Standard Header File, Version 1.02 Microchip Technology, Inc. ;========================================================================== ; Verify Processor ;========================================================================== IFNDEF __12C508 MESSG "Processor-header file mismatch. Verify selected processor." ENDIF ;========================================================================== ; Register Definitions ;========================================================================== W EQU H'0000' F EQU H'0001' ;----- Register Files ----------------------------------------------------- INDF EQU H'0000' ; Uses FSR to address data mem. TMR0 EQU H'0001' ; 8 bit real time clock/counter PCL EQU H'0002' ; Low order 8 bits of PC STATUS EQU H'0003' ; STATUS FSR EQU H'0004' ; Indirect data memory addr pointer OSCCAL EQU H'0005' ; Calibration data for osc. GPIO EQU H'0006' ; General Purpose I/O ;----- STATUS Bits -----------Page 14-------------------------------------- GPWUF EQU H'0007' ; GPIO reset bit PA0 EQU H'0005' ; Program Page preselect NOT_TO EQU H'0004' ; Time Out bit NOT_PD EQU H'0003' ; Power Down bit Z EQU H'0002' ; Zero bit DC EQU H'0001' ; Digit carry/*borrow bit C EQU H'0000' ; carry/*borrow bit ;----- OPTION Bits -----------Page 15-------------------------------------- NOT_GPWU EQU H'0007' ; Enable wake-up on pin change NOT_GPPU EQU H'0006' ; Enable weak pull-ups T0CS EQU H'0005' ; Timer0 clock source select T0SE EQU H'0004' ; Timer0 sources edge select PSA EQU H'0003' ; Prescalar assignment bit PS2 EQU H'0002' ;\ PS1 EQU H'0001' ; > Prescalar rate select bits PS0 EQU H'0000' ;/ ;========================================================================== ; RAM Definition ;========================================================================== __MAXRAM H'3F' ;========================================================================== ; Configuration Bits ;========================================================================== _MCLRE_ON EQU H'0FFF' _MCLRE_OFF EQU H'0FEF' _CP_ON EQU H'0FF7' _CP_OFF EQU H'0FFF' _WDT_ON EQU H'0FFF' _WDT_OFF EQU H'0FFB' _LP_OSC EQU H'0FFC' _XT_OSC EQU H'0FFD' _IntRC_OSC EQU H'0FFE' _ExtRC_OSC EQU H'0FFF' __CONFIG ( _MCLRE_OFF & _CP_OFF & _WDT_OFF & _IntRC_OSC ) ;========================================================================== ; Program Variables ;========================================================================== ;Port definitions and other constants or macros #define STEST GPIO,5 ; Pin 2 = GP5 = bit 5 O(LED) #define SERVO3 shadow,4 ; Pin 3 = GP4 = Bit 4 O #define DATAIN GPIO,3 ; Pin 4 = GP3 = Bit 3 I SERIAL #define SERVO2 shadow,2 ; Pin 5 = GP2 = Bit 2 O #define SERVO1 shadow,1 ; Pin 6 = GP1 = Bit 1 O #define SERVO0 shadow,0 ; Pin 7 = GP0 = Bit 0 O #define SCONFIG B'11101000' ; Only really using first 4 servos #define DELAY1 D'15' ; 2400 BAUD delay amount (~425us) #define DELAY2 D'20' ; 1.3 bit times (start bit) #define SSTART D'16' ; Start of look up table #define TSHORT D'128' ; Add this for short throw LIST ;Register locations for important stuff,only 9-15 are available before table rcount EQU D'8' ; bit count rcvreg EQU D'9' ; incoming byte storage delay EQU D'10' ; delay storage register scratch EQU D'11' ; math scratch pad register ntmr EQU D'12' ; most recent TMR0 reading otmr EQU D'13' ; last stored TMR0 reading rocount EQU D'14' ; rollover count (10 max) shadow EQU D'15' ; I/O shadow register Serv0 EQU D'16' Serv1 EQU D'17' Serv2 EQU D'18' Serv3 EQU D'19' short EQU D'21' ; flag register for short throw sfsr EQU D'22' ; shadow FSR register sdata EQU D'23' ; shadow data register smask EQU D'24' ; servo enable mask ;================================ ; Main Code = ;================================ ORG 0x1FF ; MOVLW 0x50 ; My EPROM calibration value,remove for OTPs start ORG 0x000 MOVWF OSCCAL ; Store the factory osc. calibration value MOVLW SCONFIG ; Set GPIO 3 and 5 as input, rest outputs TRIS GPIO ; Configure pins MOVLW B'01000010' ; Set OPTION bits Prescaler divide by 8 OPTION ; Implement OPTION bits MOVLW SSTART ; preset servos to center position MOVWF FSR MOVLW D'192' ; This is ~1.5ms (1/2 90 deg servo swing) MOVWF INDF ; servo 0 INCF FSR,F MOVWF INDF ; servo 1 INCF FSR,F MOVWF INDF ; servo 2 INCF FSR,F MOVWF INDF ; servo 3 INCF FSR,F MOVWF INDF ; Servos 4 now are setup MOVLW 0x3F ; set all servos to '1' MOVWF shadow ; save in the shadow register MOVWF GPIO ; set the bits MOVLW 0x3F ; All servos masked on MOVWF smask CLRF otmr ; initial setup, whatever... MOVLW 0x0A ; rollover count MOVWF rocount ; reset rollover count too CLRF TMR0 CLRF short ; set pulse to long BTFSC STEST ; check jumper setting '1'=short BSF short,0 ; Now set it to short throw main ;=========================================================================== ; This program will take a single byte of serial data at 2400 baud and ;control the chosen servo with one of 64 possible settings X 4, max 252. ;The two MS bits choose the servo, 0-3 and the LS 6 bits the 64 positions. ;Hopefully in real-time. ; If GP5 is pulled high on powerup, the pulse width will be 1ms to 2ms ;with 1.5ms being the center position, or 90 degree movement, 1.4 degrees ;per position value. If GP5 is pulled low on powerup then servos will ;be given positions from approx 0ms to 2ms, or 180 degrees of motion and ;2.8 degrees per step. ; If there is a match or near miss in the compare, then set that bit (servo) ;to '0'. If a new timer value is less than the last saved timer value then ;it means that we rolled over the counter. After 20ms all servos are set back ;to '1'. Any RS232 value is gotten in between about a dozen timer checks ;for all of the servos. There will be a little jitter, but not enough to ;matter, and this only during RS232 communication times. ; A 0 location on servo 0 will disable servos 0 and 1. It will not set the ;position of this servo, this will remain at its last setting, so will the ;setting for servo 1. When a 0 position is used for positions 1,2 or3 then ;servos 0 and 1 will be re-enabled. You can change the starting position ;of servos 0 and 1 while they are disabled and they will start up at the ;new positions as soon as they are re-enabled. ;Copyright Dennis Clark and TTT Enterprises Revision B.1 7/29/99 ;=========================================================================== mloop BTFSS DATAIN ; Look for a start bit CALL getbyte ; Start bit detected, get the input byte now MOVLW 0x01 MOVWF delay CALL bloop ; Nope, just check out the servos then GOTO mloop ; and start the loop over again ;================================ ; ; Subroutines Follow ; ;================================ getbyte ;--------------------------------- ; This is the routine that will ;get the data byte from the serial ;line. We do LOTS of in-between ;servo value checking while doing ;math here, to stop jitter. ;--------------------------------- MOVLW 6 ; only getting first 6 bits of positional data here MOVWF rcount ; save bit count here CLRF sdata ; clear out the receive register CLRF sfsr ; clear out the servo register MOVLW DELAY2 ; 1.3 bit times to move well into the next bit MOVWF delay ; delay counter r_next CALL bdelay ; do the delay, which checks all servo timers too BCF STATUS,C ; default to zero for incoming bit RRF sdata,F ; and rotate it into the receive buffer BTFSC DATAIN ; check the incoming bit now BSF sdata,7 ; nope, its a one really MOVLW DELAY1 ; normal delay time MOVWF delay DECFSZ rcount,F ; and loop to get the rest GOTO r_next BCF STATUS,C ; leave data rotated one place (mult by 2) RRF sdata,F ; rotate to proper locations MOVLW DELAY1 ; 1 bit time to first servo bit MOVWF delay CALL bdelay ; do the delay, which checks all servo timers too BTFSC DATAIN ; check the incoming bit now BSF sfsr,0 ; nope, its a one really MOVLW DELAY1 ; 1 bit time to second servo bit MOVWF delay CALL bdelay ; do the delay, which checks all servo timers too BTFSC DATAIN ; check the incoming bit now BSF sfsr,1 ; nope, its a one really MOVLW DELAY1 ; delay time for stop bit MOVWF delay CALL bdelay BTFSS DATAIN ; check stop bit, must = 1 or its a nogo GOTO freturn ; its 0, framing error, don't use this cfor0 MOVF sdata,F ; check for zero value, here no settings change BTFSS STATUS,Z GOTO gotall ; Naw, just leave MOVF sfsr,F ; check for zero on zero servo BTFSS STATUS,Z GOTO cfor1 ; There we look for setting others to zero MOVLW 0x3C ; masking off servo 1 and 0 MOVWF smask GOTO freturn ; all done, servo 0 and 1 are now disabled cfor1 MOVLW 0x3F ; clear all mask bits MOVWF smask GOTO freturn ; do NOT update servo locations! gotall ; now have the data, we need to decode and use it BTFSC short,0 ; using short throw servos GOTO cshort RLF sdata,F ; mult by 2 again GOTO returnb cshort MOVLW TSHORT ; short offset ADDWF sdata,F ; add to what's already there returnb MOVLW SSTART ; FSR location MOVWF FSR MOVF sfsr,W ; get new FSR ADDWF FSR,F MOVF sdata,W ; get new data MOVWF INDF ; save new servo value freturn RETLW 0 ; We are done with data and check, return bdelay ;--------------------------------- ; This will do the usual 417us ; delay, but it will do useful ; stuff during the delay, it will ; check each servo for proper '0' ; settings. It will also check ; for rollover to reset all servos ;--------------------------------- bloop ; Just service the servos here then MOVF Serv0,W SUBWF TMR0,W ; ntmr - W -> W BTFSC STATUS,C ; C = 1 then W is less than TMR0 BCF SERVO0 ; bit now = 0 MOVF Serv1,W ; Get servo to scan SUBWF TMR0,W ; TMR0 - W -> W BTFSC STATUS,C ; C = 1 then W is less than TMR0 BCF SERVO1 ; bit now = 0 MOVF Serv2,W ; Get servo to scan SUBWF TMR0,W ; TMR0 - W -> W BTFSC STATUS,C ; C = 1 then W is less than TMR0 BCF SERVO2 ; bit now = 0 MOVF Serv3,W ; Get servo to scan SUBWF TMR0,W ; TMR0 - W -> W BTFSC STATUS,C ; C = 1 then W is less than TMR0 BCF SERVO3 ; bit now = 0 MOVF TMR0,W ; get THE most recent timer value SUBWF otmr,W ; subtract from the last saved value BTFSS STATUS,C ; if c = 1 then we rolled over GOTO bdone ; no rollover brllovr MOVLW 0x28 ; clear all bits MOVWF shadow ; always output through shadow register CLRF TMR0 ; and start at zero again DECF rocount,F ; decrement the rollover count BTFSS STATUS,Z ; do a rollover if we have 0 GOTO bdone ; do nothing more if not brset MOVF smask,W ; Set all unmasked servos back to '1' MOVWF shadow ; and set the bits now MOVLW 0x0A ; and reset counter MOVWF rocount bdone MOVF TMR0,W ; Get that last one MOVWF otmr ; and make it the last saved value MOVF shadow,W MOVWF GPIO ; output the shadow register now DECFSZ delay,F ; decrement loop count GOTO bloop ; and do it again RETLW 0 ; all done now END ; directive 'end of program'