📄 head_control_module.asm
字号:
subwf MaxChangePerPeriod,w ; w = MaxChangePerPeriod - w btfss STATUS,C ; Skip if w >= 0 goto PanErrorGreaterThanMaxChange2; MaxChangePerPeriod >= Error movf PanSetpoint,w movwf PanActual goto DonePanPanErrorGreaterThanMaxChange2; Error is greater than MaxChangePerPeriod movf MaxChangePerPeriod,w subwf PanActual,f ; PanActual -= MaxChangePerPeriod goto DonePanDonePan movf TiltActual,w subwf TiltSetpoint,w ; w = TiltSetpoint - TiltActual btfsc STATUS,Z ; Skip if w != 0 goto DoneTilt; Tilt actual position not same as target/setpoint btfss STATUS,C ; Skip if w >= 0 goto TiltGreaterThanSetpoint ; w < 0; TiltActual < TiltSetpoint subwf MaxChangePerPeriod,w ; w = MaxChangePerPeriod - w btfss STATUS,C ; Skip if w >= 0 goto TiltErrorGreaterThanMaxChange1; MaxChangerPerPeriod >= Error movf TiltSetpoint,w movwf TiltActual goto DonePanTiltErrorGreaterThanMaxChange1; Error is greater than MaxChangerPerPeriod movf MaxChangePerPeriod,w addwf TiltActual,f ; TiltActual += MaxChangePerPeriod goto DoneTiltTiltGreaterThanSetpoint; TiltActual > TiltSetpoint movf TiltSetpoint,w subwf TiltActual,w ; w = TiltActual - TiltSetpoint subwf MaxChangePerPeriod,w ; w = MaxChangePerPeriod - w btfss STATUS,C ; Skip if w >= 0 goto TiltErrorGreaterThanMaxChange2; MaxChangePerPeriod >= Error movf TiltSetpoint,w movwf TiltActual goto DoneTiltTiltErrorGreaterThanMaxChange2; Error is greater than MaxChangePerPeriod movf MaxChangePerPeriod,w subwf TiltActual,f ; TiltActual -= MaxChangePerPeriod DoneTilt return; ----------------------------------------------------------------------LoadSettings ; Read the configuration settings from EEPROM and store them in variables call GetCheckValueEeprom sublw b'11001101' btfss STATUS,Z goto SetDefaults ; Check number is wrong - use defaults call GetMaxChangePerPeriodEeprom movwf MaxChangePerPeriod call GetIRSensorNeutralEeprom movwf IRSensorNeutral call GetIRTriggerThresholdEeprom movwf IRSensorTriggerThreshold call GetIRTriggeredEventModeEeprom movwf IRTriggeredEventMode returnSetDefaults ; Check value was corrupted - use defaults movlw 2 movwf MaxChangePerPeriod call WriteMaxChangePerPeriodEeprom movlw 128 movwf IRSensorNeutral call WriteIRSensorNeutralEeprom movlw 20 movwf IRSensorTriggerThreshold call WriteIRTriggerThresholdEeprom movlw 3 movwf IRTriggeredEventMode call WriteIRTriggeredEventModeEeprom movlw b'11001101' call WriteCheckValueEeprom return; ----------------------------------------------------------------------; Global interrupts are assumed to be disabled before this subroutine is calledWriteCheckValueEeprom bsf STATUS,RP1 bcf STATUS,RP0 movwf EEDATA^0x100 movlw CHECK_VALUE_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,WREN movlw 0x055 movwf EECON2^0x180 movlw 0x0AA movwf EECON2^0x180 bsf EECON1^0x180,WR btfsc EECON1^0x180,WR goto $-1 bcf EECON1^0x180,WREN bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------; Global interrupts are assumed to be disabled before this subroutine is calledWriteMaxChangePerPeriodEeprom bsf STATUS,RP1 bcf STATUS,RP0 movwf EEDATA^0x100 movlw MAX_CHANGE_PER_PERIOD_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,WREN movlw 0x055 movwf EECON2^0x180 movlw 0x0AA movwf EECON2^0x180 bsf EECON1^0x180,WR btfsc EECON1^0x180,WR goto $-1 bcf EECON1^0x180,WREN bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------; Global interrupts are assumed to be disabled before this subroutine is calledWriteIRSensorNeutralEeprom bsf STATUS,RP1 bcf STATUS,RP0 movwf EEDATA^0x100 movlw IR_SENSOR_NEUTRAL_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,WREN movlw 0x055 movwf EECON2^0x180 movlw 0x0AA movwf EECON2^0x180 bsf EECON1^0x180,WR btfsc EECON1^0x180,WR goto $-1 bcf EECON1^0x180,WREN bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------; Global interrupts are assumed to be disabled before this subroutine is calledWriteIRTriggerThresholdEeprom bsf STATUS,RP1 bcf STATUS,RP0 movwf EEDATA^0x100 movlw IR_TRIGGER_THRESHOLD_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,WREN movlw 0x055 movwf EECON2^0x180 movlw 0x0AA movwf EECON2^0x180 bsf EECON1^0x180,WR btfsc EECON1^0x180,WR goto $-1 bcf EECON1^0x180,WREN bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------; Global interrupts are assumed to be disabled before this subroutine is calledWriteIRTriggeredEventModeEeprom bsf STATUS,RP1 bcf STATUS,RP0 movwf EEDATA^0x100 movlw IR_TRIGGERED_EVENT_MODE_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,WREN movlw 0x055 movwf EECON2^0x180 movlw 0x0AA movwf EECON2^0x180 bsf EECON1^0x180,WR btfsc EECON1^0x180,WR goto $-1 bcf EECON1^0x180,WREN bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------GetCheckValueEeprom bsf STATUS,RP1 bcf STATUS,RP0 movlw CHECK_VALUE_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,RD bsf STATUS,RP1 bcf STATUS,RP0 movf EEDATA^0x100,w bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------GetMaxChangePerPeriodEeprom bsf STATUS,RP1 bcf STATUS,RP0 movlw MAX_CHANGE_PER_PERIOD_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,RD bsf STATUS,RP1 bcf STATUS,RP0 movf EEDATA^0x100,w bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------GetIRSensorNeutralEeprom bsf STATUS,RP1 bcf STATUS,RP0 movlw IR_SENSOR_NEUTRAL_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,RD bsf STATUS,RP1 bcf STATUS,RP0 movf EEDATA^0x100,w bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------GetIRTriggerThresholdEeprom bsf STATUS,RP1 bcf STATUS,RP0 movlw IR_TRIGGER_THRESHOLD_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,RD bsf STATUS,RP1 bcf STATUS,RP0 movf EEDATA^0x100,w bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------GetIRTriggeredEventModeEeprom bsf STATUS,RP1 bcf STATUS,RP0 movlw IR_TRIGGERED_EVENT_MODE_EE_ADDR movwf EEADR^0x100 bsf STATUS,RP0 bsf STATUS,RP1 bcf EECON1^0x180,EEPGD bsf EECON1^0x180,RD bsf STATUS,RP1 bcf STATUS,RP0 movf EEDATA^0x100,w bcf STATUS,RP0 bcf STATUS,RP1 return; ----------------------------------------------------------------------ReadIR movlw b'01000001' ; Select AN0 movwf ADCON0; Wait required acquisition time call ADCDelay bsf ADCON0,GO_DONE ; Start conversion btfsc ADCON0,GO_DONE goto $-1 movf ADRESH,w ; Get result movwf RawAnalogIR movlw b'01000000' ; Shut off ADC movwf ADCON0 return; ----------------------------------------------------------------------ADCDelay ; 20 microsecond delay nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop return; ----------------------------------------------------------------------InitClock bsf STATUS,RP0 bcf STATUS,RP1 movlw 0x070 ; Select 8MHz internal clock. movwf OSCCON^0x080 bcf STATUS,RP0 return; ----------------------------------------------------------------------InitInterrupts; Enable I2C interrupt. bsf STATUS,RP0 bcf STATUS,RP1 bsf PIE1^0x080,SSPIE ; Enable I2C interrupt bcf STATUS,RP0 bsf INTCON,PEIE ; Enable unmasked peripheral interrupts bsf INTCON,GIE ; Global Interrupt Enable return; ----------------------------------------------------------------------InitTimer; Set timer 0 to 8us period bsf STATUS,RP0 bcf STATUS,RP1 bcf OPTION_REG^0x080,T0CS ; Select Timer 0 Mode bcf OPTION_REG^0x080,PSA ; Assign prescaler to Timer 0 bsf OPTION_REG^0x080,PS0 bsf OPTION_REG^0x080,PS1 bcf OPTION_REG^0x080,PS2 ; Set timer 0 prescaler bcf STATUS,RP0; Set timer 1 to 1us period bsf T1CON,T1CKPS0 bcf T1CON,T1CKPS1 ; Set timer 1 prescaler bcf T1CON,T1OSCEN ; Disable built-in oscillator bcf T1CON,TMR1CS ; Use Fosc/4 as source for timer 1 bsf T1CON,TMR1ON ; Turn on timer 1; Set timer 2 to 8us period movlw b'00000111' movwf T2CON; Set up CCP for Compare movlw HIGH(65536-SERVO_CONTROL_PERIOD_MICROSECOND+SERVO_ALWAYS_ON_TIME_MICROSECOND) movwf CCPR1H movlw LOW(65536-SERVO_CONTROL_PERIOD_MICROSECOND+SERVO_ALWAYS_ON_TIME_MICROSECOND) movwf CCPR1L movlw b'00001010' movwf CCP1CON return; ----------------------------------------------------------------------InitIO; Set port A and B data directions clrf PORTA clrf PORTB bsf STATUS,RP0 bcf STATUS,RP1 movlw b'00100001' ; Set Port A data direction bits. movwf TRISA^0x080 movlw b'00010010' ; Set Port B data direction bits. movwf TRISB^0x080 bcf STATUS,RP0 return; ----------------------------------------------------------------------InitI2C clrf SMBusState bcf SSPCON,SSPM3 ; SSPM<3:0> = 0110 bsf SSPCON,SSPM2 ; (7-bit address slave mode) bsf SSPCON,SSPM1 bcf SSPCON,SSPM0 bsf SSPCON,CKP ; SCK release control: Enable Clock bcf SSPCON,SSPOV ; Make sure overflow bit is clear movf SSPBUF,w ; Read SSPBUF to ensure buffer flag BF is clear movlw I2C_ADDRESS << 1 bsf STATUS,RP0 bcf STATUS,RP1 movwf SSPADD^0x80 ; I2C slave address of this device bcf STATUS,RP0 bsf SSPCON,SSPEN ; Enable I2C return; ----------------------------------------------------------------------InitADC bsf STATUS,RP0 bcf STATUS,RP1 movlw b'00001110' ; Only RA0/AN0 input is analog (others digital) movwf ADCON1^0x80 bcf STATUS,RP0 return; ----------------------------------------------------------------------; End end⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -