How To Radio Control Servos Cheaply
Sample Command Code In Other PIC
#include <P16F687.inc> __config (_INTRC_OSC_NOCLKOUT & _WDT_OFF & _PWRTE_OFF &_MCLRE_ON & _CP_ON & _BOR_OFF & _IESO_OFF & _FCMEN_OFF) ;USING TIMER 0 BUT MAY BE USED IN ANOTHER ROUTINE cblock 0x20 ;start a block of variables TESTREG0 DELAY1 DELAY2 endc ORG 0x0000 ;processor reset vector NOP GOTO MAIN ; go to beginning of program MAIN ;-------- GOTO POINT ----------------- ;MAINSETUP ;SETUP FOR CLOCK BANKSEL OSCCON BSF OSCCON,IRCF2 ;SET FOR 8MHZ BSF OSCCON,IRCF1 BSF OSCCON,IRCF0 ; This is where the chip's clock can be tuned. If you .. ; have check everything and the servo controller moves .. ; the servos a few times and then stop working. The .. ; problem may be that the two chip's clock are too far .. ; apart time wise. The serial BAUD RATE comes from this .. ; clock ;TWO'S COMPLEMENT MAX FREQ = B'00001111' MIN =B'00010000 ;MOVLW B'00011111' ;MOVWF OSCTUNE BANKSEL ANSEL CLRF ANSEL ;TURN OFF ANALOG INPUTS CLRF ANSELH ;TURN OFF ANALOG INPUTS ;SETUP FOR INPUTs BANKSEL TRISA BSF TRISA,3 ; PIN 4 MCLR/VPP BSF TRISA,0 ; PIN 19 ICSPDAT BSF TRISA,1 ; PIN 18 ICSPCLK BSF TRISC,7 ; PIN 9 TELLS PIC TO SEND NEXT COMMAND ;SETUP FOR OUTPUTs BCF TRISB,7 ;PIN 10 TX CLK ;SPARES BCF TRISA,5 ;PIN 2 BCF TRISA,2 ;PIN 17 BCF TRISA,4 ;PIN 3 BCF TRISB,6 ;PIN 11 BCF TRISB,5 ;PIN 12 BCF TRISB,4 ;PIN 13 BCF TRISC,0 ;PIN 16 BCF TRISC,1 ;PIN 15 BCF TRISC,2 ;PIN 14 BCF TRISC,3 ;PIN 7 BCF TRISC,4 ;PIN 6 BCF TRISC,5 ;PIN 5 BCF TRISC,6 ;PIN 8 ; ---------------------------------------------------- BANKSEL TESTREG0 CLRF PORTA CLRF PORTB CLRF PORTC CLRF DELAY1 CLRF DELAY2 ;set for 19200 BAUD WHICH SHOULD TRANSMIT 1 BYTE OF .. ;DATA IN ABOUT 520 US ;SETTING UP SENDING DATA EUSART BANKSEL TXSTA BSF TXSTA,TXEN ;ENABLES THE TRANSMITTER CIRCUITRY BANKSEL RCSTA BSF RCSTA,SPEN ;CONFIGURES THE TX/CK I/O PIN AS AN OUTPUT ;SETTING UP THE EUSART BAUD RATE BANKSEL TXSTA BCF TXSTA,SYNC ;CONFIGURES FOR ASYNCHRONOUS BCF BAUDCTL,BRG16 BSF TXSTA,BRGH MOVLW D'25' MOVWF SPBRG MOVLW D'0' MOVWF SPBRGH BANKSEL TESTREG0 MAINPROGRAMLOOP ;-------- GOTO POINT ----------------- ;DO NOT PUT IN A NUMBER HIGHER THAN 250 UNLESS YOU WANT .. ;THE SERVO CONTROLLER TO PUT A HIGH ON THE OUTPUT .. ;THIS WILL CAUSE THE SERVO TO LOSE POWER ;EVERY COUNT OF 2 = .02 MS ;IF YOU PUT IN NUMBERS HIGHER OR LOWER THAN YOUR SERVO .. ;CAN MOVE, YOU MAY BURN OUT YOUR SERVO!! START WITH 150 .. ;THEN NUMBERS HIGHER AND LOWER THAN THE 150 UNTIL YOU .. ;KNOW YOUR SERVO LIMITS. ;THE SERVO CONTROLLER CAN TAKE UP TO 4 COMMANDS EVERY .. ;20 MS ;SERVO 1 ;THE SERVO CONTROLLER LETS THE PIC KNOW THAT IT CAN .. ;SEND THE NEXT GROUP OF COMMANDS CALL CLEAR_TO_SEND MOVLW D'200' ;WRITE DATA BYTE 1ST, PWM VALUE MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA ;IT TAKES THE SERVO CONTROLLER .520 MS TO GET THE 1ST BYTE CALL BYTE_WAIT_TIMER ;WAIT .512 MS MOVLW D'1' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND ;IT TAKES THE SERVO CONTROLLER .520 MS TO GET THE 2ND BYTE CALL BYTE_WAIT_TIMER ;IT TAKES THE SERVO CONTROLLER TIME TO PROCESS THE COMMAND CALL BYTE_WAIT_TIMER ;SERVO 2 MOVLW D'200' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'2' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 3 MOVLW D'100' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 4 MOVLW D'200' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'4' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;THE SERVO CONTROLLER CAN TAKE UP TO FOUR COMMANDS .. ;EVERY 20 MS BUT THE SERVO CAN NOT MOVE THIS FAST. SO ;A DELAY IS PUT HERE SO THAT THE SERVO HAS TIME TO MOVE .. ;INTO POSTION. IN A REAL PROGRAM, THE PROGRAM WOULD BE .. ;DOING OTHER THINGS WHILE WAITING FOR THE SERVO TO MOVE CALL COMMAND_WAIT_TIMER ;SERVO 1 CALL CLEAR_TO_SEND MOVLW D'180' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'1' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 2 MOVLW D'182' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'2' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 3 MOVLW D'125' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER ;SERVO 3 CALL CLEAR_TO_SEND MOVLW D'150' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 5 MOVLW D'100' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'5' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 6 MOVLW D'117' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'6' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER ;HERE WE ARE SENDING JUST 1 COMMAND. REMEMBER THAT YOU .. ;CAN SEND 1 TO 4 COMMANDS BEFORE YOU HAVE TO WAIT FOR .. ;A "CLEAR TO SEND" FROM THE SERVO CONTROLLER ;SERVO 3 CALL CLEAR_TO_SEND MOVLW D'175' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER ;SERVO 3 CALL CLEAR_TO_SEND MOVLW D'200' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER ;SERVO 3 CALL CLEAR_TO_SEND MOVLW D'175' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER ;SERVO 3 CALL CLEAR_TO_SEND MOVLW D'150' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER ;SERVO 3 CALL CLEAR_TO_SEND MOVLW D'125' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'3' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER ;SERVO 5 MOVLW D'150' ;WRITE DATA BYTE 1ST, PWM MOVWF TXREG ;WRITE TO THIS REG TO SEND DATA CALL BYTE_WAIT_TIMER MOVLW D'5' ;WRITE COMMAND MOVWF TXREG ;WRITE TO THIS REG TO SEND COMMAND CALL BYTE_WAIT_TIMER CALL BYTE_WAIT_TIMER CALL COMMAND_WAIT_TIMER GOTO MAINPROGRAMLOOP ; ------------ START OF SUBROUTINES ----------------- ;-------- START OF COMMAND WAIT TIMER SUBROUTINE -------- COMMAND_WAIT_TIMER: DELAY: DELAYLOOP; GOTO POINT -------- DECFSZ DELAY1,f GOTO DELAYLOOP DECFSZ DELAY2,f GOTO DELAYLOOP DELAYLOOP2; GOTO POINT -------- DECFSZ DELAY1,f GOTO DELAYLOOP2 DECFSZ DELAY2,f GOTO DELAYLOOP2 DELAYLOOP3; GOTO POINT -------- DECFSZ DELAY1,f GOTO DELAYLOOP3 DECFSZ DELAY2,f GOTO DELAYLOOP3 RETURN ;-------- END OF COMMAND WAIT TIMER SUBROUTINE -------- ;-------- START OF BYTE WAIT TIMER SUBROUTINE -------- BYTE_WAIT_TIMER: ;IF CLOCK IS LESS THAN .512 MS ,NO OVERFLOW ;1 / 250000 PULSE/SEC = .000002 ;256 * .000002 = .512 MS ;SETUP FOR TIMER 0 BANKSEL OPTION_REG CLRWDT ;MUST CLEAR WATCHDOG TIMER BEFORE USING ... ;PRESCALER ON TIMER 0 BCF OPTION_REG,PSA ;BIT 3, L = PRESCALER TURNED ON BCF OPTION_REG,T0CS ;BIT 5, L = INTERNAL /4 CLOCK ;/4 CLOCK = 2000000 / 4 = 500000 OUTPUT OF PRESCALER ,1 BCF OPTION_REG,PS2 BCF OPTION_REG,PS1 BSF OPTION_REG,PS0 BANKSEL INTCON BCF INTCON,T0IF ;CLEAR OVERFLOW OF TIMER 0 BANKSEL TMR0 CLRF TMR0 ;REGS OF TIMER, READ & WRITABLE BANKSEL TESTREG0 BSF PORTA,4 ;TEST PIN 3 TEMP TAKE OUT LATER WAIST_TIME1 ;GOTO POINT -------------------- BTFSS INTCON,T0IF ;CHECK CLOCK IF L WAIST TIME GOTO WAIST_TIME1 BCF PORTA,4 ;TEST PIN 3 TEMP TAKE OUT LATER RETURN ;-------- END OF BYTE WAIT TIMER SUBROUTINE -------- ;-------- START OF CLEAR TO SEND SUBROUTINE -------- CLEAR_TO_SEND: CHECK_FOR_CLEAR_TO_SEND ;GOTO POINT IF H SEND ---------- ;----------- IF L DO ---------------- BTFSC PORTC,7 ;IF L ON PIN 9 DO GOTO THEEND1 GOTO CHECK_FOR_CLEAR_TO_SEND ;DO THEEND1 ;GOTO POINT --------------------- RETURN ;-------- END OF CLEAR TO SEND SUBROUTINE -------- ; ************** END OF SUBROUTINES ***************** END
Next Page
Home Page