📄 head_control_module.asm
字号:
addwf PCL,f ; SMBus state jump table goto I2CState3SMBusState0 goto I2CState3SMBusState1 goto I2CState3SMBusState2 IF ((HIGH($)) != (HIGH($-3))) ERROR("Table crosses page boundary!") ENDIFI2CState3SMBusState0 clrf SSPBUF ; Shouldn't be here, but the master is bsf SSPCON,CKP ; expecting data, so we must oblige. goto FinishISR I2CState3SMBusState1 movf SMBusCommand,w sublw READ_PAN_SERVO_SETPOINT_COMMAND btfss STATUS,Z goto NotReadPanSetpoint; Read Pan setpoint movf PanSetpoint,w ; Pan setpoint movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadPanSetpoint movf SMBusCommand,w sublw READ_PAN_SERVO_ACTUAL_COMMAND btfss STATUS,Z goto NotReadPanActual; Read Pan Servo Actual movf Timer,w movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock movf PanActual,w ; Pan actual movwf SecondDataByte ; Store value of timer movlw 2 movwf SMBusState ; SMBus state 2nd data byte next goto FinishISRNotReadPanActual movf SMBusCommand,w sublw READ_TILT_SERVO_SETPOINT_COMMAND btfss STATUS,Z goto NotReadTiltSetpoint; Read Tilt setpoint movf TiltSetpoint,w ; Tilt setpoint movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadTiltSetpoint movf SMBusCommand,w sublw READ_TILT_SERVO_ACTUAL_COMMAND btfss STATUS,Z goto NotReadTiltActual; Read Tilt Actual movf Timer,w movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock movf TiltActual,w ; TiltActual movwf SecondDataByte ; Store value of timer movlw 2 movwf SMBusState ; SMBus state 2nd data byte next goto FinishISRNotReadTiltActual movf SMBusCommand,w sublw READ_MAX_CHANGE_PER_PERIOD btfss STATUS,Z goto NotReadMaxChangePerPeriod; Read Max Change Per Period setting movf MaxChangePerPeriod,w ; MaxChangePerPeriod movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command nextNotReadMaxChangePerPeriod movf SMBusCommand,w sublw READ_RAW_ANALOG_IR_SENSOR btfss STATUS,Z goto NotReadRawAnalog; Read raw analog movf RawAnalogIR,w ; Switch state movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadRawAnalog movf SMBusCommand,w sublw READ_IR_SENSOR_NEUTRAL_VALUE btfss STATUS,Z goto NotReadIRSensorNeutral; Read IR Sensor Neutral movf IRSensorNeutral,w ; IR sensor neutral setting movwf SSPBUF ; Put transmit data in buffer bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRSensorNeutral movf SMBusCommand,w sublw READ_IR_SENSOR_TRIGGER_THRESHOLD btfss STATUS,Z goto NotReadIRSensorTriggerThreshold; Read IR Sensor Trigger Threshold movf IRSensorTriggerThreshold,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRSensorTriggerThreshold movf SMBusCommand,w sublw READ_IR_EVENT_PAN_ACTUAL btfss STATUS,Z goto NotReadIRTriggeredEventPan; Read IR Triggered Event Pan movf IRTriggeredEventPan,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRTriggeredEventPan movf SMBusCommand,w sublw READ_IR_EVENT_STATUS btfss STATUS,Z goto NotReadIRTriggeredEventStatus; Read IR Triggered Event Status movf IRTriggeredEventStatus,w movwf SSPBUF btfsc STATUS,Z goto NoEventPending; event stored bsf IRTriggeredEventLocked,0 ; Lock event NoEventPending bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRTriggeredEventStatus movf SMBusCommand,w sublw READ_IR_EVENT_MODE btfss STATUS,Z goto NotReadIRTriggeredEventMode; Read IR Triggered Event mode movf IRTriggeredEventMode,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRTriggeredEventMode movf SMBusCommand,w sublw READ_IR_EVENT_ANALOG btfss STATUS,Z goto NotReadIRTriggeredEventAnalog; Read IR Triggered Event Analog movf IRTriggeredEventADC,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRTriggeredEventAnalog movf SMBusCommand,w sublw READ_IR_EVENT_TIMER_COUNT btfss STATUS,Z goto NotReadIRTriggeredEventTimer; Read IR Triggered Event timer movf IRTriggeredEventTimer,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadIRTriggeredEventTimer movf SMBusCommand,w sublw READ_CURRENT_TIMER_COUNT_COMMAND btfss STATUS,Z goto NotReadCurrentTimer; Read Current Timer movf Timer,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadCurrentTimer movf SMBusCommand,w sublw READ_PWM_OUTPUT_STATUS_COMMAND btfss STATUS,Z goto NotReadOutputStatus; Read PWM Output Status bsf STATUS,RP0 btfss PIE1^0x080,TMR1IE goto OutputNotEnabled; Output Enabled bcf STATUS,RP0 movlw 1 movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISROutputNotEnabled bcf STATUS,RP0 clrf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadOutputStatus clrf SSPBUF ; Shouldn't be here, but the master is bsf SSPCON,CKP ; expecting data, so we must oblige. goto FinishISR I2CState3SMBusState2 clrf SSPBUF ; Shouldn't be here, but the master is bsf SSPCON,CKP ; expecting data, so we must oblige. goto FinishISR I2CNotState3 movlw b'00101100' ; I2C state 4 (Read, last byte data)? subwf SspStatSaved,w btfss STATUS,Z goto I2CNotState4 ; No; Read, last byte data (I2C State 4) movlw HIGH($+4) movwf PCLATH movf SMBusState,w addwf PCL,f ; SMBus state jump table goto I2CState4SMBusState0 goto I2CState4SMBusState1 goto I2CState4SMBusState2 IF ((HIGH($)) != (HIGH($-3))) ERROR("Table crosses page boundary!") ENDIFI2CState4SMBusState0 clrf SSPBUF ; Shouldn't be here, but the master is bsf SSPCON,CKP ; expecting data, so we must oblige. goto FinishISR I2CState4SMBusState1 clrf SSPBUF ; Shouldn't be here, but the master is bsf SSPCON,CKP ; expecting data, so we must oblige. goto FinishISR I2CState4SMBusState2 movf SMBusCommand,w sublw READ_PAN_SERVO_ACTUAL_COMMAND btfss STATUS,Z goto NotReadPanActual2; Read Pan Actual movf SecondDataByte,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadPanActual2 movf SMBusCommand,w sublw READ_TILT_SERVO_ACTUAL_COMMAND btfss STATUS,Z goto NotReadTiltActual2; Read Tilt Actual movf SecondDataByte,w movwf SSPBUF bsf SSPCON,CKP ; SCK release control: Enable Clock clrf SMBusState ; SMBus state is expecting command next goto FinishISRNotReadTiltActual2 clrf SSPBUF ; Shouldn't be here, but the master is bsf SSPCON,CKP ; expecting data, so we must oblige. goto FinishISR I2CNotState4 movlw b'00101000' ; Nack received from master after reading data? subwf SspStatSaved,w btfss STATUS,Z goto FinishISR ; No. Impossible state.; Nack received from master (I2C State 5) clrf SMBusState ; SMBus state is expecting command next goto FinishISR ; Nothing further to doFinishISR retfie; ----------------------------------------------------------------------; Main Program start pointMainStart call InitClock ; Initialize internal clock oscillator call LoadSettings call InitADC ; Initialize the ADC call InitIO ; Initialize IO ports call InitTimer ; Initialize Timer call InitI2C ; Initialize the I2C interface ; Initialize variables clrf Timer movlw 128 ; Initialize both servos dead centre movwf PanSetpoint movwf TiltSetpoint movwf PanActual movwf TiltActual clrf RawAnalogIR clrf IRTriggeredEventStatus clrf IRTriggeredEventLocked clrf IRTriggeredEventADC clrf IRTriggeredEventTimer clrf IRTriggeredEventPan clrf IRTriggeredEventMaxThreshold clrf IRDeviationFromNeutral call InitInterrupts ; Enable interrupts goto $ ; Loop forever; ----------------------------------------------------------------------; Subroutines; ----------------------------------------------------------------------ResetEvent clrf IRTriggeredEventLocked clrf IRTriggeredEventStatus clrf IRTriggeredEventADC clrf IRTriggeredEventTimer clrf IRTriggeredEventPan return; ----------------------------------------------------------------------SenseIR movf IRTriggeredEventMode,w btfss STATUS,Z goto ModeNotZero; Mode 0 - IR disabled goto FinishSenseIRModeNotZero; IR is enabled, so take a reading call ReadIR movf IRTriggeredEventLocked,w btfsc STATUS,Z goto NotLocked; IR event locked goto FinishSenseIRNotLocked movf IRTriggeredEventMode,w sublw 1 btfss STATUS,Z goto ModeNotOne; Mode 1 - first event movf IRTriggeredEventStatus,w btfss STATUS,Z goto FinishSenseIR ; Event already stored; No event stored goto StoreIREventModeNotOne movf IRTriggeredEventMode,w sublw 2 btfss STATUS,Z goto ModeNotTwo; Mode 2 - last eventStoreIREvent movf RawAnalogIR,w subwf IRSensorNeutral,w ; w = IRSensorNeutral - RawAnalogIR btfsc STATUS,C ; skip if w is negative goto ADCLessThanOrEqualToNeutral ; w is positive; ADC count greater than neutral movf IRSensorNeutral,w subwf RawAnalogIR,w ; w = RawAnalogIR - IRSensorNeutralADCLessThanOrEqualToNeutral ; w is positive at this point movwf IRDeviationFromNeutral movf IRSensorTriggerThreshold,w subwf IRDeviationFromNeutral,w ; w = IRDeviationFromNeutral - IRSensorTriggerThreshold btfss STATUS,C goto FinishSenseIR ; Not reached trigger threshold; Reached trigger thresholdReachedThreshold movf IRDeviationFromNeutral,w movwf IRTriggeredEventMaxThreshold movf RawAnalogIR,w movwf IRTriggeredEventADC movf Timer,w movwf IRTriggeredEventTimer movf PanActual,w movwf IRTriggeredEventPan bsf IRTriggeredEventStatus,0 goto FinishSenseIRModeNotTwo movf IRTriggeredEventMode,w sublw 3 btfss STATUS,Z goto ModeNotThree; Mode 3 - maximum event movf IRTriggeredEventStatus,w btfsc STATUS,Z goto StoreIREvent ; Not yet stored event; An event is already stored movf RawAnalogIR,w subwf IRSensorNeutral,w btfsc STATUS,C goto ADCLessThanOrEqualToNeutralMode3; ADC count greater than neutral movf IRSensorNeutral,w subwf RawAnalogIR,wADCLessThanOrEqualToNeutralMode3 movwf IRDeviationFromNeutral movf IRTriggeredEventMaxThreshold,w subwf IRDeviationFromNeutral,w btfss STATUS,C goto FinishSenseIR ; Not reached max trigger threshold; Reached max trigger threshold goto ReachedThresholdModeNotThree ; Unrecognized modeFinishSenseIR return; ----------------------------------------------------------------------StartPWMCycle bcf T1CON,TMR1ON ; Stop timer 1 movlw HIGH(65536-SERVO_CONTROL_PERIOD_MICROSECOND) movwf TMR1H ; Set timer 1 to time out after 20ms movlw LOW(65536-SERVO_CONTROL_PERIOD_MICROSECOND) movwf TMR1L bcf PIR1,CCP1IF ; Clear any possible false interrupt. bsf T1CON,TMR1ON ; Restart timer 1 bsf PAN_SERVO ; Pan servo signal high bsf TILT_SERVO ; Tilt servo signal high bsf STATUS,RP0 bcf STATUS,RP1 bsf PIE1^0x080,CCP1IE ; Enable CCP interrupt bcf PIE1^0x080,TMR2IE ; Disable timer 2 interrupt bcf STATUS,RP0 bcf INTCON,TMR0IE ; Disable timer 0 interrupt return; ----------------------------------------------------------------------UpdateActuals movf PanActual,w subwf PanSetpoint,w ; w = PanSetpoint - PanActual btfsc STATUS,Z ; Skip if w != 0 goto DonePan; Pan actual position not same as target/setpoint btfss STATUS,C ; Skip if w >= 0 goto PanGreaterThanSetpoint ; w < 0; PanActual < PanSetpoint subwf MaxChangePerPeriod,w ; w = MaxChangePerPeriod - w btfss STATUS,C ; Skip if w >= 0 goto PanErrorGreaterThanMaxChange1; MaxChangerPerPeriod >= Error movf PanSetpoint,w movwf PanActual goto DonePanPanErrorGreaterThanMaxChange1; Error is greater than MaxChangerPerPeriod movf MaxChangePerPeriod,w addwf PanActual,f ; PanActual += MaxChangePerPeriod goto DonePanPanGreaterThanSetpoint; PanActual > PanSetpoint movf PanSetpoint,w subwf PanActual,w ; w = PanActual - PanSetpoint⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -