' Electronic Speed_Controler ... Sends out PWM to H-Bridge speed control ' Works great - sends out pulse train thats controlled by trim pot ' With 20 Mhz Osc, and PR2 = $FF, period = 0.8 ms = 1.25 kHz ' ' v2 - Added interupts ' Looks Like it's working great - counter Inc's via interupt cycle ' ' v3 - Adding Quad Encoder Routines ' Inc's & Decs QuadCount as Byte [-127..0..127] ' Uses jump table for low overhead ' ' v4 Add real Quad Routines ' @ Device HS_OSC ' High Speed Crystal @ Device WDT_ON ' Watch Dog timer ON @ Device PWRT_ON ' Power Reset timer ON @ Device BOD_ON ' Brown out Protection ON @ Device LVP_OFF ' Low voltage programming capability @ Device CPD_OFF ' Code Protection OFF define __16F873A 1 define OSC 20 DEFINE INTHAND DO_ISR DEFINE DEBUG_REG PortC DEFINE DEBUG_BIT 6 DEFINE DEBUG_BAUD 9600 DEFINE DEBUG_MODE 0 ' ------------- Hardware Addresses --------------------------- GIE VAR INTCON.7 ' Global Interupt Enable PEIE VAR INTCON.6 ' Perif. Eq. Interupt Enable ADIE VAR PIE1.6 ' A-2-D Converter Interupt Enable TMR1IE VAR PIE1.0 ' PIE address 10h TMR2IE VAR PIE1.1 TMR1ON VAR T1CON.0 ' Register addresses are in PIC.inc TMR2ON VAR T2CON.2 EnaPin VAR PORTC.2 ' PWM pin FwdPin VAR PORTB.5 RevPin VAR PORTB.4 QuadPin1 Var PORTC.0 LED var PORTC.3 ' ------------- Allocate space for Interupt Storage ----------------------- wSave VAR BYTE $20 SYSTEM ; Save W reg location if in bank0 wSave1 VAR BYTE $A0 SYSTEM ; Save W reg location if in bank1 ; wSave2 VAR BYTE $120 SYSTEM ; Save W reg location if in bank2 ; wSave3 VAR BYTE $1A0 SYSTEM ; Save W reg location if in bank3 sSave VAR BYTE Bank0 SYSTEM ; Save location for STATUS reg pSave VAR BYTE Bank0 SYSTEM ; Save location for PCLATH reg StatFlag VAR BYTE Bank0 GyroTaskFlag VAR StatFlag.0 OddCycleFlag VAR StatFlag.1 QuadStateFlag var Statflag.2 QuadCount VAR Byte Bank0 ; Count [-127..0..127] encoder clicks QuadPins Var Byte Bank0 ; current state of pins QuadPrev VAR Byte Bank0 ; previous state of Quad Encoder pins QuadByte Var Byte Bank0 ' -------------- Allocate variables --------------------------- ADVal Var Word DutyCycle Var Byte nLoops VAR Byte '============================================================================ '============================================================================ CodeStart: Goto InitSeq ' Skip over subroutines '============================================================================ '============================================================================ ' Do_ISR ' Interupt Service Routine ' Handle all the Interupts Asm DO_ISR IF (CODE_SIZE <= 2) movwf wsave ; save W register swapf STATUS,W ; save STATUS reg clrf STATUS ; switch to bank 0 movwf ssave ; save STATUS reg to area in Bank 0 movf PCLATH,W ; save Program Counter movwf psave ; save in Bank 0 location EndIF ; ========================================================================== ; See who caused the Interupt. ; Vector to the correct routines clrf STATUS ; change to bank 0 regardless of current bank BTFSC PIR1,TMR1IF ; Timer1 overflow? GOTO T1_INT ; Yes - go to Timer1 routine BTFSC PIR1,TMR2IF ; Timer2 overflow? GOTO T2_INT ; Yes - go to Timer2 routine CLRF PIR1 GOTO ISR_Done ; Error - never handled interupt ; ========================================================================== ; Return -1,0,1 in W reg ; This routine MUST be in pzge 0 QUAD1_JUMP CLRF PCLATH ; PAGE ZERO ADDWF PCL,F ; Indirect jump RETLW 0 ; 00 -> 00 RETLW 1 ; 00 -> 01 RETLW 1 ; 00 -> 10 RETLW 1 ; 00 -> 11 RETLW 1 ; 01 -> 00 RETLW 0 ; 01 -> 01 RETLW 1 ; 01 -> 10 RETLW 1 ; 01 -> 11 RETLW 1 ; 10 -> 00 RETLW 1 ; 10 -> 01 RETLW 0 ; 10 -> 10 RETLW 1 ; 10 -> 11 RETLW 1 ; 11 -> 00 RETLW 1 ; 11 -> 01 RETLW 1 ; 11 -> 10 RETLW 0 ; 11 -> 11 ; ========================================================================== T1_INT ; Handle Timer1 Overflow BCF T1CON,TMR1ON ; Turn OFF timer1 BCF PIR1, TMR1IF ; Clear the interupt flag ; BCF PORTB,5 ; Turn Off Pin Pulse - turn both pins off ; BCF PORTB,4 ; only 1 will be on, but clearing both is OK GOTO ISR_Done ; ============================================================================ ; Get the Quadrature Encoder Data ; Inc/Dec QuadCount during interupts @ 1.25 kHz sample rate ; Clear QuadCount during steering cycle T2_INT ; Handle Timer2 / PWM Interupt BCF PIR1, TMR2IF ; Clear the interupt flag MOVF PORTC,W ; Read Quad Sensor Pins - this needs to change ANDLW 1 ; AND with 0001 MOVWF _QuadPins MOVF _QuadPrev,W ADDWF _QuadPins,W MOVWF _QuadByte BTFSC _QuadByte.0 ; Check Bit 0 INCF _QuadCount,F MOVF _QuadPins,W MOVWF _QuadPrev GOTO ISR_Done ; BCF PORTB,4 ; INCF _QuadCount,F ; ------ BCF STATUS,C ; Clear carry bit MOVF PORTC,W ; Read Quad Sensor Pins - this needs to change ANDLW 1 ; AND with 0001 ; ANDLW 6 ; AND with 0110 MOVWF _QuadPins ; RRF _QuadPins,F ; Roll right -> 0011 RLF _QuadPrev,F ; Roll prev pins into top bits RLF _QuadPrev,W ; Roll into W Reg : 1100 IORWF _QuadPins,W ; OR prev and crnt states : PPcc MOVWF _QuadPrev ; save for next time ; Movlw 1 MOVF _QuadPrev,W ; seems un-necessary CALL QUAD1_JUMP ADDWF _QuadCount,F ; =============================== ISR_Done Movf psave,w ; Restore the PCLATH reg Movwf PCLATH swapf ssave,w ; Restore the STATUS reg ;(sets bank to original state) Movwf STATUS ; move W into Status reg swapf wsave,f ; Swap w_save swapf wsave,w ; Swap w_save into W RETFIE EndASM '============================================================================ '============================================================================ '============================================================================ '============================================================================ ' Subroutines to read AD converter voltages Read_Pot1: ADCON0 = %10000001 ' Set A/D to Channel 0, On ' Goto Read_AD Read_AD: nLoops = 0 Pauseus 10 ' Wait for channel to setup ADCON0.2 = 1 ' Start conversion wLoop: nLoops = nLoops + 1 Pauseus 5 ' Wait for conversion IF ADCON0.2 = 1 then wLoop adval.HIGHBYTE = ADRESH adval.LowBYTE = ADRESL Return '----------------------------------------------------------------------- '----------------------------------------------------------------------- InitSeq: TRISA = %00000001 ' Set PORTA TRISB = %00000000 ' Set PORTB TRISC = %10000001 ' Set PORTC ADCON1 = %10000000 ' Set AD clock to Foxc/64, AD uses +5v as +VRef CCP1CON = 0 ' CCP Module OFF HIGH FwdPin LOW RevPin QuadCount = 0 QuadPrev = 0 StatFlag = 0 T2CON = %00000011 ' Timer2 Pre-Scaler = 1:16, Timer OFF TMR2 = 0 ' Clear Timer2 PR2 = 255 ' Setup PWM Period = 1.25 kHz CCPR1L = 63 ' Set Duty Cycle to 25% CCP1CON = %00101100 ' PWM mode, 2 LSBs of duty cycle = 10 TMR2ON = 1 Pause 500 HIGH LED DEBUG 13,10,"Elec Speed Ctl Test v2", 13 Pause 500 GIE = 1 ' Global Interupt Enable PEIE = 1 ' Peripheral Interupt Enable TMR2IE = 1 ' Timer2 Interupt Enable bit Low LED debug "Made it to the Main Loop!", 13 pause 1000 '----------------------------------------------------------------------- mainloop: Toggle LED Gosub Read_Pot1 ; returns value 0..1023 adval = adval / 2 ; range now 0..511 ' Debug "Pot: ", DEC ADval, 13 if adval > 255 then adval = adval - 256 fwdpin = 0 revpin = 1 else adval = 255 - adval fwdpin = 1 revpin = 0 Endif Adval = 255 - Adval ; 0 = full speed ; flip so range is 0..255..0 ; reverse dir in center Debug "Duty Cycle: ", DEC ADval, " qCount: ", dec QuadCount, " qPrev: ", dec QuadPrev, 13 QuadCount = 0 CCPR1L = ADval MIN 255 ; Set Duty Cycle 0..255 Pause 400 Goto mainloop ' debug 13, "All Done.", 13 End End