include "/usr/share/gpasm/header/p16c74a.inc" ; config stuff ; these values are in tmr0 ints, which are ~1.6 msec RXTIMEOUT_INITIAL equ 0x30 ; approx. 16 bytes RESENDTIMEOUT_INITIAL equ 0xFF ; ENCODERTIMEOUT equ 0x02 ; 3.2 msec ; TOFROM_RX equ 0x2A TOFROM_TX equ 0xA2 ; packet types P_SETPWM equ 0x01 P_SENDADCRES equ 0x02 P_SENDSPEED equ 0x03 P_SETPOWER equ 0x04 P_SENDLEFTENC equ 0x05 P_SENDTICKCOUNT equ 0x06 P_PING equ 0x07 ; bank 0 register assignments: MTEMP0 equ 0x20 MTEMP1 equ 0x21 MTEMP2 equ 0x22 MTEMP3 equ 0x23 MTEMP4 equ 0x24 MTEMP5 equ 0x25 MTEMP6 equ 0x26 MTEMP7 equ 0x27 ITEMP0 equ 0x28 ITEMP1 equ 0x29 ITEMP2 equ 0x2A ITEMP3 equ 0x2B ITEMP4 equ 0x2C ITEMP5 equ 0x2D ITEMP6 equ 0x2E ITEMP7 equ 0x2F W_TEMP equ 0x30 ; store stuff during interrupts STATUS_TEMP equ 0x31 ; PCLATH_TEMP equ 0x32 ; FSR_TEMP equ 0x33 ; PACKETSUM_INDEX equ 0x34 RXTIMEOUT equ 0x35 TICKTOCK equ 0x36 ; incremented by one of the timers CONDITION equ 0x37 ; various robot state things MOVING equ 0 ; robot is moving MANUAL equ 1 ; robot pwm on manual control SERVOPOS equ 0x38 ; current servo position ; there should be some way of estimating servo movement ; or maybe there should be a servo current sense resistor T1POST equ 0x3B ; these control the leds GREEN equ 0x39 YELLOW equ 0x3A ADCMODE equ 0x3F ADCOUTPUT equ 0x40 TXAPACKET equ 0x48 ; tx ack packet TXAPTOFROM equ 0x48 TXAPNUMACK equ 0x49 TXAPFLAGLEN equ 0x4A TXAPSUM equ 0x4B RESENDTIMEOUT equ 0x4C ACKQUEUE equ 0x50 TXLEN equ 0x51 ; transmit length TXPTR equ 0x52 ; transmit pointer NEXTNUM equ 0x53 ; next packet number COMSTAT equ 0x54 ; communication status TXING equ 0 ; transmitting at the moment RXINDEX equ 0x55 TXINDEX equ 0x56 PBFLAGS equ 0x57 ; packet buffer flags RXPF equ 0 ; rx packet full TXPF equ 1 ; tx packet full TXAPF equ 2 ; tx ack packet full TXPA equ 3 ; transmission attemped at least once TXPHOLD equ 4 ; waiting for ack, don't transmit ; cleared by some timer TXPACKET equ 0x58 TXPTOFROM equ 0x58 TXPNUMACK equ 0x59 TXPFLAGLEN equ 0x5A TXPDATA equ 0x5B ; 17 bytes RXPACKET equ 0x6C RXPTOFROM equ 0x6C RXPNUMACK equ 0x6D RXPFLAGLEN equ 0x6E RXPDATA equ 0x6F ; 17 bytes ; bank 1 assignments: LENCODER equ 0xA0 LEOL equ 0xA0 LEOH equ 0xA1 LENL equ 0xA2 LENH equ 0xA3 LEDL equ 0xA4 LEDH equ 0xA5 RENCODER equ 0xA6 REOL equ 0xA6 REOH equ 0xA7 RENL equ 0xA8 RENH equ 0xA9 REDL equ 0xAA REDH equ 0xAB ENCOVER equ 0xAC LEFTO1 equ 0 LEFTO2 equ 1 RIGHTO1 equ 2 RIGHT02 equ 3 RBBO equ 0xAD RBBN equ 0xAE RBBC equ 0xAF ; W_TEMP equ 0xB0 S16TMP0 equ 0xB1 S16TMP1 equ 0xB2 S16TMP2 equ 0xB3 S16TMP3 equ 0xB4 S16TMP4 equ 0xB5 S16TMP5 equ 0xB6 T1BL equ 0xB7 T1BH equ 0xB8 LTCL equ 0xB9 LTCH equ 0xBA RTCL equ 0xBB RTCH equ 0xBC LEH equ 0xBD REH equ 0xBE SONARTIME equ 0xBF STXL equ 0xBF STXH equ 0xC0 SRXL equ 0xC1 SRXH equ 0xC2 SRL equ 0xC3 SRH equ 0xC4 LTS equ 0xC5 RTS equ 0xC6 LSTC equ 0xC7 RSTC equ 0xC8 LSTCL equ 0xC9 RSTCL equ 0xCA SONARSWEEP equ 0xD0 ; 16 bytes IRSWEEP equ 0xE0 ; 32 bytes org 0x0000 ; reset vector ; bsf PCLATH, 3 ; upper codepage goto startmain org 0x0004 ; int vector ; save various registers ; this comes from p143 of the datasheet movwf W_TEMP swapf STATUS, W clrf STATUS movwf STATUS_TEMP movf PCLATH, W movwf PCLATH_TEMP clrf PCLATH bcf STATUS, IRP movf FSR, W movwf FSR_TEMP ; interrupt priority is the order here ; but it really doesn't make much difference btfsc INTCON, RBIF call rbi_handler btfsc INTCON, T0IF call tmr0_handler btfsc PIR1, TMR1IF call tmr1_handler btfsc PIR1, TMR2IF call tmr2_handler btfsc PIR1, RCIF call usart_rx_handler btfsc PIR1, TXIF call usart_tx_handler btfsc PIR1, ADIF call adc_handler movf PCLATH_TEMP, W movwf PCLATH swapf STATUS_TEMP, W movwf STATUS swapf W_TEMP, F swapf W_TEMP, W retfie adc_handler: bcf PIR1, ADIF ; conversion is complete ; figure out which port was just read movf ADCON0, W movwf ITEMP0 rrf ITEMP0, F rrf ITEMP0, F rrf ITEMP0, F movf ITEMP0, W andlw 0x07 ; W now contains the channel that was just read movwf ITEMP0 addlw ADCOUTPUT movwf FSR ; fsr now points to where the result goes movf ADRES, W movwf INDF ; data saved. now configure the next conversion incf ITEMP0, F rlf ITEMP0, F rlf ITEMP0, F rlf ITEMP0, F movf ITEMP0, W andlw 0x38 movwf ITEMP0 ; ITEMP0 now contains the next channel in bits 3-5 ; set movf ADCON0, W andlw 0xC7 iorwf ITEMP0, W movwf ADCON0 ; adcon now prepared for next conversion ; which will be started by T0 ; FIXME ; there should really be a hold byte for that return rbi_handler: ; the order of these two instructions is important. movf PORTB, W bcf INTCON, RBIF bsf STATUS, RP0 ; save new value in RBBN movwf RBBN ; update RBBC (changed) movwf RBBC movf RBBO, W xorwf RBBC, F ; RBBC and RBBN are now set ; now read the timer into T1BL/T1BH ; this code is modified from datasheet page 67 bcf STATUS, RP0 movf TMR1H, W bsf STATUS, RP0 movwf T1BH bcf STATUS, RP0 movf TMR1L, W bsf STATUS, RP0 movwf T1BL bcf STATUS, RP0 movf TMR1H, W bsf STATUS, RP0 subwf T1BH, W btfsc STATUS, Z goto rbi_handler_readcomplete bcf STATUS, RP0 movf TMR1H, W bsf STATUS, RP0 movwf T1BH bcf STATUS, RP0 movf TMR1L, W bsf STATUS, RP0 movwf T1BL rbi_handler_readcomplete: btfsc RBBC, 5 call sonarint ; the other things only make sense if we're moving ; e.g. encoder ticks are garaunteed to be noise and ; we probably didn't really run into anything. bcf STATUS, RP0 btfss CONDITION, MOVING goto rbi_handler_done bsf STATUS, RP0 btfsc RBBC, 7 call leftencodertick btfsc RBBC, 6 call rightencodertick btfsc RBBC, 4 call bumpint ; new isn't new anymore movf RBBN, W movwf RBBO rbi_handler_done: bcf STATUS, RP0 return bumpint: ; do nothing on un-bump btfss RBBN, 4 return ; we just ran into a wall. ; it might be a good idea to stop or something. bcf STATUS, RP0 movlw 0x00 movwf CCPR1L movwf CCPR2L bsf STATUS, RP0 return sonarint: ; if the line just became high ; we just pinged btfss RBBN, 5 goto sonarint_rx sonarint_tx: ; save time movf T1BL, W movwf STXL movf T1BH, W movwf STXH goto sonarint_done sonarint_rx: ; save time movf T1BL, W movwf SRXL movf T1BH, W movwf SRXH ; subtract movlw SONARTIME movwf FSR call sub16 sonarint_done: return leftencodertick: movf LEH, F btfss STATUS, Z return movlw ENCODERTIMEOUT movwf LEH movlw 0x20 bcf STATUS, RP0 movwf YELLOW bsf STATUS, RP0 incf LTCH, F incfsz LTCL, F decf LTCH, F ; movf LENL, W ; movwf LEOL ; movf LENH, W ; movwf LEOH ; movf T1BL, W ; movwf LENL ; movf T1BH, W ; movwf LENH ; ; movlw LENCODER ; movwf FSR ; call sub16 incf LSTC, F return rightencodertick: movf REH, F btfss STATUS, Z return movlw ENCODERTIMEOUT movwf REH movlw 0x20 bcf STATUS, RP0 movwf GREEN bsf STATUS, RP0 incf RTCH, F incfsz RTCL, F decf RTCH, F ; movf RENL, W ; movwf REOL ; movf RENH, W ; movwf REOH ; movf T1BL, W ; movwf RENL ; movf T1BH, W ; movwf RENH ; ; movlw RENCODER ; movwf FSR ; call sub16 incf RSTC, F return ; sub16 ; performs 16 bit subtraction of 6 byte structures ; precondition: FSR points to the beginning of the struct ; e.g. new-old sub16: movf INDF, W movwf S16TMP0 incf FSR, F movf INDF, W movwf S16TMP1 incf FSR, F movf INDF, W movwf S16TMP2 incf FSR, F movf INDF, W movwf S16TMP3 incf FSR, F ; FSR now ready to write back result movf S16TMP3, W ; A high movwf S16TMP5 ; result high movf S16TMP1, W ; B high subwf S16TMP5, F ; (a high) - (b high) ; if that generated a borrow, it's not our business. movf S16TMP2, W ; A low movwf S16TMP4 ; result low movf S16TMP0, W ; B low subwf S16TMP4, F ; (a low) - (b low) ; that might have caused a borrow ; if it did, C is zero btfss STATUS, C decf S16TMP5, F ; FSR from before movf S16TMP4, W movwf INDF incf FSR, F movf S16TMP5, W movwf INDF return ; timer 0 handler tmr0_handler: bcf INTCON, T0IF ; clear interrupt bcf PORTC, 0 ; servo signal low ; usart recieve timeout section decfsz RXTIMEOUT, F goto tmr0_handler_notimeout ; reset the timeout movlw RXTIMEOUT_INITIAL movwf RXTIMEOUT ; see if the buffer is full - if it is, don't do anything btfsc PBFLAGS, RXPF goto tmr0_handler_notimeout ; buffer not full, so reset it clrf RXINDEX bcf RCSTA, CREN ; might have overrun. bsf RCSTA, CREN ; so reset reciever. tmr0_handler_notimeout: ; resend section btfss PBFLAGS, TXPHOLD goto tmr0_handler_resend_done ; we are holding for resend decfsz RESENDTIMEOUT, F goto tmr0_handler_resend_done ; and it has timed out ; reset timeout movlw RESENDTIMEOUT_INITIAL movwf RESENDTIMEOUT ; stop holding bcf PBFLAGS, TXPHOLD ; enable tx interrupts bsf STATUS, RP0 bsf PIE1, TXIE bcf STATUS, RP0 tmr0_handler_resend_done: ; green led control section movf GREEN, F btfss STATUS, Z bsf PORTD, 3 btfsc STATUS, Z bcf PORTD, 3 btfss STATUS, Z decf GREEN, F ; yellow led control section movf YELLOW, F btfss STATUS, Z bsf PORTD, 2 btfsc STATUS, Z bcf PORTD, 2 btfss STATUS, Z decf YELLOW, F ; encoder timeout section bsf STATUS, RP0 movf LEH, F btfss STATUS, Z decf LEH, F movf REH, F btfss STATUS, Z decf REH, F bcf STATUS, RP0 ; start the adc bsf ADCON0, GO tmr0_handler_done: return ;timer 1 handler tmr1_handler: bcf PIR1, TMR1IF incf TICKTOCK, F ; if we're moving btfss CONDITION, MOVING ; goto tmr1_handler_done return ; and on auto control btfsc CONDITION, MANUAL ; goto tmr1_handler_done return ; and only every tenth time decfsz T1POST, F ; goto tmr1_handler_done return movlw 0x0A movwf T1POST ; ok, update motor speeds ; bank 1 is where all the timing stuff is bsf STATUS, RP0 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; movf LSTC, W movwf LSTCL movf LTS, W subwf LSTC, W movwf S16TMP2 ; if it's zero, we're all good. btfsc STATUS, Z goto tmr1_handler_right ; movlw 0x0A ; movwf S16TMP0 bcf STATUS, RP0 movf CCPR2L, W bsf STATUS, RP0 movwf S16TMP1 ; if LSTC > LTS, we're going too fast. ; in which case carry is set. slow down. ; btfss STATUS, C ; goto tmr1_handler_left_faster ; goto tmr1_handler_left_slower ; ;tmr1_handler_left_faster: ; incf S16TMP1, F ; btfsc STATUS, Z ; decf S16TMP1, F ; decfsz S16TMP0, F ; goto tmr1_handler_left_faster ; goto tmr1_handler_left_update ;tmr1_handler_left_slower: ; movf S16TMP1, F ; btfss STATUS, Z ; decf S16TMP1, F ; decfsz S16TMP0, F ; goto tmr1_handler_left_slower ; goto tmr1_handler_left_update call adjustspeed tmr1_handler_left_update: movf S16TMP1, W bcf STATUS, RP0 movwf CCPR2L bsf STATUS, RP0 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; tmr1_handler_right: movf RSTC, W movwf RSTCL movf RTS, W subwf RSTC, W movwf S16TMP2 ; if it's zero, we're all good. btfsc STATUS, Z goto tmr1_handler_done ; movlw 0x0A ; movwf S16TMP0 ; bcf STATUS, RP0 movf CCPR1L, W bsf STATUS, RP0 movwf S16TMP1 ; ; ; if RSTC > RTS, we're going too fast. ; ; in which case carry is set. slow down. ; btfss STATUS, C ; goto tmr1_handler_right_faster ; goto tmr1_handler_right_slower ; ;tmr1_handler_right_faster: ; incf S16TMP1, F ; btfsc STATUS, Z ; decf S16TMP1, F ; decfsz S16TMP0, F ; goto tmr1_handler_right_faster ; goto tmr1_handler_right_update ;tmr1_handler_right_slower: ; movf S16TMP1, F ; btfss STATUS, Z ; decf S16TMP1, F ; decfsz S16TMP0, F ; goto tmr1_handler_right_slower ; goto tmr1_handler_right_update call adjustspeed tmr1_handler_right_update: movf S16TMP1, W bcf STATUS, RP0 movwf CCPR1L bsf STATUS, RP0 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; tmr1_handler_done: movlw 0x00 movwf LSTC movwf RSTC bcf STATUS, RP0 return ; difference is in S16TMP2 ; speed is in S16TMP1 adjustspeed: ;remember if diff was + or - bcf S16TMP3, 0 btfsc S16TMP2, 7 bsf S16TMP3, 0 ; if it was negative, invert it btfss S16TMP3, 0 goto adjustspeed_noinvert movf S16TMP2, W sublw 0x00 movwf S16TMP2 adjustspeed_noinvert: ; positive (0) means too fast, slow down. ; negative (1) means too slow, speed up. btfss S16TMP3, 0 goto adjustspeed_slower adjustspeed_faster: incf S16TMP1, F btfsc STATUS, Z decf S16TMP1, F decfsz S16TMP2, F goto adjustspeed_faster return adjustspeed_slower: movf S16TMP1, F btfss STATUS, Z decf S16TMP1, F decfsz S16TMP2, F goto adjustspeed_slower return ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; timer 2 handler tmr2_handler: bcf PIR1, TMR2IF ; 76hz speed movf SERVOPOS, W sublw 0x00 ; 0x00 - servocon movwf TMR0 ; set timer 0 bsf PORTC, 0 ; servo signal high tmr2_handler_done: return ; serial reception handler usart_rx_handler: movf RCREG, W ; clear the interrupt movwf ITEMP7 ; reset the timeout counter movlw RXTIMEOUT_INITIAL movwf RXTIMEOUT btfsc COMSTAT, TXING ; if we're transmitting now... goto usart_rx_handler_done ; we don't want to hear it. btfsc PBFLAGS, RXPF ; if the recieve buffer is full... goto usart_rx_handler_done ; we have no place to put it. movf RXINDEX, W addlw RXPACKET ; index RXINDEX bytes into RXPACKET movwf FSR ; and dereference movf ITEMP7, W movwf INDF ; load into RXPACKET ; now we need to examine the packet and decide what to do next incf RXINDEX, F movf RXINDEX, W sublw 0x02 ; btfsc STATUS, C ; goto usart_rx_handler_incomplete ; ...in which case we wait for more. ; ok, there's at least 3 bytes so we have received the length nibble movf RXPFLAGLEN, W andlw 0x0F ; kill the flags addlw 0x04 ; adjust length for header and checksum subwf RXINDEX, W ; RXINDEX - LENGTH ; zero: packet complete ; negative: packet incomplete (carry is clear) ; positive: packet too long. (carry is set) ; positive should be impossible to achieve btfss STATUS, Z goto usart_rx_handler_incomplete ; packet is complete. ; check if tofrom is correct movlw TOFROM_RX xorwf RXPTOFROM, W btfss STATUS, Z ; if Z is cleared... goto usart_rx_handler_reset ; ...packet is not valid. ; check if the sum is correct movlw RXPACKET movwf FSR call packetsum ; W now contains the checksum, FSR points to the advertised checksum xorwf INDF, W ; compare the checksums btfss STATUS, Z goto usart_rx_handler_reset ; bad checksum. ; packet is properly formatted. ; if it is an ack, recognize it ; but only if we're looking for one btfss PBFLAGS, TXPA goto usart_rx_handler_norecack ; get num of last packet sent swapf TXPNUMACK, W xorwf RXPNUMACK, W andlw 0x0F ; throw away wrong nibble ; if this is zero, they match btfss STATUS, Z goto usart_rx_handler_norecack ; they match bcf PBFLAGS, TXPA ; not looking for another ack bcf PBFLAGS, TXPHOLD ; not holding anymore bcf PBFLAGS, TXPF ; buffer now empty usart_rx_handler_norecack: ; now decide whether we should process the data ; if its number is nonzero, ack and process it swapf RXPNUMACK, W andlw 0x0F btfsc STATUS, Z goto usart_rx_handler_reset ; ok, packet can be processed. ; acknowledge it, using the secondary tx buffer. ; this needs to be done sometime ; also, it might be used to tell if reception is in progress clrf RXINDEX swapf RXPTOFROM, W ; switch to/from movwf TXAPTOFROM swapf RXPNUMACK, W ; ack = num, num = 0 andlw 0x0F movwf TXAPNUMACK movlw 0x00 movwf TXAPFLAGLEN ; flag = 0, len = 0 movlw TXAPACKET movwf FSR call packetsum movwf INDF bsf PBFLAGS, TXAPF ; send it off ; turn on tx int bsf STATUS, RP0 bsf PIE1, TXIE bcf STATUS, RP0 bsf PBFLAGS, RXPF ; set the full flag. ; blink green ; movlw 0x30 ; movwf GREEN ; packet should now be processed by the main event loop. goto usart_rx_handler_done usart_rx_handler_incomplete: ; nothing to do. goto usart_rx_handler_done usart_rx_handler_reset: ; we're here because the packet is bad in some way. ; so, make like it never happened. clrf RXINDEX ; reset index bcf PBFLAGS, RXPF ; not full any more. ; is this really all that needs to be done? goto usart_rx_handler_done usart_rx_handler_done: return delay: bcf PIR1, TXIF delay2: btfss PIR1, TXIF goto delay2 bcf PIR1, TXIF return usart_tx_handler: ; handles tx interrupts ; TXLEN, TXPTR movf TXLEN, F btfsc STATUS, Z goto usart_tx_handler_txdone movf TXPTR, W movwf FSR movf INDF, W movwf TXREG incf TXPTR, F decf TXLEN, F goto usart_tx_handler_done usart_tx_handler_txdone: ; current transmit (if existent) is done bcf COMSTAT, TXING ; not transmitting anymore ; see if there's anything else to transmit btfsc PBFLAGS, TXAPF ; ack has priority goto usart_tx_handler_sendack btfsc PBFLAGS, TXPF goto usart_tx_handler_sendmain goto usart_tx_handler_nomoretx ; we know TXING is clear, otherwise we wouldn't be here usart_tx_handler_sendack: bsf COMSTAT, TXING bcf PBFLAGS, TXAPF ; don't wait for this one movlw (TXAPACKET+1) movwf TXPTR movlw 0x03 movwf TXLEN movf (TXAPACKET+0), W movwf TXREG goto usart_tx_handler_done usart_tx_handler_sendmain: movlw 0x20 movwf YELLOW ; we might be here because a new packet is ready ; we might be here because we're retrying btfsc PBFLAGS, TXPA goto usart_tx_handler_resend ; so this packet hasn't been tried yet. call preppacket ; set header and checksum bsf PBFLAGS, TXPA ; has been attempted now goto usart_tx_handler_sendmainnow usart_tx_handler_resend: ; it might not be time yet btfsc PBFLAGS, TXPHOLD goto usart_tx_handler_nomoretx ; turn off for now ; so we need to retransmit ; fortunetely the packet is ready to go usart_tx_handler_sendmainnow: movlw RESENDTIMEOUT_INITIAL ; set timeout movwf RESENDTIMEOUT bsf PBFLAGS, TXPHOLD ; don't want to resend immediately movlw (TXPACKET+1) movwf TXPTR movf TXPFLAGLEN, W andlw 0x0F addlw 0x03 ; (header + data + checksum) - 1 movwf TXLEN movf (TXPACKET+0), W movwf TXREG goto usart_tx_handler_done usart_tx_handler_nomoretx: bsf STATUS, RP0 bcf PIE1, TXIE bcf STATUS, RP0 goto usart_tx_handler_done usart_tx_handler_done: return ; preppacket prepares TXPACKET for transmission preppacket: ; set tofrom movlw TOFROM_TX movwf TXPTOFROM ; set numack swapf NEXTNUM, W ; low bits should be zero movwf TXPNUMACK ; update NEXTNUM incf NEXTNUM, F movlw 0x01 btfsc NEXTNUM, 4 ; if NEXTNUM == 16 movwf NEXTNUM ; set NEXTNUM = 1 ; flaglen is set in userland ; checksum movlw TXPACKET movwf FSR call packetsum movwf INDF return ; packetsum ; precondition: FSR contains a packet pointer ; postcondition: W contains the real checksum ; FSR points to the advertised checksum packetsum: incf FSR, F ; skip to len incf FSR, F movf INDF, W andlw 0x0F ; only the lower nibble is the length addlw 0x03 ; header too movwf PACKETSUM_INDEX decf FSR, F ; go back decf FSR, F ; PACKETSUM_INDEX contains the total length movlw 0xA5 ; initialize xor sum ; we use this so that noise that causes all zero ; or all one things don't have good checksums packetsum_one: xorwf INDF, W ; add one byte incf FSR, F ; advance the pointer decfsz PACKETSUM_INDEX, F goto packetsum_one return ; org 0x0800 startmain: ; initialization code bsf STATUS, RP0 ; code page 1 ; tri-state initialization ; all input movlw 0xff movwf TRISA movwf TRISB movlw 0xb8 movwf TRISC ; all output movlw 0x00 movwf TRISD ; still on bank 1 ; interrupt initialization ; eventually all these are set bsf INTCON, PEIE bsf INTCON, T0IE ; bsf INTCON, INTE bsf INTCON, RBIE bsf PIE1, ADIE bsf PIE1, RCIE ; bsf PIE1, TXIE ; this one waits for something to transmit bsf PIE1, TMR2IE bsf PIE1, TMR1IE ; bsf INTCON, GIE ; save this for last ; still on bank 1 ; UART initialization ; movlw 0xff ; 1200 baud movlw 0x81 ; 2400 baud movwf SPBRG bcf TXSTA, SYNC ; asynchronous bcf TXSTA, BRGH ; low baud bsf TXSTA, TXEN ; transmit when TXREG filled bcf STATUS, RP0 ; register bank 0 bsf RCSTA, SPEN ; serial enable bsf RCSTA, CREN ; don't know about this ; adc initialization bsf STATUS, RP0 movlw 0x01 movwf ADCON1 bcf STATUS, RP0 movlw 0x81 movwf ADCON0 ; communication system initialization bcf COMSTAT, TXING clrf PBFLAGS clrf RXINDEX movlw 0x01 movwf NEXTNUM ; still on bank 0 ; timer initialization ; timer 0, portb pullups, int edge... ; option = 11000101 bsf STATUS, RP0 ; register bank 1 movlw 0xC5 movwf OPTION_REG bcf STATUS, RP0 ; register bank 0 ; timer 1 movlw 0x31 ; 8:1 prescale xx110001 movwf T1CON ; timer 2 (pwm and servo clock) movlw 0xff ; 16:1 prescale, 16:1 postscale movwf T2CON ; this means 1.22 khz pwm and 76hz int movlw 0x0C movwf CCP1CON movwf CCP2CON movlw 0x00 movwf CCPR1L ; stop right motor movwf CCPR2L ; stop left motor bcf CONDITION, MOVING ; not moving bsf CONDITION, MANUAL ; turn off auto update movlw 0x80 movwf SERVOPOS ; center sensor array bsf PORTD, 5 ; sensor power on. bsf PORTD, 6 ; drive power on. bsf PORTD, 7 ; encoder power on. ; system initialization complete. bsf INTCON, GIE ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; maineventloop: ; if there's a packet, process it btfsc PBFLAGS, RXPF call processpacket goto maineventloop processpacket: ; check for null packet movf RXPFLAGLEN, W andlw 0x0F btfsc STATUS, Z call p_handler_nullpacket movlw P_SETPWM xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_setpwm movlw P_SENDADCRES xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_sendadcres movlw P_SENDSPEED xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_sendspeed movlw P_SENDLEFTENC xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_sendleftenc movlw P_SENDTICKCOUNT xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_sendtickcount movlw P_PING xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_ping movlw P_SETPOWER xorwf (RXPDATA+0), W btfsc STATUS, Z call p_handler_setpower bcf PBFLAGS, RXPF ; done with packet return p_handler_ping: movlw 0x20 movwf YELLOW bsf PORTD, 4; ; need to wait 50 usec now movlw 0xff ; that's way more movwf MTEMP0 p_handler_ping_wait: nop nop nop nop nop nop decfsz MTEMP0, F goto p_handler_ping_wait bcf PORTD, 4 return p_handler_nullpacket: return p_handler_setpwm: btfsc (RXPDATA+1),0 bsf PORTD, 0 btfss (RXPDATA+1),0 bcf PORTD, 0 btfsc (RXPDATA+1),1 bsf PORTD, 1 btfss (RXPDATA+1),1 bcf PORTD, 1 ; set servo movf (RXPDATA+4), W movwf SERVOPOS btfss (RXPDATA+1),2 goto p_handler_setpwm_raw bsf CONDITION, MOVING bcf CONDITION, MANUAL ; set timed mode parameters movf (RXPDATA+2), W bsf STATUS, RP0 movwf LTS bcf STATUS, RP0 movf (RXPDATA+3), W bsf STATUS, RP0 movwf RTS bcf STATUS, RP0 movlw 0x0A movwf T1POST return p_handler_setpwm_raw: bsf CONDITION, MANUAL movf (RXPDATA+2), W movwf CCPR1L movf (RXPDATA+3), W movwf CCPR2L iorwf CCPR1L, W btfsc STATUS, Z bsf CONDITION, MOVING btfss STATUS, Z bcf CONDITION, MOVING return p_handler_sendadcres: movlw 0x20 movwf GREEN movlw P_SENDADCRES movwf (TXPDATA+0) movf (ADCOUTPUT+0), W movwf (TXPDATA+1) movf (ADCOUTPUT+1), W movwf (TXPDATA+2) movf (ADCOUTPUT+2), W movwf (TXPDATA+3) movf (ADCOUTPUT+3), W movwf (TXPDATA+4) movf (ADCOUTPUT+4), W movwf (TXPDATA+5) movf (ADCOUTPUT+5), W movwf (TXPDATA+6) movf (ADCOUTPUT+6), W movwf (TXPDATA+7) movf (ADCOUTPUT+7), W movwf (TXPDATA+8) movlw 0x09 movwf TXPFLAGLEN bsf PBFLAGS, TXPF ; send bsf STATUS, RP0 bsf PIE1, TXIE bcf STATUS, RP0 return p_handler_sendspeed: movlw P_SENDSPEED movwf (TXPDATA+0) bsf STATUS, RP0 movf LTS, W bcf STATUS, RP0 movwf (TXPDATA+1) bsf STATUS, RP0 movf LSTCL, W bcf STATUS, RP0 movwf (TXPDATA+2) movf CCPR1L, W movwf (TXPDATA+3) bsf STATUS, RP0 movf RTS, W bcf STATUS, RP0 movwf (TXPDATA+4) bsf STATUS, RP0 movf RSTCL, W bcf STATUS, RP0 movwf (TXPDATA+5) movf CCPR2L, W movwf (TXPDATA+6) movlw 0x07 movwf TXPFLAGLEN bsf PBFLAGS, TXPF bsf STATUS, RP0 bsf PIE1, TXIE bcf STATUS, RP0 return p_handler_sendleftenc: movlw LENCODER movwf FSR movf INDF, W movwf (TXPDATA+0) incf FSR, F movf INDF, W movwf (TXPDATA+1) incf FSR, F movf INDF, W movwf (TXPDATA+2) incf FSR, F movf INDF, W movwf (TXPDATA+3) incf FSR, F movf INDF, W movwf (TXPDATA+4) incf FSR, F movf INDF, W movwf (TXPDATA+5) incf FSR, F movlw 0x06 movwf TXPFLAGLEN bsf PBFLAGS, TXPF bsf STATUS, RP0 bsf PIE1, TXIE bcf STATUS, RP0 return p_handler_sendtickcount: movlw P_SENDTICKCOUNT movwf (TXPDATA+0) bsf STATUS, RP0 movf LTCH, W clrf LTCH bcf STATUS, RP0 movwf (TXPDATA+1) bsf STATUS, RP0 movf LTCL, W clrf LTCL bcf STATUS, RP0 movwf (TXPDATA+2) bsf STATUS, RP0 movf RTCH, W clrf RTCH bcf STATUS, RP0 movwf (TXPDATA+3) bsf STATUS, RP0 movf RTCL, W clrf RTCL bcf STATUS, RP0 movwf (TXPDATA+4) movlw 0x05 movwf TXPFLAGLEN bsf PBFLAGS, TXPF bsf STATUS, RP0 bsf PIE1, TXIE bcf STATUS, RP0 return p_handler_setpower: ; set drive power btfsc (RXPDATA+1),0 bsf PORTD, 6 btfss (RXPDATA+1),0 bcf PORTD, 6 ; set encoder power btfsc (RXPDATA+1),1 bsf PORTD, 7 btfss (RXPDATA+1),1 bcf PORTD, 7 ; set sensor power btfsc (RXPDATA+1),2 bsf PORTD, 5 btfss (RXPDATA+1),2 bcf PORTD, 5 ; ; set radio power ; btfsc (RXPDATA+1),4 ; bsf PORTC, 5 ; btfss (RXPDATA+1),4 ; bcf PORTC, 5 return org 0x0800 nop end