; FLAP1 LIST P=16C84, R=DEC include "p16c84.inc" ;-------------------------------------------------------------------------- ; Variables ;-------------------------------------------------------------------------- ScratchPadRam EQU 0x0C rInCH0CountH EQU ScratchPadRam+0 rInCH0CountL EQU ScratchPadRam+1 rInCH1CountH EQU ScratchPadRam+2 rInCH1CountL EQU ScratchPadRam+3 rInCH2CountH EQU ScratchPadRam+4 rInCH2CountL EQU ScratchPadRam+5 rInCH3CountH EQU ScratchPadRam+6 rInCH3CountL EQU ScratchPadRam+7 rOutCH0CountH EQU ScratchPadRam+8 rOutCH0CountL EQU ScratchPadRam+9 rOutCH1CountH EQU ScratchPadRam+10 rOutCH1CountL EQU ScratchPadRam+11 rOutCH2CountH EQU ScratchPadRam+12 rOutCH2CountL EQU ScratchPadRam+13 rOutCH3CountH EQU ScratchPadRam+14 rOutCH3CountL EQU ScratchPadRam+15 rAuxH EQU ScratchPadRam+16 rAuxL EQU ScratchPadRam+17 rMode EQU ScratchPadRam+18 rFlapIncH EQU ScratchPadRam+19 rFlapIncL EQU ScratchPadRam+20 rThresholdH EQU ScratchPadRam+21 rThresholdL EQU ScratchPadRam+22 rFlapIncMaxH EQU ScratchPadRam+23 rFlapIncMaxL EQU ScratchPadRam+24 rAuxLimH EQU ScratchPadRam+25 rAuxLimL EQU ScratchPadRam+26 rAux8 EQU ScratchPadRam+27 ;-------------------------------------------------------------------------- ; EEPROM ;-------------------------------------------------------------------------- eFlapIncMaxH EQU 0 eFlapIncMaxL EQU 1 eThresholdH EQU 2 eThresholdL EQU 3 ;-------------------------------------------------------------------------- ; Constants ;-------------------------------------------------------------------------- #define cPB_PIN_OUT_CH0 0 ; Out Left Aileron #define cPB_PIN_OUT_CH1 1 ; Out Right Aileron #define cPB_PIN_OUT_CH2 2 ; NOT USED #define cPB_PIN_OUT_CH3 3 ; NOT USED #define cPB_PIN_IN_CH0 4 ; In Channel 0 'AILERON' #define cPB_PIN_IN_CH1 5 ; In Channel 1 'ELEVATOR' #define cPB_PIN_IN_CH2 not_connected ; In Channel 2 'THROTLE' #define cPB_PIN_IN_CH3 6 ; In Channel 3 'RUDDER' #define cPA_PIN_IN_0 0 ; In 'UP' #define cPA_PIN_IN_1 1 ; In 'DOWN' #define cPA_PIN_IN_2 2 ; In 'MODE´ #define cPA_PIN_OUT_3 3 ; Out for speaker #define cBEEP_HIGH_FREQ H'13' #define cBEEP_MED_FREQ H'62' ;#define cBEEP_LOW_FREQ H'E2' #define cBEEP_LOW_FREQ H'FF' ;-------------------------------------------------------------------------- ; Macros ;-------------------------------------------------------------------------- ;-------------------------------------------------------------------------- ; MACROS: DISABLE_TIMER_OVERFLOW_INTERRUPT ; ENABLE_TIMER_OVERFLOW_INTERRUPT ; ; DISABLE_ALL_INTERRUPTS ; ENABLE_ALL_INTERRUPTS ; ; M_DISABLE_PORTB_CHANGE_INTERRUPT ; M_ENABLE_PORTB_CHANGE_INTERRUPT ;-------------------------------------------------------------------------- M_DISABLE_TIMER_OVERFLOW_INTERRUPT MACRO BCF INTCON, T0IE ; Disable Timer0 overflow Interrupt ENDM M_ENABLE_TIMER_OVERFLOW_INTERRUPT MACRO BSF INTCON, T0IE ; Enable Timer0 overflow Interrupt ENDM M_ENABLE_ALL_INTERRUPTS MACRO BSF INTCON, GIE ; Enable all interrupts ENDM M_DISABLE_ALL_INTERRUPTS MACRO BCF INTCON, GIE ; Enable all interrupts ENDM M_ENABLE_PORTB_CHANGE_INTERRUPT MACRO BSF INTCON, RBIE ; Enable Port B change interrupt ENDM M_DISABLE_PORTB_CHANGE_INTERRUPT MACRO BCF INTCON, RBIE ; Disable Port B change interrupt ENDM M_ADD_16_8bits MACRO VALUE MOVLW VALUE ; W = VALUE ADDWF TMR0, F ; TMR0 += W BTFSC STATUS, C ; Skip next line if not carry INCF rTMR0H, F ; rTMR0H ++ ENDM ;-------------------------------------------------------------------------- ; Program Code ;-------------------------------------------------------------------------- ;-------------------------------------------------------------------------- ; Set the RESET vector here. ;-------------------------------------------------------------------------- ORG 0 GOTO Start ;-------------------------------------------------------------------------- ; Set the INTERRUPT vector here. ;-------------------------------------------------------------------------- ORG 4 ;GOTO Interrupt NOP NOP ;-------------------------------------------------------------------------- ; Keyboard ;-------------------------------------------------------------------------- #define cMAX_MODE 2 ; 4->valid modes -> 0, 1, 2, 3 Keyboard ; M O D E BTFSS PORTA, cPA_PIN_IN_2 GOTO KeyModePressed BTFSC PORTA, cPA_PIN_IN_0 GOTO lNoKeyUpPressed ; U P ; key up has been pressed ; GOTO($+rMode) MOVF rMode, W ; W = rMode ANDLW 1 ; ???? ADDWF PCL, F ; PCL += W ; Goto look-up table GOTO KeyUpPressedMode0 ; if(rMode==0) GOTO KeyUpPressedMode1 ; if(rMode==1) ;GOTO KeyUpPressedMode2 ; if(rMode==2) ;GOTO KeyUpPressedMode3 ; if(rMode==3) ;GOTO KeyUpPressedMode4 ; if(rMode==4) ;GOTO KeyUpPressedMode5 ; if(rMode==5) lNoKeyUpPressed ; D O W N BTFSC PORTA, cPA_PIN_IN_1 GOTO lNoKeyDownPressed ; key down has been pressed ; GOTO($+rMode) MOVF rMode, W ; W = rMode ANDLW 1 ; ???? ADDWF PCL, F ; PCL += W ; Goto look-up table GOTO KeyDownPressedMode0 ; if(rMode==0) GOTO KeyDownPressedMode1 ; if(rMode==1) ;GOTO KeyDownPressedMode2 ; if(rMode==2) ;GOTO KeyDownPressedMode3 ; if(rMode==3) ;GOTO KeyDownPressedMode4 ; if(rMode==4) ;GOTO KeyDownPressedMode5 ; if(rMode==5) lNoKeyDownPressed RETURN ;-------------------------------------------------------------------------- ;-------------------------------------------------------------------------- KeyModePressed INCF rMode, F ; rMode++ ; if(rMode >= cMAX_MODE) MOVF rMode, W ; rMode -> W SUBLW cMAX_MODE ; cMAX_MODE - W -> W BTFSC STATUS, Z GOTO lThenKMP1 GOTO lElseKMP1 lThenKMP1 ; then CLRF rMode ; 0 -> rMode CALL BeepHappy CALL SaveEEPROM ; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1 GOTO lEndIfKMP1 lElseKMP1 ; else CALL BeepNext lEndIfKMP1 BTFSS PORTA, cPA_PIN_IN_2 ; Wait until key is released GOTO $-1 MOVLW 10 CALL DelayW ; Delay anti-bouncing RETURN ;-------------------------------------------------------------------------- ;-------------------------------------------------------------------------- KeyUpPressedMode0 MOVLW rThresholdH ; MOVWF FSR ; CALL Inc16 ; rFlapIncMaxHL ++ CALL BeepMove RETURN ;-------------------------------------------------------------------------- ;-------------------------------------------------------------------------- KeyDownPressedMode0 MOVLW rThresholdH ; MOVWF FSR ; CALL Dec16 ; rFlapIncMaxHL -- CALL BeepMove RETURN ;-------------------------------------------------------------------------- ; KeyUpPressedMode1 ; rFlapIncMax ++ ;-------------------------------------------------------------------------- KeyUpPressedMode1 CALL BeepMove ; limit max value of rFlapIncMax MOVF rFlapIncMaxL, W ANDLW B'10000000' ; Limit is 3*256 BTFSC STATUS, Z GOTO lSkipLimit CALL BeepLimit GOTO lSkipInc lSkipLimit MOVLW rFlapIncMaxH ; MOVWF FSR ; CALL Inc16 ; rFlapIncMaxHL ++ lSkipInc RETURN ;-------------------------------------------------------------------------- ; KeyDownPressedMode1 ; rFlapIncMax -- ;-------------------------------------------------------------------------- KeyDownPressedMode1 MOVLW rFlapIncMaxH ; MOVWF FSR ; CALL Dec16 ; rFlapIncMaxHL -- CALL BeepMove ; if(rFlapIncMax < 0) rFlapIncMaxH = 0 BTFSS rFlapIncMaxH, 7 GOTO lSkipZero CLRF rFlapIncMaxH CLRF rFlapIncMaxL CALL BeepLimit lSkipZero RETURN ;-------------------------------------------------------------------------- ; Main Program ;-------------------------------------------------------------------------- Start CALL InitPorts CALL InitVar ;CALL OutputServoInitialPos CALL StartUpDemo GOTO MainLoop ;-------------------------------------------------------------------------- ; MainLoop ;-------------------------------------------------------------------------- MainLoop CALL ReadPWM ; MOVLW 255 ; DelayW ; MOVLW 255 ; DelayW ; MOVLW 255 ; DelayW CALL ComputePWM CALL OutputPWM CALL Keyboard ;watchdog.......... CLRWDT GOTO MainLoop ;-------------------------------------------------------------------------- ; ReadPWM ;-------------------------------------------------------------------------- ReadPWM CALL ReadCH0 CALL ReadCH1 CALL ReadCH2 CALL ReadCH3 RETURN ;-------------------------------------------------------------------------- ; ComputePWM ;-------------------------------------------------------------------------- ComputePWM ; OutCH0 = InCH0 MOVF rInCH0CountH, W MOVWF rOutCH0CountH MOVF rInCH0CountL, W MOVWF rOutCH0CountL ; OutCH1 = InCH0 MOVF rInCH0CountH, W MOVWF rOutCH1CountH MOVF rInCH0CountL, W MOVWF rOutCH1CountL ; OutCH2 = InCH2 MOVF rInCH2CountH, W MOVWF rOutCH2CountH MOVF rInCH2CountL, W MOVWF rOutCH2CountL ; OutCH3 = InCH3 MOVF rInCH3CountH, W MOVWF rOutCH3CountH MOVF rInCH3CountL, W MOVWF rOutCH3CountL ; COMPUTE NEW LEFT_AILERON POSITION ; OutCH0Count += rFlapInc MOVF rFlapIncL, W ; rFlapIncL -> W ADDWF rOutCH0CountL, F ; W + rOutCH0CountL -> rOutCH0CountL BTFSC STATUS, C INCF rOutCH0CountH, F ; if overflow -> rOutCH0CountH++ MOVF rFlapIncH, W ; rFlapIncH -> W ADDWF rOutCH0CountH, F ; W + rOutCH0CountH -> rOutCH0CountH ; COMPUTE NEW RIGHT_AILERON POSITION ; OutCH1Count -= rFlapInc MOVF rFlapIncL, W ; rFlapIncL -> W SUBWF rOutCH1CountL, F ; rOutCH0CountL - W -> rOutCH1CountL BTFSS STATUS, C DECF rOutCH1CountH, F ; if overflow -> rOutCH1CountH++ MOVF rFlapIncH, W ; rFlapIncH -> W SUBWF rOutCH1CountH, F ; rOutCH1CountH - W -> rOutCH1CountH break ; COMPUTE (inc or dec) rFlapInc ; if(ThroPosHL(CH2) > rThresholdHL) rFlapInc-- ; else rFlapInc++ ; (rAuxH H, rAuxL L) = rInCH2Count - rThreshold MOVF rInCH2CountH, W MOVWF rAuxH ; rInCH2CountH -> rAuxH MOVF rInCH2CountL, W MOVWF rAuxL ; rInCH2CountL -> rAuxL ; rAUX -= rThreshold movlw rThresholdH movwf FSR call Sub16 ; if(rAUX>0) goto lElseCPWM1 BTFSC rAuxH, 7 GOTO lElseCPWM1 ; dec lThenCPWM1 ; Increment rFlapIncHL ; rFlapIncHL ++ MOVLW rFlapIncH ; MOVWF FSR ; CALL Inc16 ; rFlapIncHL ++ ; LIMIT VALUE OF rFlapIncHL ; if(rFlapIncHL >= rFlapIncMaxHL) break2 ; (rAuxH H, rAuxL L) = rFlapInc - rFlapIncMax MOVF rFlapIncH, W MOVWF rAuxH ; rFlapIncH -> rAuxH MOVF rFlapIncL, W MOVWF rAuxL ; rFlapIncL -> rAuxL ; rAUX -= rFlapIncMax movlw rFlapIncMaxH movwf FSR call Sub16 ; if(rAUX<0) goto lElseCPWM2 BTFSC rAuxH, 7 GOTO lElseCPWM2 ; skip reset to max value lThenCPWM2 MOVF rFlapIncMaxH, W ; MOVWF rFlapIncH ; MOVF rFlapIncMaxL, W ; MOVWF rFlapIncL ; rFlapIncHL = rFlapIncMaxHL GOTO lEndIfCPWM2 lElseCPWM2 lEndIfCPWM2 GOTO lEndIfCPWM1 lElseCPWM1 ; Decrement rFlapIncHL ; if(rFlapIncHL==0) goto lEndIfCPWM1 (e.g. do not decrement) MOVF rFlapIncL, W IORWF rFlapIncH, W BTFSC STATUS, Z GOTO lEndIfCPWM1 ; rFlapIncHL -- MOVLW rFlapIncH ; MOVWF FSR ; CALL Dec16 ; rFlapIncHL -- lEndIfCPWM1 RETURN ;-------------------------------------------------------------------------- ; OutputPWM ;-------------------------------------------------------------------------- OutputPWM CALL OutputPWM_CH0 CALL OutputPWM_CH1 CALL OutputPWM_CH2 CALL OutputPWM_CH3 RETURN ;-------------------------------------------------------------------------- ; InitVar ;-------------------------------------------------------------------------- InitVar CLRF rInCH0CountH CLRF rInCH0CountL CLRF rInCH1CountH CLRF rInCH1CountL CLRF rInCH2CountH CLRF rInCH2CountL CLRF rInCH3CountH CLRF rInCH3CountL CLRF rOutCH0CountH CLRF rOutCH0CountL CLRF rOutCH1CountH CLRF rOutCH1CountL CLRF rOutCH2CountH CLRF rOutCH2CountL CLRF rOutCH3CountH CLRF rOutCH3CountL CLRF rAuxH CLRF rAuxL CLRF rMode CLRF rFlapIncH CLRF rFlapIncL ; Default Configuration MOVLW 1 MOVWF rThresholdH MOVLW 69 MOVWF rThresholdL CLRF rFlapIncMaxH MOVLW 30 MOVWF rFlapIncMaxL ; If 'Mode' is pressed then use default configuration instead of those in EEPROM BTFSC PORTA, cPA_PIN_IN_2 GOTO lModeNotPressedII lModePressedII Call BeepNext Call BeepHappy GOTO lEndIfII lModeNotPressedII CALL InitFromEEPROM lEndIfII RETURN ;-------------------------------------------------------------------------- ; InitPorts ;-------------------------------------------------------------------------- InitPorts BSF STATUS, RP0 ; Select bank 1 ; (TRISB: 1=in, 0=out) RB 7654 inputs, RB 3210 outputs MOVLW 255 ; All pins IN by default MOVWF TRISB MOVWF TRISA ; PORT B BCF TRISB, cPB_PIN_OUT_CH0 BCF TRISB, cPB_PIN_OUT_CH1 BCF TRISB, cPB_PIN_OUT_CH2 BCF TRISB, cPB_PIN_OUT_CH3 BSF TRISB, cPB_PIN_IN_CH0 BSF TRISB, cPB_PIN_IN_CH1 ;BSF TRISB, cPB_PIN_IN_CH2 BSF TRISB, cPB_PIN_IN_CH3 ; PORT A BSF TRISA, cPA_PIN_IN_0 BSF TRISA, cPA_PIN_IN_1 BSF TRISA, cPA_PIN_IN_2 BCF TRISA, cPA_PIN_OUT_3 BCF STATUS, RP0 ; Select bank 0 CLRF PORTB CLRF PORTA RETURN ;-------------------------------------------------------------------------- ; > > > > < < < < ;-------------------------------------------------------------------------- ; if(TimerCount > rL_AIL_COUNT) end pulse for left aileron ; Variables used: rTMR0H:TMR0 rL_AIL_CountH:rL_AIL_CountL ; if(rTMR0H < rL_AIL_CountH) goto lCont1 ; MOVF rL_AIL_CountH, W ; W = rTMR0H ; SUBWF rTMR0H, W ; W = rL_AIL_COUNTH - W , CARRY = (W<0) ; BTFSS STATUS, C ; skip if no carry if(W>=0) ; GOTO lCont1 ; else ; if(TMR0 < rL_AIL_CountL) goto lCont1 ; MOVF rL_AIL_CountL, W ; W = TMR0 ; SUBWF TMR0, W ; W = rL_AIL_COUNTL - W , CARRY = (W<0) ; BTFSC STATUS, C ; skip if no carry if(W>=0) ; else ; BCF PORTB, cPB_PIN_OUT_L_AIL ; End pulse for left aileron ;lCont1 lINCPWM ;INCFSZ rR_AIL_CountL, F RETURN ;INCF rR_AIL_CountH, F RETURN lDECPWM ;DECF rR_AIL_CountL, F ;COMF rR_AIL_CountL, W BTFSC STATUS, Z ;DECF rR_AIL_CountH, F RETURN ;-------------------------------------------------------------------------- ; Beep ; Call with frequency in W ;-------------------------------------------------------------------------- Beep MOVWF rAuxH ; W -> rAuxH save frequency in rAuxH MOVLW 55 ; 155 -> W duration MOVWF rAuxL ; W -> rAuxL MOVF rAuxH, W ; W = rAuxH restore frequency to W lBEEP_1 BSF PORTA, cPA_PIN_OUT_3 ; SET pin out speaker lBEEP_2 DECFSZ rAuxH, F ; rAuxH-- GOTO lBEEP_2 ; if(rAUX!=0) goto lBEEP_2 MOVWF rAuxH ; W -> rAuxH BCF PORTA, cPA_PIN_OUT_3 ; CLEAR pin out speaker lBEEP_3 DECFSZ rAuxH, F ; rAuxH-- GOTO lBEEP_3 ; if(rAuxH!=0) goto lBEEP_3 DECFSZ rAuxL, F ; rAuxL-- GOTO lBEEP_1 ; if(rAuxL!=0) goto lBEEP_1 RETURN ;-------------------------------------------------------------------------- ; Delay(time) ; ; FSR must point to High byte of parameter 'time' ; Address of timeL must be (address of timeH)+1 ; then call 'Delay' ; ; e.g. movlw rOutCH0CountH ; movwf FSR ; call Delay ;-------------------------------------------------------------------------- Delay ;delay(timeH*256) MOVF INDF, W ; W = [FSR->]timeH ANDLW 255 ; BTFSC STATUS, Z ; if(W==0) goto lSkipDelay256 GOTO lSkipDelay256 ; MOVWF rAuxL ; rAuxL = W lLoop1 MOVLW 255 ;???? ; W = 253 MOVWF rAuxH ; rAuxH = W lLoop2 NOP NOP DECFSZ rAuxH, F ; dec rAuxH GOTO lLoop2 ; if(rAuxH!=0) goto lLoop2 DECFSZ rAuxL, F ; dec rAuxL GOTO lLoop1 ; if(rAuxL!=0) goto lLoop1 lSkipDelay256 ;delay(timeL*1) INCF FSR, F ; FSR++ MOVF INDF, W ; W = [FSR->]timeL ANDLW 255 ; BTFSC STATUS, Z ; if(W==0) goto lSkipDelay1 GOTO lSkipDelay1 ; MOVWF rAuxH ; rAuxH = W lLoop3 NOP NOP DECFSZ rAuxH, F ; dec rAuxH GOTO lLoop3 ; if(rAuxH!=0) goto lLoop3 lSkipDelay1 lbreak2 RETURN ;-------------------------------------------------------------------------- ; Inc16(reg16) ; ; FSR must point to High byte of 'register' to increment ; Address of reg16L must be (address of reg16H)+1 ; then call 'Inc16' ; ; e.g. movlw rOutCH0CountH ; movwf FSR ; call Inc16 ;-------------------------------------------------------------------------- Inc16 INCF FSR, F ; FSR++ now point to L INCF INDF, F ; reg16L++ BTFSS STATUS, Z RETURN ; if(reg16L!=0) return DECF FSR, F ; FSR-- now point to H INCF INDF, F ; if(rFlapIncL==0) rFlapIncH++ RETURN ;-------------------------------------------------------------------------- ; Dec16(reg16) ; ; FSR must point to High byte of 'register' to decrement ; Address of reg16L must be (address of reg16H)+1 ; then call 'Inc16' ; ; e.g. movlw rOutCH0CountH ; movwf FSR ; call Dec16 ;-------------------------------------------------------------------------- Dec16 INCF FSR, F ; FSR++ now point to L DECF INDF, F ; reg16L-- COMF INDF, W ; W = 255 - reg16L BTFSS STATUS, Z ; RETURN ; if(W==0) return DECF FSR, F ; FSR-- now point to H DECF INDF, F ; reg16H -- RETURN ;-------------------------------------------------------------------------- ; Sub16(reg16) rAuxHL = rAuxHL - reg16 ; ; FSR must point to High byte of 'register' to decrement ; Address of reg16L must be (address of reg16H)+1 ; then call 'Inc16' ; ; This function doesn't affect the value of FSR ; ; e.g. ; ;(rAuxHL) = rInCH2Count - rThreshold ; ; MOVF rInCH2CountH, W ; MOVWF rAuxH ; rInCH2CountH -> rAuxH ; MOVF rInCH2CountL, W ; MOVWF rAuxL ; rInCH2CountL -> rAuxL ; ; movlw rThresholdH ; movwf FSR ; call Sub16 ;-------------------------------------------------------------------------- Sub16 ; rAuxHL = rAuxHL - rReg16 ; rAuxHL -= rReg16 INCF FSR, F ; FSR++ now point to L MOVF INDF, W ; rRegL -> W SUBWF rAuxL, W ; rAuxL - W -> W , CARRY = (W<0) BTFSS STATUS, C ; DECF rAuxH, F ; if(carry) rAuxL-- DECF FSR, F ; FSR-- now point to H MOVF INDF, W ; rRegH -> W SUBWF rAuxH, F ; rAuxH - W -> rAuxH RETURN ;-------------------------------------------------------------------------- ; ReadCH0 ;-------------------------------------------------------------------------- ReadCH0 CLRF rInCH0CountH ; rInCH0CountH = 0 CLRF rInCH0CountL ; rInCH0CountL = 0 BTFSS PORTB, cPB_PIN_IN_CH0 GOTO $-1 ; while(CH0==0) {} lCH0Loop BTFSS PORTB, cPB_PIN_IN_CH0 GOTO lCH0_GOES_OFF ; if(CH0==0) goto lCH0_GOES_OFF INCFSZ rInCH0CountL, F ; rInCH0CountL++ GOTO lCH0Loop ; if(rInCH0CountH!=0) goto lCH0Loop INCF rInCH0CountH, F ; rInCH0CountH++ GOTO lCH0Loop ; goto lCH0Loop lCH0_GOES_OFF RETURN ;-------------------------------------------------------------------------- ; ReadCH1 ;-------------------------------------------------------------------------- ReadCH1 CLRF rInCH1CountH ; rInCH1CountH = 0 CLRF rInCH1CountL ; rInCH1CountL = 0 ; not necesary ; BTFSS PORTB, cPB_PIN_IN_CH1 ; GOTO $-1 ; while(CH1==0) {} lCH1Loop BTFSS PORTB, cPB_PIN_IN_CH1 GOTO lCH1_GOES_OFF ; if(CH1==0) goto lCH1_GOES_OFF INCFSZ rInCH1CountL, F ; rInCH1CountL++ GOTO lCH1Loop ; if(rInCH1CountH!=0) goto lCH1Loop INCF rInCH1CountH, F ; rInCH1CountH++ GOTO lCH1Loop ; goto lCH1Loop lCH1_GOES_OFF RETURN ;-------------------------------------------------------------------------- ; ReadCH2 ;-------------------------------------------------------------------------- ReadCH2 CLRF rInCH2CountH ; rInCH2CountH = 0 CLRF rInCH2CountL ; rInCH2CountL = 0 ; not necesary ; BTFSS PORTB, cPB_PIN_IN_CH2 ; GOTO $-1 ; while(CH2==0) {} lCH2Loop BTFSC PORTB, cPB_PIN_IN_CH3 GOTO lCH2_GOES_OFF ; if(CH3==1) goto lCH2_GOES_OFF INCFSZ rInCH2CountL, F ; rInCH3CountL++ GOTO lCH2Loop ; if(rInCH2CountH!=0) goto lCH2Loop INCF rInCH2CountH, F ; rInCH2CountH++ GOTO lCH2Loop ; goto lCH2Loop lCH2_GOES_OFF RETURN ;-------------------------------------------------------------------------- ; ReadCH3 ;-------------------------------------------------------------------------- ReadCH3 CLRF rInCH3CountH ; rInCH3CountH = 0 CLRF rInCH3CountL ; rInCH3CountL = 0 ; not necesary ; BTFSS PORTB, cPB_PIN_IN_CH3 ; GOTO $-1 ; while(CH3==0) {} lCH3Loop BTFSS PORTB, cPB_PIN_IN_CH3 GOTO lCH3_GOES_OFF ; if(CH3==0) goto lCH3_GOES_OFF INCFSZ rInCH3CountL, F ; rInCH3CountL++ GOTO lCH3Loop ; if(rInCH3CountH!=0) goto lCH3Loop INCF rInCH3CountH, F ; rInCH3CountH++ GOTO lCH3Loop ; goto lCH3Loop lCH3_GOES_OFF RETURN ;-------------------------------------------------------------------------- ; OutputCH0 ;-------------------------------------------------------------------------- OutputPWM_CH0 MOVLW rOutCH0CountH MOVWF FSR CALL CheckLimits MOVLW rOutCH0CountH MOVWF FSR BSF PORTB, cPB_PIN_OUT_CH0 CALL Delay BCF PORTB, cPB_PIN_OUT_CH0 RETURN ;-------------------------------------------------------------------------- ; OutputCH1 ;-------------------------------------------------------------------------- OutputPWM_CH1 MOVLW rOutCH1CountH MOVWF FSR CALL CheckLimits MOVLW rOutCH1CountH MOVWF FSR BSF PORTB, cPB_PIN_OUT_CH1 CALL Delay BCF PORTB, cPB_PIN_OUT_CH1 RETURN ;-------------------------------------------------------------------------- ; OutputCH2 ;-------------------------------------------------------------------------- OutputPWM_CH2 MOVLW rOutCH2CountH MOVWF FSR CALL CheckLimits MOVLW rOutCH2CountH MOVWF FSR BSF PORTB, cPB_PIN_OUT_CH2 CALL Delay BCF PORTB, cPB_PIN_OUT_CH2 RETURN ;-------------------------------------------------------------------------- ; OutputCH3 ;-------------------------------------------------------------------------- OutputPWM_CH3 MOVLW rOutCH3CountH MOVWF FSR CALL CheckLimits MOVLW rOutCH3CountH MOVWF FSR BSF PORTB, cPB_PIN_OUT_CH3 CALL Delay BCF PORTB, cPB_PIN_OUT_CH3 RETURN ;-------------------------------------------------------------------------- ; CheckLimits(reg16) ; ; FSR must point to High byte of 'reg16' to check ; Address of reg16L must be (address of reg16H)+1 ; then call 'Inc16' ; ; e.g. movlw rOutCH0CountH ; movwf FSR ; call CheckLimits ;-------------------------------------------------------------------------- #define cLimMaxH 1 #define cLimMaxL 142+26 #define cLimMinH 0 #define cLimMinL 200 CheckLimits MOVF FSR, W ; Save FSR in MOVWF rAux8 ; rAux8 ;goto labcde ; CHECK LOWER LIMIT ; if(reg16 >= LimMin) ; (rAuxHL) = reg16 - LimMin ; rAuxLimHL = cLimMaxHL MOVLW cLimMinH ; cLimMinH -> W MOVWF rAuxLimH ; W -> rAuxLimH MOVLW cLimMinL ; cLimMinL -> W MOVWF rAuxLimL ; W -> rAuxLimL MOVF INDF, W MOVWF rAuxH ; rReg16H -> rAuxH INCF FSR ; FSR++ now point to L MOVF INDF, W MOVWF rAuxL ; rReg16L -> rAuxL MOVLW rAuxLimH MOVWF FSR CALL Sub16 ; rAux -= rReg16 ; if(rAUX>=0) goto lElseCL1 BTFSS rAuxH, 7 GOTO lElseCL1 lThenCL1 ; reset FSR to LimMin MOVF rAux8, W ; Restore FSR from MOVWF FSR ; rAux8 MOVLW cLimMinH ; cLimMinH -> W MOVWF INDF ; W -> rRegH INCF FSR ; FSR++ now point to L MOVLW cLimMinL ; cLimMinL -> W MOVWF INDF ; W -> rRegL RETURN ;GOTO lEndIfCL1 lElseCL1 lEndIfCL1 labcde ; CHECK UPPER LIMIT MOVF rAux8, W ; Restore FSR from MOVWF FSR ; rAux8 ; if(reg16 >= LimMax) ; (rAuxHL) = reg16 - LimMax ; rAuxLimHL = cLimMaxHL MOVLW cLimMaxH ; cLimMaxH -> W MOVWF rAuxLimH ; W -> rAuxLimH MOVLW cLimMaxL ; cLimMaxL -> W MOVWF rAuxLimL ; W -> rAuxLimL MOVF INDF, W MOVWF rAuxH ; rReg16H -> rAuxH INCF FSR ; FSR++ now point to L MOVF INDF, W MOVWF rAuxL ; rReg16L -> rAuxL MOVLW rAuxLimH MOVWF FSR CALL Sub16 ; rAux -= rReg16 ; if(rAUX>=0) goto lElseCL2 BTFSC rAuxH, 7 GOTO lElseCL2 lThenCL2 ; reset FSR to LimMax MOVF rAux8, W ; Restore FSR from MOVWF FSR ; rAux8 MOVLW cLimMaxH ; cLimMaxH -> W MOVWF INDF ; W -> rRegH INCF FSR ; FSR++ now point to L MOVLW cLimMaxL ; cLimMaxL -> W MOVWF INDF ; W -> rRegL RETURN ;GOTO lEndIfCL2 lElseCL2 lEndIfCL2 RETURN ;-------------------------------------------------------------------------- ; StartUpDemo ;-------------------------------------------------------------------------- StartUpDemo GOTO BeepHappy ;-------------------------------------------------------------------------- ; BeepHappy ;-------------------------------------------------------------------------- BeepHappy ; Beep 1 MOVLW cBEEP_LOW_FREQ ; frequency CALL Beep ; Beep 2 MOVLW cBEEP_MED_FREQ ; frequency CALL Beep ; Beep 3 MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep RETURN ;-------------------------------------------------------------------------- ; BeepSad ;-------------------------------------------------------------------------- BeepSad ; Beep 1 MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep ; Beep 2 MOVLW cBEEP_MED_FREQ ; frequency CALL Beep ; Beep 3 MOVLW cBEEP_LOW_FREQ ; frequency CALL Beep RETURN ;-------------------------------------------------------------------------- ; BeepNext ;-------------------------------------------------------------------------- BeepNext ; Beep 1 MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep RETURN ;-------------------------------------------------------------------------- ; BeepLimit ;-------------------------------------------------------------------------- BeepLimit ; Beep 1 MOVLW cBEEP_MED_FREQ ; frequency CALL Beep MOVLW 100 CALL DelayW ; Beep 2 MOVLW cBEEP_MED_FREQ ; frequency CALL Beep RETURN ;-------------------------------------------------------------------------- ; BeepMove ;-------------------------------------------------------------------------- BeepMove ; Beep 1 MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep RETURN ;-------------------------------------------------------------------------- ; DelayW ; send delay time in W ; delay approx 256*W cycles ;-------------------------------------------------------------------------- DelayW MOVWF rAuxL ; W -> rAuxL lDelayW2 MOVLW 255 ; 255 -> W MOVWF rAuxH ; W -> rAuxH lDelayW1 DECFSZ rAuxH, F ; rAuxH-- GOTO lDelayW1 ; if(rAuxH!=0) goto lDelayW1 DECFSZ rAuxL, F ; rAuxL-- GOTO lDelayW2 ; if(rAuxL!=0) goto lDelayW2 RETURN ;-------------------------------------------------------------------------- ; SaveEEPROM ;-------------------------------------------------------------------------- ;eFlapIncH ;eFlapIncL ;eThresholdH ;eThresholdL SaveEEPROM MOVLW eFlapIncMaxH MOVWF EEADR MOVF rFlapIncMaxH, W CALL SaveW_EEPROM MOVLW eFlapIncMaxL MOVWF EEADR MOVF rFlapIncMaxL, W CALL SaveW_EEPROM MOVLW eThresholdH MOVWF EEADR MOVF rThresholdH, W CALL SaveW_EEPROM MOVLW eThresholdL MOVWF EEADR MOVF rThresholdL, W CALL SaveW_EEPROM RETURN ;-------------------------------------------------------------------------- ; InitFromEEPROM ;-------------------------------------------------------------------------- InitFromEEPROM MOVLW eFlapIncMaxH CALL ReadEEPROMSequence MOVWF rFlapIncMaxH MOVLW eFlapIncMaxL CALL ReadEEPROMSequence MOVWF rFlapIncMaxL MOVLW eThresholdH CALL ReadEEPROMSequence MOVWF rThresholdH MOVLW eThresholdL CALL ReadEEPROMSequence MOVWF rThresholdL RETURN ;-------------------------------------------------------------------------- ; WriteEEPROMSequence ; Input: EEADR = Address to write to ; W = Value to write ; Output: nothing ; ; e.g. ; ; MOVLW eFlapIncH ; MOVF EEADR, W ; MOVLW rFlapIncH ; ; CALL SaveW_EEPROM ;-------------------------------------------------------------------------- SaveW_EEPROM MOVWF EEDATA BSF STATUS, RP0 ; Select Bank 1 BCF INTCON, GIE ; Disable All Interrupts BSF EECON1, WREN ; Enable Write ; Write sequence MOVLW H'55' MOVWF EECON2 ; Write 55h MOVLW H'AA' MOVWF EECON2 ; Write AAh BSF EECON1, WR ; Begin write lWhileWriting BTFSC EECON1, WR GOTO lWhileWriting ; Wait until write is complete BCF STATUS, RP0 ; Select Bank 0 RETURN ;-------------------------------------------------------------------------- ; ReadEEPROMSequence ; Input: W = Address ; Output: W = Data ; ; e.g. MOVLW eAddress ; eAddress -> W ; ; CALL LoadEEPROMSequence ; On return, data is available in W ; ; MOVWF rReg8 ; W -> rReg8 Save data in register ;-------------------------------------------------------------------------- ReadEEPROMSequence MOVWF EEADR ; W -> EEADR BSF STATUS, RP0 ; Select Bank 1 BSF EECON1, RD ; EE Read BCF STATUS, RP0 ; Select Bank 0 MOVF EEDATA, W ; EEDATA -> W RETURN END ;-------------------------------------------------------------------------- ; OutputServoInitialPos ;-------------------------------------------------------------------------- OutputServoInitialPos MOVLW 1 MOVWF rOutCH0CountH MOVWF rOutCH1CountH MOVLW 50 MOVWF rOutCH0CountL MOVWF rOutCH1CountL CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM MOVLW cBEEP_HIGH_FREQ ; frequency CALL Beep CALL OutputPWM RETURN