; eBASIC PIC16 Code Generator ; Copyright(c) 2004, Compubotics LLC ; www.Compubotics.com ORG 0x0 ; File: servos.bas ; ' --------------------------------------------- ; ' program to position multiple servos ; ' servos connected to the four servo ports ; ' PS1 -> PS4 on the RC40 ; ' ; ' D. Evans ; ' September 7, 2004 ; ' ---------------------------------------------- ; ; #include "rc40.inc" ; ; ' constant for loop ; Forever con 1 ; ; ' misc variables ; I var byte ; pulseTime var word ; ; ' misc constants ; ServoLeft con 500 ; ; ' Array to hold the positions for the servos ; ServoPos var byte(4) ; ' Array to hold ports for servos ; ServoPort var byte(4) ; ; ' variable used to accumulate loop iterations ; loopCounter var word ; ; ' variable to hold animatronic state ; machineState var byte ; ; ' constants to define machine states ; ST_START con 0 ; ST_LEFT con 1 ; ST_RIGHT con 2 ; ST_UP CON 3 ; ST_DOWN con 4 ; ; ' --------- ; ' main loop ; ' --------- ; Main: nop clrf 0xa goto _startup ORG 0x4 retfie _STR001: retlw 0x6d retlw 0x61 retlw 0x63 retlw 0x68 retlw 0x69 retlw 0x6e retlw 0x65 retlw 0x53 retlw 0x74 retlw 0x61 retlw 0x74 retlw 0x65 retlw 0x3d retlw 0x0 _startup: movlw 0x83 bsf 0x3,5 bcf 0x3,6 movwf 0x81 bcf 0x3,5 bcf 0xb,5 bsf 0xb,6 movlw 0x31 movwf 0x10 movlw 0x60 movwf 0x78 call _uartSetup goto Main ; ; ' initialize everything ; gosub Init Main: call Init ; ; ' never-ending loop ; while (Forever) ; ; ' periodically change to the next machine state ; ' if this loop executes every 30 ms, this would ; ' be about every 3.3 seconds ; if (loopCounter = 100) then _j000: bcf 0x3,5 bcf 0x3,6 movf 0x2b,0 movwf 0x70 movf 0x2c,0 movwf 0x71 movlw 0x64 movwf 0x72 clrf 0x73 call _comp16 btfsc 0x3,2 goto _j002 clrf 0x70 clrf 0x71 goto _j003 _j002: movlw 0x1 movwf 0x70 clrf 0x71 _j003: clrf 0x72 clrf 0x73 call _comp16 btfsc 0x3,2 goto _j004 ; gosub servoState call servoState ; loopCounter = 0 bcf 0x3,5 bcf 0x3,6 clrf 0x2b clrf 0x2c ; endif ; ; ' increment loop counter ; loopCounter = loopCounter + 1 _j004: movf 0x2b,0 movwf 0x70 movf 0x2c,0 movwf 0x71 movlw 0x1 movwf 0x72 clrf 0x73 call _add16a movf 0x70,0 bcf 0x3,5 bcf 0x3,6 movwf 0x2b movf 0x71,0 movwf 0x2c ; ; ' kick the servos ; gosub kickServos call kickServos ; ; ' pause a little ; pause 20 movlw 0x14 movwf 0x70 clrf 0x71 call _pause ; ; ' if you are doing other things, you might ; ' place additional code here and reduce or ; ' eliminate the pause statement ; ; wend goto _j000 ; ; END _j001: sleep goto $-1 ; ; ; ' ------------------------ ; ' initialize all variables ; ' ------------------------ ; Init: ; ; ' initialize loop counter ; loopCounter = 0 Init: bcf 0x3,5 bcf 0x3,6 clrf 0x2b clrf 0x2c ; ; ' initialize machine state ; machineState = ST_START clrf 0x2d ; ; ' initialize servo positions to center ; for i = 0 to 3 clrf 0x70 clrf 0x71 _j005: movf 0x70,0 movwf 0x20 ; servoPos(1) = 0 clrf 0x70 clrf 0x71 movf 0x70,0 movwf 0x24 ; next _j007: movlw 0x3 movwf 0x70 clrf 0x71 movf 0x70,0 call _push movf 0x71,0 call _push bcf 0x3,5 bcf 0x3,6 movf 0x20,0 movwf 0x70 clrf 0x71 movlw 0x1 movwf 0x72 clrf 0x73 call _add16a _j008: call _pop movwf 0x73 call _pop movwf 0x72 call _comp16 btfsc 0x3,2 goto _j005 btfsc 0x3,0 goto _j006 goto _j005 movf 0x70,0 bcf 0x3,5 bcf 0x3,6 movwf 0x20 ; ; ' set servo ports ; servoPort(0) = 1 ' PS1 _j006: movlw 0x1 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x27 ; servoPort(1) = 2 ' PS2 movlw 0x2 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x28 ; servoPort(2) = 8 ' PS3 movlw 0x8 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x29 ; servoPort(3) = 9 ' PS4 movlw 0x9 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x2a ; ; RETURN return ; ; ' ----------------------------------------- ; ' state machine for servo positioning ; ' this routine positions the servos ; ' based on the current machine state ; ' all of these servo values are nonsense, ; ' you'll have to figure out the real stuff ; ' based on your application ; ' ----------------------------------------- ; servoState: ; ; debug "machineState=", dec machineState, CR servoState: movlw 0x5 movwf 0x70 clrf 0x71 call _dbgStr bcf 0x3,5 bcf 0x3,6 movf 0x2d,0 movwf 0x70 clrf 0x71 call _dbgDec movlw 0xd call _uartTX movlw 0xa call _uartTX ; ; branch machineState, [State0, State1, State2, State3, State4] bcf 0x3,5 bcf 0x3,6 movf 0x2d,0 movwf 0x70 clrf 0x71 movlw 0x0 xorwf 0x70,0 btfsc 0x3,2 goto State0 movlw 0x1 xorwf 0x70,0 btfsc 0x3,2 goto State1 movlw 0x2 xorwf 0x70,0 btfsc 0x3,2 goto State2 movlw 0x3 xorwf 0x70,0 btfsc 0x3,2 goto State3 movlw 0x4 xorwf 0x70,0 btfsc 0x3,2 goto State4 ; ; ' safety feature! ; return return ; ; State0: ; ; ' set the servo positions for this state ; servoPos(0) = 1 State0: movlw 0x1 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x23 ; servopos(1) = 4 movlw 0x4 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x24 ; servoPos(2) = 7 movlw 0x7 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x25 ; servoPos(3) = 5 movlw 0x5 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x26 ; ; ' set next state ; machineState = ST_LEFT movlw 0x1 movwf 0x2d ; ; return return ; ; State1: ; ' set the servo positions for this state ; servoPos(0) = 2 State1: movlw 0x2 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x23 ; servopos(1) = 3 movlw 0x3 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x24 ; servoPos(2) = 8 movlw 0x8 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x25 ; servoPos(3) = 1 movlw 0x1 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x26 ; ; ' set next state ; machineState = ST_RIGHT movlw 0x2 movwf 0x2d ; ; return return ; ; State2: ; ; ' set the servo positions for this state ; servoPos(0) = 0 State2: clrf 0x70 clrf 0x71 movf 0x70,0 movwf 0x23 ; servopos(1) = 5 movlw 0x5 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x24 ; servoPos(2) = 2 movlw 0x2 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x25 ; servoPos(3) = 9 movlw 0x9 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x26 ; ; ' set next state ; machineState = ST_UP movlw 0x3 movwf 0x2d ; ; return return ; ; State3: ; ; ' set the servo positions for this state ; servoPos(0) = 3 State3: movlw 0x3 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x23 ; servopos(1) = 8 movlw 0x8 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x24 ; servoPos(2) = 1 movlw 0x1 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x25 ; servoPos(3) = 4 movlw 0x4 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x26 ; ; ' set next state ; machineState = ST_DOWN movlw 0x4 movwf 0x2d ; ; return return ; ; State4: ; ; ' set the servo positions for this state ; servoPos(0) = 1 State4: movlw 0x1 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x23 ; servopos(1) = 4 movlw 0x4 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x24 ; servoPos(2) = 3 movlw 0x3 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x25 ; servoPos(3) = 2 movlw 0x2 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x26 ; ; ' set next state (back to start state) ; machineState = ST_START clrf 0x2d ; ; ' ------------------------------------------- ; ' routine to periodically kick all servos ; ' we'll assume that each servo has a range ; ' of positions ; ' ; ' 0 far left ; ' 5 center ; ' 10 far right ; ' ; ' My servos use a 1500us pulse ; ' center value, with +-500us for the complete ; ' range of motion. Your mileage may vary... ; ' ; ' our timing for PULSOUT is 2us per tick, so ; ' the range is 500-1000 ticks for the full range ; ' ------------------------------------------- ; KickServos: ; ; ' kick all four servos, using the ; ' current positions stored in servopos(x) ; for i = 0 to 3 KickServos: clrf 0x70 clrf 0x71 _j009: movf 0x70,0 movwf 0x20 ; pulseTime = servoPOS(i) * 50 + servoLeft movwf 0x70 clrf 0x71 movf 0x70,0 addlw 0x23 movwf 0x4 movf 0x0,0 movwf 0x70 clrf 0x71 movlw 0x32 movwf 0x72 clrf 0x73 call _mult16a movlw 0xf4 movwf 0x72 movlw 0x1 movwf 0x73 call _add16a movf 0x70,0 bcf 0x3,5 bcf 0x3,6 movwf 0x21 movf 0x71,0 movwf 0x22 ; pulsout servoPort(i), pulseTime movf 0x20,0 movwf 0x70 clrf 0x71 movf 0x70,0 addlw 0x27 movwf 0x4 movf 0x0,0 movwf 0x70 clrf 0x71 movf 0x70,0 movwf 0x74 movf 0x71,0 movwf 0x75 movf 0x21,0 movwf 0x70 movf 0x22,0 movwf 0x71 call _pulsout ; next _j011: movlw 0x3 movwf 0x70 clrf 0x71 movf 0x70,0 call _push movf 0x71,0 call _push bcf 0x3,5 bcf 0x3,6 movf 0x20,0 movwf 0x70 clrf 0x71 movlw 0x1 movwf 0x72 clrf 0x73 call _add16a _j012: call _pop movwf 0x73 call _pop movwf 0x72 call _comp16 btfsc 0x3,2 goto _j009 btfsc 0x3,0 goto _j010 goto _j009 movf 0x70,0 bcf 0x3,5 bcf 0x3,6 movwf 0x20 ; ; Return _j010: return _uartSetup: movlw 0x67 bsf 0x3,5 bcf 0x3,6 movwf 0x99 bsf 0x98,2 bsf 0x98,5 bcf 0x3,5 bsf 0x18,4 bsf 0x18,7 return _comp16: movf 0x73,0 subwf 0x71,0 btfss 0x3,2 return movf 0x72,0 subwf 0x70,0 return _add16a: movf 0x72,0 addwf 0x70,1 movf 0x73,0 btfsc 0x3,0 incf 0x73,0 addwf 0x71,1 return _dec16: decf 0x70,1 incfsz 0x70,0 incf 0x71,1 decf 0x71,1 movf 0x70,0 iorwf 0x71,0 return _pause: bcf 0x3,5 bcf 0x3,6 bcf 0xb,2 btfss 0xb,2 goto $-1 call _dec16 btfss 0x3,2 goto _pause return _push: bcf 0xb,7 movwf 0x79 movf 0x78,0 movwf 0x4 movf 0x79,0 movwf 0x0 incf 0x78,1 bsf 0xb,7 return _pop: bcf 0xb,7 decf 0x78,1 movf 0x78,0 movwf 0x4 movf 0x0,0 bsf 0xb,7 return _uartTX: bcf 0x3,5 bcf 0x3,6 btfss 0xc,4 goto $-1 movwf 0x19 return _inc16: incfsz 0x70,0 decf 0x71,1 incf 0x71,1 movwf 0x70 iorwf 0x71,0 return _dbgStr: call _j013 iorlw 0x0 btfsc 0x3,2 return call _uartTX call _inc16 goto _dbgStr _j013: movf 0x71,0 movwf 0xa movf 0x70,0 movwf 0x2 _shftl16: bcf 0x3,0 rlf 0x72,1 rlf 0x73,1 decfsz 0x74,1 goto _shftl16 return _shftr16: bcf 0x3,0 rrf 0x73,1 rrf 0x72,1 decfsz 0x74,1 goto _shftr16 return _sub16a: movf 0x72,0 subwf 0x70,1 movf 0x73,0 btfss 0x3,0 incf 0x73,0 subwf 0x71,1 return _div16x: clrf 0x76 clrf 0x77 movf 0x72,0 iorwf 0x73,0 btfsc 0x3,2 retlw 0xff movlw 0x1 movwf 0x74 movf 0x73,0 iorlw 0x0 btfss 0x3,2 goto _j014 movf 0x72,0 xorlw 0x1 btfss 0x3,2 goto _j014 movf 0x70,0 movwf 0x76 movf 0x71,0 movwf 0x77 clrf 0x70 clrf 0x71 retlw 0x0 _j014: movf 0x70,0 iorwf 0x71,0 btfss 0x3,2 goto _j015 retlw 0x0 _j015: btfsc 0x73,7 goto _j016 incf 0x74,1 movf 0x74,0 call _push movlw 0x1 movwf 0x74 call _shftl16 call _pop movwf 0x74 goto _j015 _j016: movf 0x72,0 call _push movf 0x73,0 call _push movf 0x76,0 movwf 0x72 movf 0x77,0 movwf 0x73 movf 0x74,0 call _push movlw 0x1 movwf 0x74 call _shftl16 call _pop movwf 0x74 movf 0x72,0 movwf 0x76 movf 0x73,0 movwf 0x77 call _pop movwf 0x73 call _pop movwf 0x72 call _sub16a btfsc 0x3,0 goto _j017 call _add16a goto _j018 _j017: bsf 0x76,0 _j018: decf 0x74,1 btfsc 0x3,2 retlw 0x0 movf 0x74,0 call _push movlw 0x1 movwf 0x74 call _shftr16 call _pop movwf 0x74 goto _j016 _compza: movlw 0x0 iorwf 0x71,0 btfss 0x3,2 return iorwf 0x70,1 return _neg16: comf 0x70,1 comf 0x71,1 goto _INC16 _dbgSDec: btfss 0x71,7 goto _dbgDec movlw 0x2d call _uartTX call _neg16 _dbgDec: clrf 0x50 clrf 0x51 clrf 0x52 clrf 0x53 clrf 0x54 movlw 0x5 movwf 0x75 _j019: decf 0x75,1 movlw 0xa movwf 0x72 clrf 0x73 call _div16x movlw 0x50 addwf 0x75,0 movwf 0x4 movf 0x70,0 addlw 0x30 movwf 0x0 movf 0x76,0 movwf 0x70 movf 0x77,0 movwf 0x71 call _compza btfsc 0x3,2 goto _j020 movf 0x75,0 iorlw 0x0 btfss 0x3,2 goto _j019 _j020: movlw 0x0 movwf 0x74 _j021: movlw 0x50 addwf 0x74,0 movwf 0x4 movf 0x0,0 btfss 0x3,2 call _uartTX incf 0x74,1 movf 0x74,0 xorlw 0x5 btfss 0x3,2 goto _j021 return _add16x: movf 0x70,0 addwf 0x76,1 movf 0x71,0 btfsc 0x3,0 incf 0x71,0 addwf 0x77,1 return _MULT16A: clrf 0x76 clrf 0x77 clrf 0x74 clrf 0x75 movf 0x70,0 iorwf 0x71,0 btfss 0x3,2 goto _j022 retlw 0x0 _j022: movf 0x72,0 iorwf 0x73,0 btfsc 0x3,2 goto _j025 btfss 0x72,0 goto _j024 _j023: call _add16x btfss 0x3,0 goto _j024 rlf 0x74,1 rlf 0x75,1 _j024: bcf 0x3,0 rrf 0x73,1 rrf 0x72,1 bcf 0x3,0 rlf 0x70,1 rlf 0x71,1 goto _j022 _j025: movf 0x76,0 movwf 0x70 movf 0x77,0 movwf 0x71 retlw 0x0 _shftl16a: bcf 0x3,0 movf 0x72,1 btfsc 0x3,2 return rlf 0x70,1 rlf 0x71,1 decfsz 0x72,1 goto _shftl16a return _DCD: movf 0x70,0 movwf 0x72 movf 0x71,0 movwf 0x73 movlw 0x1 movwf 0x70 clrf 0x71 call _shftl16a return _low: call _DCD bsf 0x3,5 bcf 0x3,6 movf 0x70,0 iorwf 0x86,1 movf 0x71,0 iorwf 0x87,1 movlw 0xff xorwf 0x70,1 xorwf 0x71,1 movf 0x70,0 bcf 0x3,5 andwf 0x6,1 movf 0x71,0 andwf 0x7,1 movf 0x70,0 bsf 0x3,5 andwf 0x86,1 movf 0x70,0 andwf 0x87,1 return _high: call _DCD bcf 0x3,5 bcf 0x3,6 movf 0x70,0 iorwf 0x7a,1 movf 0x71,0 iorwf 0x7b,1 movf 0x70,0 iorwf 0x6,1 movf 0x71,0 iorwf 0x7,1 movlw 0xff xorwf 0x70,1 xorwf 0x71,1 movf 0x70,0 bsf 0x3,5 andwf 0x86,1 movf 0x71,0 andwf 0x87,1 return _pulsout: movf 0x70,0 movwf 0x72 movf 0x71,0 movwf 0x73 movlw 0xff movwf 0x70 movwf 0x71 call _sub16a movf 0x70,0 bcf 0x3,5 bcf 0x3,6 movwf 0xe movf 0x71,0 movwf 0xf movf 0x74,0 movwf 0x70 clrf 0x71 call _high bcf 0x3,5 bcf 0x3,6 bcf 0xc,0 btfss 0xc,0 goto $-1 bcf 0xc,0 movf 0x74,0 movwf 0x70 clrf 0x71 call _low return END ; end of code generation