motor_control_module.asm

这个开发机器人项目源码

;    $Id: motor_control_module.asm,v 1.5 2003/10/08 02:45:20 dwalters Exp $
;    Copyright (C) 2003  Dafydd Walters <[e-mail]>
;
;    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 (COPYING.SOFTWARE); if not, write to the 
;    Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, 
;    MA  02111-1307  USA

; ----------------------------------------------------------------------
 title "motor_control_module.asm - OAP Motor Control Module"

 processor pic16f876
 list r=dec ; set radix to decimal
 include "p16f876.inc"

; ----------------------------------------------------------------------
; Digital Inputs:
;  RB0 - Right quadrature encoder A channel
;  RB7 - Left quadrature encoder A channel
;  RC5 - Right quadrature encoder B channel
;  RC6 - Left quadrature encoder B channel
;
; Digital Outputs:
;  RA2 - LMD18200 'pwm' signals
;  RA3 - LMD18200 'brake' signals
;  RC1/CCP2 - Left LMD18200 'direction' signal
;  RC2/CCP1 - Right LMD18200 'direction' signal
;  
; I2C (initially inputs):
;  RC3 - SCL
;  RC4 - SDA
;
; Not Used:
;  RA0
;  RA1
;  RA4
;  RA5
;  RB1
;  RB2
;  BR3
;  RB4
;  RB5
;  RB6
;  RC0
;  RC7
;
;
; I2C and SMBus:
; 
; The SMBus protocol commands are handled entirely in the interrupt handling
; code. The Motor Control Module appears as a slave device on the I2C bus.
; Each time an I2C interrupt is detected, the interrupt handler code
; determines which of the following 5 states the I2C interface is in:
; 
; - State1: Host wants to write (send) a byte to us, and has just sent us 
;           our device address. In this state, we must read (and discard) the
;           address from the receive buffer. The device does not need to take
;           any further action in response to this state. We should be seeing
;           State2 next.
;
; - State2: Host has just finished sending a byte to us. We must read the data
;           byte from the receive buffer, and act on it (what we do at this
;           point depends on whether we were expecting a command byte from the
;           host, or whether we had already recevied that, and are now waiting
;           for data from the host).
;
; - State3: Host wants to read a byte from us, and has just sent us our device
;           address. We must first read the address, and then write a byte to
;           the buffer (this would typically be a response to host's request
;           for data).
;
; - State4: Host has just read a byte from us, and is now expecting us to send
;           another byte. This state is for transfers of more than one byte
;
; - State5: Host has just read a byte from us, and does NOT expect us to send
;           any more. The device does not need to do anything in response to
;           this state.
;
; The interrupt handler also keeps track of which of the following states the
; SMBus transfers are in as follows:
;
; - State 0: Waiting for a command byte from the host.
;
; - State 1: Transferring a data byte (either reading or writing) to/from host
;            for a 'Read Byte Data' or 'Write Byte Data' sequence, or the
;            first data byte in a 'Write Word Data'/'Read Word Data' sequence.
;
; - State 2: Transferring 'number of data bytes' for a 'Block Read' sequence.
;
; - State 3: Transferring first data byte in a 'Block Read' sequence.
;
; - State 4: Transferring second data byte in a 'Block Read' sequence.
;
; - State 5: Transferring third data byte in a 'Block Read' sequence.
;
; - State 6: Transferring fourth data byte in a 'Block Read' sequence.
;
; - State 7: Transferring fifth data byte in a 'Block Read' sequence.
;
; - State 8: Transferring second data byte in a 'Write Word Data' or
;            'Read Word Data' sequence.
;

; We can sometimes set the SMBus state according to which state the I2C 
; interface is in.  For example, in response to I2C State1 or State5, we know
; that the next byte we receive from the host should be a SMBus command, so we
; set the SMBus state to 0.
;
; See the README document for more information on the SMBus commands that
; are implemented by this device.
;

;
; Timers:
;
; - TMR1 is used for driving the 8-bit free-running up counter
; - TMR2 is used for PWM output
;

; ----------------------------------------------------------------------
; Constants
#define I2C_ADDRESS 89
#define ENCODER_MASK_EE_ADDR 0
#define RIGHT_ENCODER_A PORTB,0
#define RIGHT_B 5                           ; Port C bit number only
#define LEFT_ENCODER_A PORTB,7
#define LEFT_B 6                            ; Port C bit number only
#define PWM PORTA,2
#define BRAKE PORTA,3
#define LEFT_DIR PORTC,1
#define RIGHT_DIR PORTC,2
; SMBus commands from Host:
#define GET_ODOMETRY_COMMAND 20             ; SMBus: Block Read 
#define GET_OUTPUT_STATE_COMMAND 21         ; SMBus: Read Byte Data 
#define GET_PWM_COMMAND 22                  ; SMBus: Read Word Data
#define GET_TIMER_COMMAND 23                ; SMBus: Read Byte Data 
#define GET_ENCODER_MASK_COMMAND 24         ; SMBus: Read Byte Data
#define WRITE_PWM_COMMAND 120               ; SMBus: Write Word Data
#define ACTIVATE_OUTPUT_COMMAND 121         ; SMBus: Send Byte
#define DEACTIVATE_OUTPUT_COMMAND 122       ; SMBus: Send Byte
#define ZERO_TIMER_COMMAND 123              ; SMBus: Send Byte
#define WRITE_ENCODER_MASK_COMMAND 124      ; SMBus: Write Byte Data

; ----------------------------------------------------------------------
; Variable Declarations (GPR) starting at data address 0x020
 cblock 0x020

Timer                           ; Up counter
LeftCounterLSB                  ; Left wheel odometer count least sig. byte
LeftCounterMSB                  ; Left wheel odometer count most sig. byte
RightCounterLSB                 ; Right wheel odometer count least sig. byte
RightCounterMSB                 ; Right wheel odometer count most sig. byte
SMBusCommand                    ; Most recent SMBus command from host
SMBusState                      ; Current SMBus state (see comments above)
I2CDataBuffer0                  ; 1st data byte of current data block transfer
I2CDataBuffer1                  ; 2nd data byte of current data block transfer
I2CDataBuffer2                  ; 3rd data byte of current data block transfer
I2CDataBuffer3                  ; 4th data byte of current data block transfer
I2CDataBuffer4                  ; 5th data byte of current data block transfer
I2CRxByte                       ; Most recent I2C byte received from host
SspStatSaved                    ; Temporary variable to hold SSPSTAT register
OutputState                     ; Output mode: 0=low power, 1=active
PortCTemp                       ; Temporary variable to hold Port C value
EncoderMask                     ; Port C mask byte
 endc

; ----------------------------------------------------------------------
; Configuration bits: 
;
; - Flash program memory code protection: Off
; - In-circuit debugger mode: disabled (RB6 and RB7 are I/O)
; - Flash program memory write enable: Off
; - Data EE memory code protection: Off
; - Low voltage programming: Off (RB3 is I/O)
; - Brown-out reset enable bit: Enabled
; - Power-up timer enabled: On
; - Watchdog timer: Off
; - Oscillator: HS (high speed)

 __CONFIG _CP_OFF & _DEBUG_OFF & _WRT_ENABLE_OFF & _CPD_OFF & _LVP_OFF & _BODEN_ON & _PWRTE_ON & _WDT_OFF & _HS_OSC

; ----------------------------------------------------------------------
; Program entry point at device reset
 org 0
 goto MainStart

; ----------------------------------------------------------------------
; Interrupt Handler entry point
 org 4

 btfss INTCON,RBIF              ; Has Left encoder A state gone Low to High?
 goto NotPortBChange
; Left quadrature encoder A channel went Low to High
 movf PORTB,w                   ; Read Port B to clear interrupt condition
 bcf INTCON,RBIF                ; Clear interrupt flag
 movf PORTC,w
 xorwf EncoderMask,w
 movwf PortCTemp                ; Save Port C value ASAP
 btfss PortCTemp,LEFT_B         ; B channel high?
 goto LeftBLow                  ; No
; Left quadrature encoder B channel is High
 movf LeftCounterLSB,f
 btfsc STATUS,Z
 decf LeftCounterMSB,f
 decf LeftCounterLSB,f          ; Decrement 16-bit counter
 goto FinishISR
LeftBLow
; Left quadrature encoder B channel is Low
 incf LeftCounterLSB,f          ; Increment 16-bit counter
 btfsc STATUS,Z 
 incf LeftCounterMSB,f
 goto FinishISR
NotPortBChange
 btfss INTCON,INTF              ; Right A channel gone high?
 goto NotIntPin
; Right quadrature encoder A channel has gone Low to High
 bcf INTCON,INTF                ; Clear interrupt flag
 movf PORTC,w
 xorwf EncoderMask,w
 movwf PortCTemp                ; Save Port C value ASAP
 btfss PortCTemp,RIGHT_B        ; B channel high?
 goto RightBLow                 ; No
; Right quadrature encoder B channel is High
 movf RightCounterLSB,f
 btfsc STATUS,Z
 decf RightCounterMSB,f
 decf RightCounterLSB,f         ; Decrement 16-bit counter
 goto FinishISR
RightBLow
; Right quadrature encoder B channel is Low
 incf RightCounterLSB,f         ; Increment 16-bit counter
 btfsc STATUS,Z 
 incf RightCounterMSB,f
 goto FinishISR
NotIntPin
 btfss PIR1,TMR1IF              ; Timer 1 ?
 goto NotTimer1
; Timer 1
 bcf PIR1,TMR1IF                ; Clear Timer 1 interrupt flag
 incf Timer,f                   ; Increment timer count
 goto FinishISR 
NotTimer1 
 btfss PIR1,SSPIF
 goto FinishISR
; I2C
 bcf PIR1,SSPIF                 ; Clear interrupt flag
 btfsc SSPCON,SSPOV             ; Check overflow bit
 bcf SSPCON,SSPOV               ; Make sure overflow bit is clear
 bsf STATUS,RP0                 ; Select Page 1
 movf SSPSTAT^0x080,w           ; Get I2C status (SSPSTAT register)
 bcf STATUS,RP0                 ; Back to page 0
 andlw b'00101101'              ; Mask for interesting bits in SSPSTAT
 movwf SspStatSaved             ; Save the result in a variable
 movlw b'00001001'              
 subwf SspStatSaved,w
 btfss STATUS,Z                 ; Is it I2C State 1 (Write, last byte address)?
 goto I2CNotState1
; Write, last byte is address (I2C State 1)
 movf SSPBUF,w                  ; Read receive buffer byte (clear BF)
 movwf I2CRxByte                ; Save received byte
 clrf SMBusState                ; Expecting SMBus command code next
 goto FinishISR                 ; Nothing more to do in this state
I2CNotState1
 movlw b'00101001'              ; Is it I2C State 2 (Write, last byte data)?
 subwf SspStatSaved,w
 btfss STATUS,Z
 goto I2CNotState2              ; No
; Write, last byte data (I2C State 2)
 movf SSPBUF,w                  ; Read receive buffer byte (clear BF)
 movwf I2CRxByte                ; Save received byte
 movlw HIGH($+4)
 movwf PCLATH
 movf SMBusState,w
 addwf PCL,f                    ; Jump table for SMBus state
 goto I2CState2SMBusState0
 goto I2CState2SMBusState1
 goto I2CState2SMBusState2
 goto I2CState2SMBusState3
 goto I2CState2SMBusState4
 goto I2CState2SMBusState5
 goto I2CState2SMBusState6
 goto I2CState2SMBusState7
 goto I2CState2SMBusState8
 IF ((HIGH($)) != (HIGH($-10)))
     ERROR("Table crosses page boundary!")
 ENDIF
I2CState2SMBusState0            ; SMBus State 0 (expecting command byte next)
 movf I2CRxByte,w
 movwf SMBusCommand             ; Save SMBus command code
 sublw ACTIVATE_OUTPUT_COMMAND
 btfss STATUS,Z
 goto NotActivateOutput
; Activate Output command (Send Byte)
 bsf OutputState,0
 bsf PWM
 bcf BRAKE
 clrf TMR2
 movlw b'00001100'              ; PWM mode
 movwf CCP1CON
 movwf CCP2CON
 clrf SMBusState
 goto FinishISR
NotActivateOutput
 movf SMBusCommand,w
 sublw DEACTIVATE_OUTPUT_COMMAND
 btfss STATUS,Z
 goto NotDeActivateOutput
; Deactivate output Command (Send Byte)
 clrf OutputState
 bcf PWM
 bsf BRAKE
 clrf CCP1CON
 clrf CCP2CON
 bcf LEFT_DIR
 bcf RIGHT_DIR
 clrf SMBusState
 goto FinishISR
NotDeActivateOutput
 movf SMBusCommand,w
 sublw ZERO_TIMER_COMMAND
 btfss STATUS,Z
 goto NotZeroTimer
; Zero Timer
 clrf Timer
 clrf SMBusState
 goto FinishISR
NotZeroTimer
 movf SMBusCommand,w
 sublw GET_ODOMETRY_COMMAND
 btfss STATUS,Z
 goto NotGetOdometryCommand
; Get odometry command
 movlw 2
 movwf SMBusState               ; Next SMBus state = 2
 goto FinishISR
NotGetOdometryCommand
 movlw 1
 movwf SMBusState               ; Next SMBus state = 1
 goto FinishISR

I2CState2SMBusState1            ; SMBus state 1 (transferring data byte)
 movf SMBusCommand,w
 sublw WRITE_PWM_COMMAND
 btfss STATUS,Z
 goto NotWritePWM
; First data byte of Write PWM command
 movf I2CRxByte,w
 movwf I2CDataBuffer0             ; Cache the Left PWM command 
 movlw 8
 movwf SMBusState               ; Next SMBus state is 8 (second data byte)
 goto FinishISR
NotWritePWM
 movf SMBusCommand,w
 sublw WRITE_ENCODER_MASK_COMMAND
 btfss STATUS,Z
 goto NotWriteEncoderMask
; Write XOR Encoder Mask
 movf I2CRxByte,w
 movwf EncoderMask
 call WriteEncoderMaskEEPROM
 clrf SMBusState
 goto FinishISR 
NotWriteEncoderMask
 clrf SMBusState         
 goto FinishISR

I2CState2SMBusState2            ; SMBus state 2 (transferring num data bytes)
 clrf SMBusState
 goto FinishISR

I2CState2SMBusState3            ; SMBus state 3 (1st data byte in block)
 clrf SMBusState
 goto FinishISR

I2CState2SMBusState4            ; SMBus state 4 (2nd data byte in block)
 clrf SMBusState