;**********************************************************************
; *
; Filename: sensored.asm *
; Date: 11 Feb. 2002 *
; File Version: 1.0 *
; *
; Author: W.R. Brown *
; Company: Microchip Technology Incorporated *
; *
; *
;**********************************************************************
; *
; Files required: p16f877.inc *
; *
; *
; *
;**********************************************************************
; *
; Notes: Sensored brushless motor control Main loop uses 3-bit *
; sensor input as index for drive word output. PWM based on *
; Timer0 controls average motor voltage. PWM level is determined *
; PWM level is determined from ADC reading of potentiometer. *
; *
;**********************************************************************
list p=16f877 ; list directive to define processor
#include <p16f877.inc> ; processor specific variable definitions
__CONFIG _CP_OFF & _WDT_OFF & _BODEN_ON & _PWRTE_ON & _HS_OSC & _WRT_ENABLE_OFF & _LVP_ON &
_DEBUG_OFF & _CPD_OFF
;**********************************************************************
;*
;* Define variable storage
;*
CBLOCK 0x20
ADC ; PWM threshold is ADC result
LastSensor ; last read motor sensor data
DriveWord ; six bit motor drive data
ENDC

;**********************************************************************
;*
;* Define I/O
;*
#define OffMask B'11010101'
#define DrivePort PORTC
#define DrivePortTris TRISC
#define SensorMask B'00000111'
#define SensorPort PORTE
#define DirectionBit PORTA,1
;**********************************************************************
org 0x000 ; startup vector
nop ; required for ICD operation
clrf PCLATH ; ensure page bits are cleared
goto Initialize ; go to beginning of program
ORG 0x004 ; interrupt vector location
retfie ; return from interrupt
;**********************************************************************
;*
;* Initialize I/O ports and peripherals
;*
Initialize
clrf DrivePort ; all drivers off
banksel TRISA
; setup I/O
clrf DrivePortTris ; set motor drivers as outputs
movlw B'00000011' ; A/D on RA0, Direction on RA1, Motor sensors on RE<2:0>
movwf TRISA ;
; setup Timer0
movlw B'11010000' ; Timer0: Fosc, 1:2
movwf OPTION_REG
; Setup ADC (bank1)
movlw B'00001110' ; ADC left justified, AN0 only
movwf ADCON1
banksel ADCON0
; setup ADC (bank0)
movlw B'11000001' ; ADC clock from int RC, AN0, ADC on
movwf ADCON0
bsf ADCON0,GO ; start ADC
clrf LastSensor ; initialize last sensor reading
call Commutate ; determine present motor position
clrf ADC ; start speed control threshold at zero until first ADC
reading
;**********************************************************************
;*
;* Main control loop
;*
Loop
call ReadADC ; get the speed control from the ADC
incfsz ADC,w ; if ADC is 0xFF we're at full speed - skip timer add
goto PWM ; add Timer0 to ADC for PWM
movf DriveWord,w ; force on condition
goto Drive ; continue
PWM

movf ADC,w ; restore ADC reading
addwf TMR0,w ; add it to current Timer0
movf DriveWord,w ; restore commutation drive data
btfss STATUS,C ; test if ADC + Timer0 resulted in carry
andlw OffMask ; no carry - suppress high drivers
Drive
movwf DrivePort ; enable motor drivers
call Commutate ; test for commutation change
goto Loop ; repeat loop
ReadADC
;**********************************************************************
;*
;* If the ADC is ready then read the speed control potentiometer
;* and start the next reading
;*
btfsc ADCON0,NOT_DONE ; is ADC ready?
return ; no - return
movf ADRESH,w ; get ADC result
bsf ADCON0,GO ; restart ADC
movwf ADC ; save result in speed control threshold
return ;
;**********************************************************************
;*
;* Read the sensor inputs and if a change is sensed then get the
;* corresponding drive word from the drive table
;*
Commutate
movlw SensorMask ; retain only the sensor bits
andwf SensorPort,w ; get sensor data
xorwf LastSensor,w ; test if motion sensed
btfsc STATUS,Z ; zero if no change
return ; no change - back to the PWM loop
xorwf LastSensor,f ; replace last sensor data with current
btfss DirectionBit ; test direction bit
goto FwdCom ; bit is zero - do forward commutation
; reverse commutation
movlw HIGH RevTable ; get MS byte of table
movwf PCLATH ; prepare for computed GOTO
movlw LOW RevTable ; get LS byte of table
goto Com2
FwdCom ; forward commutation
movlw HIGH FwdTable ; get MS byte of table
movwf PCLATH ; prepare for computed GOTO
movlw LOW FwdTable ; get LS byte of table
Com2
addwf LastSensor,w ; add sensor offset
btfsc STATUS,C ; page change in table?
incf PCLATH,f ; yes - adjust MS byte
call GetDrive ; get drive word from table
movwf DriveWord ; save as current drive word
return
GetDrive
movwf PCL

;**********************************************************************
;*
;* The drive tables are built based on the following assumptions:
;* 1) There are six drivers in three pairs of two
;* 2) Each driver pair consists of a high side (+V to motor) and low side (motor to ground) drive
;* 3) A 1 in the drive word will turn the corresponding driver on
;* 4) The three driver pairs correspond to the three motor windings: A, B and C
;* 5) Winding A is driven by bits <1> and <0> where <1> is A's high side drive
;* 6) Winding B is driven by bits <3> and <2> where <3> is B's high side drive
;* 7) Winding C is driven by bits <5> and <4> where <5> is C's high side drive
;* 8) Three sensor bits constitute the address offset to the drive table
;* 9) A sensor bit transitions from a 0 to 1 at the moment that the corresponding
;* winding's high side forward drive begins.
;* 10) Sensor bit <0> corresponds to winding A
;* 11) Sensor bit <1> corresponds to winding B
;* 12) Sensor bit <2> corresponds to winding C
;*
FwdTable
retlw B'00000000' ; invalid
retlw B'00010010' ; phase 6
retlw B'00001001' ; phase 4
retlw B'00011000' ; phase 5
retlw B'00100100' ; phase 2
retlw B'00000110' ; phase 1
retlw B'00100001' ; phase 3
retlw B'00000000' ; invalid
RevTable
retlw B'00000000' ; invalid
retlw B'00100001' ; phase /6
retlw B'00000110' ; phase /4
retlw B'00100100' ; phase /5
retlw B'00011000' ; phase /2
retlw B'00001001' ; phase /1
retlw B'00010010' ; phase /3
retlw B'00000000' ; invalid
END ; directive 'end of program'

;**********************************************************************
; *
; Filename: snsrless.asm *
; Date: 14 Jan. 2002 *
; File Version: 1.0 *
; *
; Author: W.R. Brown *
; Company: Microchip Technology Incorporated *
; *
; *
;**********************************************************************
; *
; Files required: p16f877.inc *
; *
; *
; *
;**********************************************************************
; *
; Notes: Sensorless brushless motor control *
; *
; Closed loop 3 phase brushless DC motor control. *
; Two potentiometers control operation. One potentiometer (A0) *
; controls PWM (voltage) and RPM (from table). The other *
; potentiometer (A1) provides a PWM offset to the PWM derived *
; from A0. Phase A motor terminal is connected via voltage *
; divider to A3. This is read while the drive is on during *
; phase 4. The result is the peak applied voltage (Vsupply). *
; A3 is also read while the drive is on at two times during *
; phase 5. The result is the BEMF voltage. The BEMF voltage is *
; read at the quarter (t1) and mid (t2) points of the phase 5 *
; period. BEMF is compared to VSupply/2. If BEMF is above *
; VSupply/2 at t1 and below VSupply/2w at t2 then no speed *
; adjustment is made. If BEMF is high at both t1 and t2 then *
; the speed is reduced. If BEMF is low at t1 and t2 then the *
; speed is increased. *
; *
;**********************************************************************
;
list P = PIC16F877
include "p16f877.inc"
__CONFIG _CP_OFF & _WRT_ENABLE_OFF & _HS_OSC & _WDT_OFF & _PWRTE_ON & _BODEN_ON
; Acceleration/Deceleration Time = RampRate * 256 * 256 * Timer0Timer0 prescale / Fosc
#define AccelDelay D'100' ; determines full range acceleration time
#define DecelDelay D'10' ; determines full range deceleration time
#define ManThresh 0x3f ; Manual threshold is the PWM potentiomenter
; reading above which RPM is adjusted automatically
#define AutoThresh 0x100-ManThresh

OffMask equ B'11010101' ; PWM off kills the high drives
Invalid equ B'00000000' ; invalid
Phase1 equ B'00100001' ; phase 1 C high, A low
Phase2 equ B'00100100' ; phase 2 C high, B low
Phase3 equ B'00000110' ; phase 3 A high, B low
Phase4 equ B'00010010' ; phase 4 A high, C low
Phase5 equ B'00011000' ; phase 5 B high, C low
Phase6 equ B'00001001' ; phase 6 B high, A low
#define CARRY STATUS,C
#define ZERO STATUS,Z
#define subwl sublw
;*********************************************************************************
;*
;* Define I/O Ports
;*
#define ReadIndicator PORTB,0 ; diagnostic scope trigger for BEMF readings
#define DrivePort PORTC ; motor drive and lock status
;*********************************************************************************
;*
;* Define RAM variables
;*
CBLOCK 0x20
STATE ; Machine state
PWMThresh ; PWM threshold
PhaseIndx ; Current motor phase index
Drive ; Motor drive word
RPMIndex ; RPM Index workspace
ADCRPM ; ADC RPM value
ADCOffset ; Delta offset to ADC PWM threshold
PresetHi ; speed control timer compare MS byte
PresetLo ; speed control timer compare LS byte
Flags ; general purpose flags
Vsupply ; Supply voltage ADC reading
DeltaV1 ; Difference between expected and actual BEMF at T/4
DeltaV2 ; Difference between expected and actual BEMF at T/2
CCPSaveH ; Storage for phase time when finding DeltaV
CCPSaveL ; Storage for phase time when finding DeltaV
CCPT2H ; Workspace for determining T/2 and T/4
CCPT2L ; Workspace for determining T/2 and T/4
RampTimer ; Timer0 post scaler for accel/decel ramp rate
xCount ; general purpose counter workspace
Status ; relative speed indicator status
ENDC

;*********************************************************************************
;*
;* Define Flags
;*
#define DriveOnFlag Flags,0 ; Flag for invoking drive disable mask when clear
#define AutoRPM Flags,1 ; RPM timer is adjusted automatically
; Flags,3 ; Undefined
#define FullOnFlag Flags,4 ; PWM threshold is set to maximum drive
#define Tmr0Ovf Flags,5 ; Timer0 overflow flag
#define Tmr0Sync Flags,6 ; Second Timer0 overflow flag
; Flags,7 ; undefined
#define BEMF1Low DeltaV1,7 ; BEMF1 is low if DeltaV1 is negative
#define BEMF2Low DeltaV2,7 ; BEMF2 is low if DeltaV2 is negative
;*********************************************************************************
;*
;* Define State machine states and index numbers
;*
sRPMSetup equ D'0' ; Wait for Phase1, Set ADC GO, RA1->ADC
sRPMRead equ sRPMSetup+1 ; Wait for ADC nDONE, Read ADC->RPM
sOffsetSetup equ sRPMRead+1 ; Wait for Phase2, Set ADC GO, RA3->ADC
sOffsetRead equ sOffsetSetup+1 ; Wait for ADC nDONE, Read ADC->ADCOffset
sVSetup equ sOffsetRead+1 ; Wait for Phase4, Drive On, wait 9 uSec, Set ADC GO
sVIdle equ sVSetup+1 ; Wait for Drive On, wait Tacq, set ADC GO
sVRead equ sVIdle+1 ; Wait for ADC nDONE, Read ADC->Vsupply
sBEMFSetup equ sVRead+1 ; Wait for Phase5, set Timer1 compare to half phase time
sBEMFIdle equ sBEMFSetup+1 ; Wait for Timer1 compare, Force Drive on and wait 9 uSec,
; Set ADC GO, RA0->ADC
sBEMFRead equ sBEMFIdle+1 ; Wait for ADC nDONE, Read ADC->Vbemf
sBEMF2Idle equ sBEMFRead+1 ; Wait for Timer1 compare, Force Drive on and wait 9 uSec,
; Set ADC GO, RA0->ADC
sBEMF2Read equ sBEMF2Idle+1 ; Wait for ADC nDONE, Read ADC->Vbemf
;*********************************************************************************
;*
;* The ADC input is changed depending on the STATE
;* Each STATE assumes a previous input selection and changes the selection
;* by XORing the control register with the appropriate ADC input change mask
;* defined here:
;*
ADC0to1 equ B'00001000' ; changes ADCON0<5:3> from 000 to 001
ADC1to3 equ B'00010000' ; changes ADCON0<5:3> from 001 to 011
ADC3to0 equ B'00011000' ; changes ADCON0<5:3> from 011 to 000
;*********************************************************************************
;**************************** PROGRAM STARTS HERE ********************************
;*********************************************************************************
org 0x000
nop
goto Initialize
org 0x004
bsf Tmr0Ovf ; Timer0 overflow flag used by accel/decel timer
bsf Tmr0Sync ; Timer0 overflow flag used to synchronize code execution
bcf INTCON,T0IF
retfie ;
Initialize
clrf PORTC ; all drivers off
clrf PORTB

banksel TRISA
; setup I/O
clrf TRISC ; motor drivers on PORTC
movlw B'00001011' ; A/D on RA0 (PWM), RA1 (Speed) and RA3 (BEMF)
movwf TRISA ;
movlw B'11111110' ; RB0 is locked indicator
movwf TRISB
; setup Timer0
movlw B'11010000' ; Timer0: Fosc, 1:2
movwf OPTION_REG
bsf INTCON,T0IE ; enable Timer0 interrupts
; Setup ADC
movlw B'00000100' ; ADC left justified, AN0, AN1
movwf ADCON1
banksel PORTA
movlw B'10000001' ; ADC clk = Fosc/32, AN0, ADC on
movwf ADCON0
; setup Timer 1
movlw B'00100001' ; 1:4 prescale, internal clock, timer on
movwf T1CON
; setup Timer 1 compare
movlw 0xFF ; set compare to maximum count
movwf CCPR1L ; LS compare register
movwf CCPR1H ; MS compare register
movlw B'00001011' ; Timer 1 compare mode, special event - clears timer1
movwf CCP1CON
; initialize RAM
clrf PWMThresh
movlw D'6'
movwf PhaseIndx
clrf Flags
clrf Status ;
clrf STATE ; LoopIdle->STATE
bcf INTCON,T0IF ; ensure Timer0 overflow flag is cleared
bsf INTCON,GIE ; enable interrupts
MainLoop
;*****************************************************************
;
; PWM, Commutation, State machine loop
;
;*****************************************************************
btfsc PIR1,CCP1IF ; time for phase change?
call Commutate ; yes - change motor drive
PWM
bsf DriveOnFlag ; pre-set flag
btfsc FullOnFlag ; is PWM level at maximum?
goto PWM02 ; yes - only commutation is necessary
movf PWMThresh,w ; get PWM threshold
addwf TMR0,w ; compare to Timer0
btfss CARRY ; drive is on if carry is set
bcf DriveOnFlag ; timer has not reached threshold, disable drive
call DriveMotor ; output drive word
PWM02
call LockTest
call StateMachine ; service state machine
goto MainLoop ; repeat loop

StateMachine
movlw SMTableEnd-SMTable-1 ; STATE table must have 2^n entries
andwf STATE,f ; limit STATE index to state table
movlw high SMTable ; get high byte of table address
movwf PCLATH ; prepare for computed goto
movlw low SMTable ; get low byte of table address
addwf STATE,w ; add STATE index to table root
btfsc CARRY ; test for page change in table
incf PCLATH,f ; page change adjust
movwf PCL ; jump into table
SMTable ; number of STATE table entries MUST be evenly divisible by 2
goto RPMSetup ; Wait for Phase1, Set ADC GO, RA1->ADC, clear Timer0 overflow
goto RPMRead ; Wait for ADC nDONE, Read ADC->RPM
goto OffsetSetup ; Wait for Phase2, Set ADC GO, RA3->ADC
goto OffsetRead ; Wait for ADC nDONE, Read ADC->ADCOffset
goto VSetup ; Wait for Phase4
goto VIdle ; Wait for Drive On, wait Tacq, set ADC GO
goto VRead ; Wait for ADC nDONE, Read ADC->Vsupply
goto BEMFSetup ; Wait for Phase5, set Timer1 compare to half phase time
goto BEMFIdle ; When Timer1 compares force Drive on, Set ADC GO after Tacq,
RA0->ADC
goto BEMFRead ; Wait for ADC nDONE, Read ADC->Vbemf
goto BEMF2Idle ; When Timer1 compares force Drive on, Set ADC GO after Tacq,
RA0->ADC
goto BEMF2Read ; Wait for ADC nDONE, Read ADC->Vbemf
; fill out table with InvalidStates to make number of table entries evenly divisible by 2
goto InvalidState ; invalid state - reset state machine
goto InvalidState ; invalid state - reset state machine
goto InvalidState ; invalid state - reset state machine
goto InvalidState ; invalid state - reset state machine
SMTableEnd
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
RPMSetup ; Wait for Phase1, Set ADC GO, RA1->ADC, clear Timer0 overflow
movlw Phase1 ; compare Phase1 word...
xorwf Drive,w ; ...with current drive word
btfss ZERO ; ZERO if equal
return ; not Phase1 - remain in current STATE
bsf ADCON0,GO ; start ADC
movlw ADC0to1 ; prepare to change ADC input
xorwf ADCON0,f ; change from AN0 to AN1
incf STATE,f ; next STATE
bcf Tmr0Sync ; clear Timer0 overflow
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
RPMRead ; Wait for ADC nDONE, Read ADC->RPM
btfsc ADCON0,GO ; is ADC conversion finished?
return ; no - remain in current STATE
movf ADRESH,w ; get ADC result
movwf ADCRPM ; save in RPM
incf STATE,f ; next STATE
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

OffsetSetup ; Wait for Phase2, Set ADC GO, RA3->ADC
movlw Phase2 ; compare Phase2 word...
xorwf Drive,w ; ...with current drive word
btfss ZERO ; ZERO if equal
return ; not Phase2 - remain in current STATE
bsf ADCON0,GO ; start ADC
movlw ADC1to3 ; prepare to change ADC input
xorwf ADCON0,f ; change from AN1 to AN3
incf STATE,f ; next STATE
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
OffsetRead ; Wait for ADC nDONE, Read ADC->ADCOffset
btfsc ADCON0,GO ; is ADC conversion finished?
return ; no - remain in current STATE
movf ADRESH,w ; get ADC result
xorlw H'80' ; complement MSB for +/- offset
movwf ADCOffset ; save in offset
addwf ADCRPM,w ; add offset to PWM result
btfss ADCOffset,7 ; is offset a negative number?
goto OverflowTest ; no - test for overflow
btfss CARRY ; underflow?
andlw H'00' ; yes - force minimum
goto Threshold ;
OverflowTest
btfsc CARRY ; overflow?
movlw H'ff' ; yes - force maximum
Threshold
movwf PWMThresh ; PWM threshold is RPM result plus offset
btfsc ZERO ; is drive off?
goto DriveOff ; yes - skip voltage measurements
bcf FullOnFlag ; pre-clear flag in preparation of compare
sublw 0xFD ; full on threshold
btfss CARRY ; CY = 0 if PWMThresh > FullOn
bsf FullOnFlag ; set full on flag
incf STATE,f ; next STATE
return ; back to Main Loop
DriveOff
clrf Status ; clear speed indicators
movlw B'11000111' ; reset ADC input to AN0
andwf ADCON0,f ;
clrf STATE ; reset state machine
return
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
VSetup ; Wait for Phase4
movlw Phase4 ; compare Phase4 word...
xorwf Drive,w ; ...with current Phase drive word
btfss ZERO ; ZERO if equal
return ; not Phase4 - remain in current STATE
call SetTimer ; set timer value from RPM table
incf STATE,f ; next STATE
return ; back to Main Loop

;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
VIdle ; Wait for Drive On, wait Tacq, set ADC GO
btfss DriveOnFlag ; is Drive active?
return ; no - remain in current STATE
call Tacq ; motor Drive is active - wait ADC Tacq time
bsf ADCON0,GO ; start ADC
incf STATE,f ; next STATE
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
VRead ; Wait for ADC nDONE, Read ADC->Vsupply
btfsc ADCON0,GO ; is ADC conversion finished?
return ; no - remain in current STATE
movf ADRESH,w ; get ADC result
movwf Vsupply ; save as supply voltage
incf STATE,f ; next STATE
bcf Tmr0Sync ; clear Timer0 overflow
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
BEMFSetup ; Wait for Phase5, set Timer1 compare to half phase time
movlw Phase5 ; compare Phase5 word...
xorwf Drive,w ; ...with current drive word
btfss ZERO ; ZERO if equal
return ; not Phase5 - remain in current STATE
btfss Tmr0Sync ; synchronize with Timer0
return ;
btfss PWMThresh,7 ; if PWMThresh > 0x80 then ON is longer than OFF
goto BEMFS1 ; OFF is longer and motor is currently off - compute now
btfss DriveOnFlag ; ON is longer - wait for drive cycle to start
return ; not started - wait
BEMFS1
bcf CCP1CON,0 ; disable special event on compare
movf CCPR1H,w ; save current capture compare state
movwf CCPSaveH ;
movwf CCPT2H ; save copy in workspace
movf CCPR1L,w ; low byte
movwf CCPSaveL ; save
movwf CCPT2L ; and save copy
bcf CARRY ; pre-clear carry for rotate
rrf CCPT2H,f ; divide phase time by 2
rrf CCPT2L,f ;
bcf CARRY ; pre-clear carry
rrf CCPT2H,w ; divide phase time by another 2
movwf CCPR1H ; first BEMF reading at phase T/4
rrf CCPT2L,w ;
movwf CCPR1L ;
incf STATE,f ; next STATE
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

BEMFIdle ; When Timer1 compares force Drive on, Set ADC GO after Tacq, RA0-
>ADC
btfss PIR1,CCP1IF ; timer compare?
return ; no - remain in current STATE
bsf DriveOnFlag ; force drive on for BEMF reading
call DriveMotor ; activate motor drive
bsf ReadIndicator ; Diagnostic
call Tacq ; wait ADC acquisition time
bsf ADCON0,GO ; start ADC
bcf ReadIndicator ; Diagnostic
; setup to capture BEMF at phase 3/4 T
movf CCPT2H,w
addwf CCPR1H,f ; next compare at phase 3/4 T
movf CCPT2L,w ;
addwf CCPR1L,f ; set T/2 lsb
btfsc CARRY ; test for carry into MSb
incf CCPR1H,f ; perform carry
bcf PIR1,CCP1IF ; clear timer compare interrupt flag
incf STATE,f ; next STATE
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
BEMFRead ; Wait for ADC nDONE, Read ADC->Vbemf
btfsc ADCON0,GO ; is ADC conversion finished?
return ; no - remain in current STATE
rrf Vsupply,w ; divide supply voltage by 2
subwf ADRESH,w ; Vbemf - Vsupply/2
movwf DeltaV1 ; save error voltage
incf STATE,f ; next STATE
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
BEMF2Idle ; When Timer1 compares force Drive on, Set ADC GO after Tacq, RA0-
>ADC
btfss PIR1,CCP1IF ; timer compare?
return ; no - remain in current STATE
bsf DriveOnFlag ; force drive on for BEMF reading
call DriveMotor ; activate motor drive
bsf ReadIndicator ; Diagnostic
call Tacq ; wait ADC acquisition time
bsf ADCON0,GO ; start ADC
bcf ReadIndicator ; Diagnostic
movlw ADC3to0 ; prepare to change ADC input
xorwf ADCON0,f ; change from AN3 to AN0
; restore Timer1 phase time and special event compare mode
movf CCPSaveH,w
movwf CCPR1H ; next compare at phase T
movf CCPSaveL,w ;
movwf CCPR1L ; set T lsb
bcf PIR1,CCP1IF ; clear timer compare interrupt flag
bsf CCP1CON,0 ; enable special event on compare
incf STATE,f ; next STATE
return ; back to Main Loop

;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
BEMF2Read ; Wait for ADC nDONE, Read ADC->Vbemf
btfsc ADCON0,GO ; is ADC conversion finished?
return ; no - remain in current STATE
rrf Vsupply,w ; divide supply voltage by 2
subwf ADRESH,w ; Vbemf - Vsupply/2
movwf DeltaV2 ; save error voltage
clrf STATE ; reset state machine to beginning
return ; back to Main Loop
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
InvalidState ; trap for invalid STATE index
movlw B'11000111' ; reset ADC input to AN0
andwf ADCON0,f ;
clrf STATE
return
;________________________________________________________________________________________________
Tacq
;*****************************************************************
;
; Software delay for ADC acquisition time
; Delay time = Tosc*(3+3*xCount)
;
;*****************************************************************
movlw D'14 ; 14 equates to approx 9 uSec delay
movwf xCount ;
decfsz xCount,f ;
goto $-1 ; loop here until time complete
return
LockTest
;*****************************************************************
;
; T is the commutation phase period. Back EMF is measured on the
; floating motor terminal at two times during T to determine
; the approximate zero crossing of the BEMF. BEMF low means that
; the measured BEMF is below (supply voltage)/2.
; If BEMF is low at 1/4 T then accelerate.
; If BEMF is high at 1/4 T and low at 3/4 T then speed is OK.
; If BEMF is high at 1/4 T and 3/4 T then decelerate.
;
; Lock test computation is synchronized to the PWM clock such
; that the computation is performed during the PWM ON or OFF
; time whichever is longer.
;
;*****************************************************************
; synchronize test with start of Timer0
btfss Tmr0Ovf ; has Timer0 wrapped around?
return ; no - skip lock test
btfss PWMThresh,7 ; if PWMThresh > 0x80 then ON is longer than OFF
goto LT05 ; OFF is longer and motor is currently off - compute now
btfss DriveOnFlag ; ON is longer - wait for drive cycle to start
return ; not started - wait

LT05
bcf Tmr0Ovf ; clear synchronization flag
decfsz RampTimer,f ; RampTimer controls the acceleration/deceleration rate
return
; use lock results to control RPM only if not manual mode
bsf AutoRPM ; preset flag
movf ADCRPM,w ; compare RPM potentiometer...
addlw AutoThresh ; ...to the auto control threshold
btfss CARRY ; CARRY is set if RPM is > auto threshold
bcf AutoRPM ; not in auto range - reset flag
btfss BEMF1Low ; is first BEMF below Supply/2
goto LT20 ; no - test second BEMF
LT10
; accelerate if BEMF at 1/4 T is below Supply/2
movlw B'10000000' ; indicate lock test results
movwf Status ; status is OR'd with drive word later
movlw AccelDelay ; set the timer for acceleration delay
movwf RampTimer ;
btfss AutoRPM ; is RPM in auto range?
goto ManControl ; no - skip RPM adjustment
incfsz RPMIndex,f ; increment the RPM table index
return ; return if Index didn't wrap around
decf RPMIndex,f ; top limit is 0xFF
return
LT20
btfsc BEMF2Low ; BEMF1 was high...
goto ShowLocked ; ... and BEMF2 is low - show locked
; decelerate if BEMF at 3/4 T is above Supply/2
movlw B'01000000' ; indicate lock test results
movwf Status ; status is OR'd with drive word later
movlw DecelDelay ; set the timer for deceleration delay
movwf RampTimer ;
btfss AutoRPM ; is RPM in auto range?
goto ManControl ; no - skip RPM adjustment
decfsz RPMIndex,f ; set next lower RPM table index
return ; return if index didn't wrap around
incf RPMIndex,f ; bottom limit is 0x01
return
ShowLocked
movlw B'11000000' ; indicate lock test results
movwf Status ; status is OR'd with drive word later
movlw DecelDelay ; set the timer for deceleration delay
movwf RampTimer ;
btfsc AutoRPM ; was RPM set automatically?
return ; yes - we're done

ManControl
movf ADCRPM,w ; get RPM potentiometer reading...
movwf RPMIndex ; ...and set table index directly
return
Commutate
;*****************************************************************
;
; Commutation is triggered by PIR1<CCP1IF> flag.
; This flag is set when timer1 equals the compare register.
; When BEMF measurement is active the compare time is not
; cleared automatically (special event trigger is off).
; Ignore the PIR1<CCP1IF> flag when special trigger is off
; because the flag is for BEMF measurement.
; If BEMF measurement is not active then decrement phase table
; index and get the drive word from the table. Save the
; drive word in a global variable and output to motor drivers.
;
;*****************************************************************
btfss CCP1CON,0 ; is special event on compare enabled?
return ; no - this is a BEMF measurement, let state machine handle this
bcf PIR1,CCP1IF ; clear interrupt flag
movlw high OnTable ; set upper program counter bits
movwf PCLATH
decfsz PhaseIndx,w ; decrement to next phase
goto $+2 ; skip reset if not zero
movlw D'6' ; phase counts 6 to 1
movwf PhaseIndx ; save the phase index
addlw LOW OnTable
btfsc CARRY ; test for possible page boundary
incf PCLATH,f ; page boundary adjust
call GetDrive
movwf Drive ; save motor drive word
DriveMotor
movf Drive,w ; restore motor drive word
btfss DriveOnFlag ; test drive enable flag
andlw OffMask ; kill high drive if PWM is off
iorwf Status,w ; show speed indicators
movwf DrivePort ; output to motor drivers
return
GetDrive
movwf PCL ; computed goto
OnTable
retlw Invalid
retlw Phase6
retlw Phase5
retlw Phase4
retlw Phase3
retlw Phase2
retlw Phase1
retlw Invalid
SetTimer

;*****************************************************************
;
; This sets the CCP module compare registers for timer 1.
; The motor phase period is the time it takes timer 1
; to count from 0 to the compare value. The CCP module
; is configured to clear timer 1 when the compare occurs.
; Get the timer1 compare variable from two lookup tables, one
; for the compare high byte and the other for the low byte.
;
;*****************************************************************
call SetTimerHigh
movwf CCPR1H ; Timer1 High byte preset
call SetTimerLow
movwf CCPR1L ; Timer1 Low byte preset
return
SetTimerHigh
movlw high T1HighTable ; lookup preset values
movwf PCLATH ; high bytes first
movlw low T1HighTable ;
addwf RPMIndex,w ; add table index
btfsc STATUS,C ; test for table page crossing
incf PCLATH,f ;
movwf PCL ; lookup - result returned in W
SetTimerLow
movlw high T1LowTable ; repeat for lower byte
movwf PCLATH ;
movlw low T1LowTable ;
addwf RPMIndex,w ; add table index
btfsc STATUS,C ; test for table page crossing
incf PCLATH,f ;
movwf PCL ; lookup - result returned in W
#include "BLDCspd4.inc"
end

; file : BLDCSpd4.inc
;
; NOTE: This file is built using the Excel 
;       "BLDC Table Generator" Spreadsheet
;       Enter the motor specifics in the spreadsheet
;       then cut the resulting MS Byte code and LS Byte code 
;       from the spreadsheet and paste to the respective tables.
;
; BLDC speed lookup table
; MaxRPM = 8000
; MinRPM = 96
; Phases per mechanical revolution = 12
; Fosc = 20 MHz
; Timer 1 prescale = 4
; 0 RPM Axis intercept = -345 RPM
;

T1HighTable        ; MS Byte code
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'64854'
   retlw high D'49189'
   retlw high D'39062'
   retlw high D'32393'
   retlw high D'27669'
   retlw high D'24147'
   retlw high D'21421'
   retlw high D'19248'
   retlw high D'17475'
   retlw high D'16001'
   retlw high D'14756'
   retlw high D'13692'
   retlw high D'12770'
   retlw high D'11965'
   retlw high D'11255'
   retlw high D'10625'
   retlw high D'10061'
   retlw high D'9554'
   retlw high D'9096'
   retlw high D'8680'
   retlw high D'8300'
   retlw high D'7952'
   retlw high D'7632'
   retlw high D'7337'
   retlw high D'7064'
   retlw high D'6810'
   retlw high D'6574'
   retlw high D'6354'
   retlw high D'6148'
   retlw high D'5955'
   retlw high D'5774'
   retlw high D'5603'
   retlw high D'5443'
   retlw high D'5291'
   retlw high D'5147'
   retlw high D'5011'
   retlw high D'4882'
   retlw high D'4760'
   retlw high D'4643'
   retlw high D'4532'
   retlw high D'4427'
   retlw high D'4326'
   retlw high D'4229'
   retlw high D'4137'
   retlw high D'4049'
   retlw high D'3964'
   retlw high D'3883'
   retlw high D'3805'
   retlw high D'3730'
   retlw high D'3658'
   retlw high D'3589'
   retlw high D'3522'
   retlw high D'3458'
   retlw high D'3396'
   retlw high D'3336'
   retlw high D'3279'
   retlw high D'3223'
   retlw high D'3169'
   retlw high D'3117'
   retlw high D'3067'
   retlw high D'3018'
   retlw high D'2971'
   retlw high D'2925'
   retlw high D'2880'
   retlw high D'2837'
   retlw high D'2796'
   retlw high D'2755'
   retlw high D'2716'
   retlw high D'2677'
   retlw high D'2640'
   retlw high D'2604'
   retlw high D'2568'
   retlw high D'2534'
   retlw high D'2501'
   retlw high D'2468'
   retlw high D'2436'
   retlw high D'2406'
   retlw high D'2375'
   retlw high D'2346'
   retlw high D'2317'
   retlw high D'2289'
   retlw high D'2262'
   retlw high D'2235'
   retlw high D'2209'
   retlw high D'2184'
   retlw high D'2159'
   retlw high D'2135'
   retlw high D'2111'
   retlw high D'2088'
   retlw high D'2065'
   retlw high D'2043'
   retlw high D'2021'
   retlw high D'2000'
   retlw high D'1979'
   retlw high D'1958'
   retlw high D'1938'
   retlw high D'1919'
   retlw high D'1900'
   retlw high D'1881'
   retlw high D'1862'
   retlw high D'1844'
   retlw high D'1826'
   retlw high D'1809'
   retlw high D'1792'
   retlw high D'1775'
   retlw high D'1759'
   retlw high D'1742'
   retlw high D'1727'
   retlw high D'1711'
   retlw high D'1696'
   retlw high D'1681'
   retlw high D'1666'
   retlw high D'1651'
   retlw high D'1637'
   retlw high D'1623'
   retlw high D'1609'
   retlw high D'1596'
   retlw high D'1582'
   retlw high D'1569'
   retlw high D'1557'
   retlw high D'1544'
   retlw high D'1531'
   retlw high D'1519'
   retlw high D'1507'
   retlw high D'1495'
   retlw high D'1483'
   retlw high D'1472'
   retlw high D'1461'
   retlw high D'1449'
   retlw high D'1438'
   retlw high D'1428'
   retlw high D'1417'
   retlw high D'1406'
   retlw high D'1396'
   retlw high D'1386'
   retlw high D'1376'
   retlw high D'1366'
   retlw high D'1356'
   retlw high D'1346'
   retlw high D'1337'
   retlw high D'1328'
   retlw high D'1318'
   retlw high D'1309'
   retlw high D'1300'
   retlw high D'1291'
   retlw high D'1283'
   retlw high D'1274'
   retlw high D'1266'
   retlw high D'1257'
   retlw high D'1249'
   retlw high D'1241'
   retlw high D'1233'
   retlw high D'1225'
   retlw high D'1217'
   retlw high D'1209'
   retlw high D'1201'
   retlw high D'1194'
   retlw high D'1186'
   retlw high D'1179'
   retlw high D'1172'
   retlw high D'1165'
   retlw high D'1157'
   retlw high D'1150'
   retlw high D'1143'
   retlw high D'1137'
   retlw high D'1130'
   retlw high D'1123'
   retlw high D'1117'
   retlw high D'1110'
   retlw high D'1104'
   retlw high D'1097'
   retlw high D'1091'
   retlw high D'1085'
   retlw high D'1078'
   retlw high D'1072'
   retlw high D'1066'
   retlw high D'1060'
   retlw high D'1054'
   retlw high D'1049'
   retlw high D'1043'
   retlw high D'1037'
   retlw high D'1031'
   retlw high D'1026'
   retlw high D'1020'
   retlw high D'1015'
   retlw high D'1009'
   retlw high D'1004'
   retlw high D'999'
   retlw high D'994'
   retlw high D'988'
   retlw high D'983'
   retlw high D'978'
   retlw high D'973'
   retlw high D'968'
   retlw high D'963'
   retlw high D'958'
   retlw high D'954'
   retlw high D'949'
   retlw high D'944'
   retlw high D'939'
   retlw high D'935'
   retlw high D'930'
   retlw high D'926'
   retlw high D'921'
   retlw high D'917'
   retlw high D'912'
   retlw high D'908'
   retlw high D'904'
   retlw high D'899'
   retlw high D'895'
   retlw high D'891'
   retlw high D'887'
   retlw high D'883'
   retlw high D'878'
   retlw high D'874'
   retlw high D'870'
   retlw high D'866'
   retlw high D'862'
   retlw high D'859'
   retlw high D'855'
   retlw high D'851'
   retlw high D'847'
   retlw high D'843'
   retlw high D'840'
   retlw high D'836'
   retlw high D'832'
   retlw high D'829'
   retlw high D'825'
   retlw high D'821'
   retlw high D'818'
   retlw high D'814'
   retlw high D'811'
   retlw high D'807'
   retlw high D'804'
   retlw high D'801'
   retlw high D'797'
   retlw high D'794'
   retlw high D'791'
   retlw high D'787'
   retlw high D'784'
   retlw high D'781'

T1LowTable      ; LS Byte code
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'64854'
   retlw low D'49189'
   retlw low D'39062'
   retlw low D'32393'
   retlw low D'27669'
   retlw low D'24147'
   retlw low D'21421'
   retlw low D'19248'
   retlw low D'17475'
   retlw low D'16001'
   retlw low D'14756'
   retlw low D'13692'
   retlw low D'12770'
   retlw low D'11965'
   retlw low D'11255'
   retlw low D'10625'
   retlw low D'10061'
   retlw low D'9554'
   retlw low D'9096'
   retlw low D'8680'
   retlw low D'8300'
   retlw low D'7952'
   retlw low D'7632'
   retlw low D'7337'
   retlw low D'7064'
   retlw low D'6810'
   retlw low D'6574'
   retlw low D'6354'
   retlw low D'6148'
   retlw low D'5955'
   retlw low D'5774'
   retlw low D'5603'
   retlw low D'5443'
   retlw low D'5291'
   retlw low D'5147'
   retlw low D'5011'
   retlw low D'4882'
   retlw low D'4760'
   retlw low D'4643'
   retlw low D'4532'
   retlw low D'4427'
   retlw low D'4326'
   retlw low D'4229'
   retlw low D'4137'
   retlw low D'4049'
   retlw low D'3964'
   retlw low D'3883'
   retlw low D'3805'
   retlw low D'3730'
   retlw low D'3658'
   retlw low D'3589'
   retlw low D'3522'
   retlw low D'3458'
   retlw low D'3396'
   retlw low D'3336'
   retlw low D'3279'
   retlw low D'3223'
   retlw low D'3169'
   retlw low D'3117'
   retlw low D'3067'
   retlw low D'3018'
   retlw low D'2971'
   retlw low D'2925'
   retlw low D'2880'
   retlw low D'2837'
   retlw low D'2796'
   retlw low D'2755'
   retlw low D'2716'
   retlw low D'2677'
   retlw low D'2640'
   retlw low D'2604'
   retlw low D'2568'
   retlw low D'2534'
   retlw low D'2501'
   retlw low D'2468'
   retlw low D'2436'
   retlw low D'2406'
   retlw low D'2375'
   retlw low D'2346'
   retlw low D'2317'
   retlw low D'2289'
   retlw low D'2262'
   retlw low D'2235'
   retlw low D'2209'
   retlw low D'2184'
   retlw low D'2159'
   retlw low D'2135'
   retlw low D'2111'
   retlw low D'2088'
   retlw low D'2065'
   retlw low D'2043'
   retlw low D'2021'
   retlw low D'2000'
   retlw low D'1979'
   retlw low D'1958'
   retlw low D'1938'
   retlw low D'1919'
   retlw low D'1900'
   retlw low D'1881'
   retlw low D'1862'
   retlw low D'1844'
   retlw low D'1826'
   retlw low D'1809'
   retlw low D'1792'
   retlw low D'1775'
   retlw low D'1759'
   retlw low D'1742'
   retlw low D'1727'
   retlw low D'1711'
   retlw low D'1696'
   retlw low D'1681'
   retlw low D'1666'
   retlw low D'1651'
   retlw low D'1637'
   retlw low D'1623'
   retlw low D'1609'
   retlw low D'1596'
   retlw low D'1582'
   retlw low D'1569'
   retlw low D'1557'
   retlw low D'1544'
   retlw low D'1531'
   retlw low D'1519'
   retlw low D'1507'
   retlw low D'1495'
   retlw low D'1483'
   retlw low D'1472'
   retlw low D'1461'
   retlw low D'1449'
   retlw low D'1438'
   retlw low D'1428'
   retlw low D'1417'
   retlw low D'1406'
   retlw low D'1396'
   retlw low D'1386'
   retlw low D'1376'
   retlw low D'1366'
   retlw low D'1356'
   retlw low D'1346'
   retlw low D'1337'
   retlw low D'1328'
   retlw low D'1318'
   retlw low D'1309'
   retlw low D'1300'
   retlw low D'1291'
   retlw low D'1283'
   retlw low D'1274'
   retlw low D'1266'
   retlw low D'1257'
   retlw low D'1249'
   retlw low D'1241'
   retlw low D'1233'
   retlw low D'1225'
   retlw low D'1217'
   retlw low D'1209'
   retlw low D'1201'
   retlw low D'1194'
   retlw low D'1186'
   retlw low D'1179'
   retlw low D'1172'
   retlw low D'1165'
   retlw low D'1157'
   retlw low D'1150'
   retlw low D'1143'
   retlw low D'1137'
   retlw low D'1130'
   retlw low D'1123'
   retlw low D'1117'
   retlw low D'1110'
   retlw low D'1104'
   retlw low D'1097'
   retlw low D'1091'
   retlw low D'1085'
   retlw low D'1078'
   retlw low D'1072'
   retlw low D'1066'
   retlw low D'1060'
   retlw low D'1054'
   retlw low D'1049'
   retlw low D'1043'
   retlw low D'1037'
   retlw low D'1031'
   retlw low D'1026'
   retlw low D'1020'
   retlw low D'1015'
   retlw low D'1009'
   retlw low D'1004'
   retlw low D'999'
   retlw low D'994'
   retlw low D'988'
   retlw low D'983'
   retlw low D'978'
   retlw low D'973'
   retlw low D'968'
   retlw low D'963'
   retlw low D'958'
   retlw low D'954'
   retlw low D'949'
   retlw low D'944'
   retlw low D'939'
   retlw low D'935'
   retlw low D'930'
   retlw low D'926'
   retlw low D'921'
   retlw low D'917'
   retlw low D'912'
   retlw low D'908'
   retlw low D'904'
   retlw low D'899'
   retlw low D'895'
   retlw low D'891'
   retlw low D'887'
   retlw low D'883'
   retlw low D'878'
   retlw low D'874'
   retlw low D'870'
   retlw low D'866'
   retlw low D'862'
   retlw low D'859'
   retlw low D'855'
   retlw low D'851'
   retlw low D'847'
   retlw low D'843'
   retlw low D'840'
   retlw low D'836'
   retlw low D'832'
   retlw low D'829'
   retlw low D'825'
   retlw low D'821'
   retlw low D'818'
   retlw low D'814'
   retlw low D'811'
   retlw low D'807'
   retlw low D'804'
   retlw low D'801'
   retlw low D'797'
   retlw low D'794'
   retlw low D'791'
   retlw low D'787'
   retlw low D'784'
   retlw low D'781'

//---------------------------------------------------------------------------------
// Software License Agreement
//
// The software supplied herewith by Microchip Technology Incorporated
// (the “Company”) is intended and supplied to you, the Company’s customer,
// for use solely and exclusively with products manufacture by the Company.
// The software is owned by the Company and/or its supplier, and is protected under
// applicable copyright laws. All rights are reserved. Any use in violation of the
// foregoing restrictions may subject the user to criminal sanctions under applicable
// laws, as well as to civil liability for the breach of the terms and conditions of
// this icense.
//
// THIS SOFTWARE IS PROVIDED IN AN “AS IS” CONDITION. NO WARRANTIES, WHETHER EXPRESS,
// IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
// THE COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR
// CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
//---------------------------------------------------------------------------------
// File: ClosedLoopSenBLDC.c
//
// Written By:Stan D'Souza, Microchip Technology
//
// The following files should be included in the MPLAB project:
//
// ClosedLoopSenBLDC.c-- Main source code file
// p30f2010.gld-- Linker script file
//
//---------------------------------------------------------------------
// Revision History
//
// 10/01/04 -- first version
//---------------------------------------------------------------------
//***************************************************************************
ClosedLoopSenBLDC.c is a sensored Closed Loop Control for a BLDC motor.
The task consists of the following:
Sense changes in the Hall Sensors connected to CN5,6 & 7 (PortB)
During the CNInterrupt, read the sensors input by reading PortB
Mask and determine the state of the position 1, 2, ... 6.
Use the StateLoTable and the lookup table provided to determine the
Overload Control Register value. Set the OVDCON to this value.
The PWM is initialized to generate independant continuous PWMs.
The value of the Pot REF is used to determine the demand or desired
speed of the Motor. The desired speed value is then used with the
actual speed value to determine the Proportional Speed Error and the
Integral Speed Error. With these two values the new DutyCycle is determined
as: NewDutyCycle = Kp*(Portportional Speed Error) + Ki*(Integral Speed Error)
All 3 PWM Duty cycles are then loaded with the NewDutyCycloe 10-bit value.
The FPWM = 16000hz
The ADC is setup for a PWM trigger to start the conversion
********************************************************************************/

#define __dsPIC30F2010__
#include "c:\pic30_tools\support\h\p30F2010.h"
#define FCY 10000000// xtal = 5.0Mhz; PLLx8
#define MILLISEC FCY/10000// 1 mSec delay constant
#define FPWM 16000
#define Ksp1200
#define Ksi10
#define RPMConstant60*(FCY/256)
#define S2!PORTCbits.RC14
void InitTMR3(void);
void InitADC10(void);
void AverageADC(void);
void DelayNmSec(unsigned int N);
void InitMCPWM(void);
void CalculateDC(void);
void GetSpeed(void);
struct {
unsigned RunMotor : 1;
unsigned Minus : 1;
unsigned unused : 14;
} Flags;
unsigned int HallValue;
int Speed;
unsigned int Timer3;
unsigned char Count;
unsigned char SpeedCount;
int DesiredSpeed;
int ActualSpeed;
int SpeedError;
int DutyCycle;
int SpeedIntegral;
//*************************************************************
Low side driver table is as below. In this StateLoTable,
the Low side driver is PWM while the high side driver is
either on or off. This table is used in this exercise
*************************************************************/
unsigned int StateLoTable[] = {0x0000, 0x1002, 0x0420, 0x0402,
0x0108, 0x1008, 0x0120, 0x0000};
/****************************************************************
Interrupt vector for Change Notification CN5, 6 and 7 is as below.
When a Hall sensor changes states, an interrupt will be caused which
will vector to the routine below.
The user has to then read the PORTB, mask bits 3, 4 and 5,
shift and adjust the value to read as 1, 2 ... 6. This
value is then used as an offset in the lookup table StateLoTable
to determine the value loaded in the OCDCON register
*****************************************************************/

void _ISR _CNInterrupt(void)
{
IFS0bits.CNIF = 0; // clear flag
HallValue = PORTB & 0x0038; // mask RB3,4 & 5
HallValue = HallValue >> 3; // shift right 3 times
OVDCON = StateLoTable[HallValue];// Load the overide control register
}
/*********************************************************************
The ADC interrupt loads the DesiredSpeed variable with the demand pot
value. This is then used to determing the Speed error. When the motor
is not running, the PDC values use the direct Demand value from the pot.
*********************************************************************/
void _ISR _ADCInterrupt(void)
{
IFS0bits.ADIF = 0;
DesiredSpeed = ADCBUF0;
if (!Flags.RunMotor)
{
PDC1 = ADCBUF0; // get value ...
PDC2 = PDC1; // and load all three PWMs ...
PDC3 = PDC1; // duty cycles
}
}
/************************************************************************
The main routine controls the initialization, and the keypress to start
and stop the motor.
************************************************************************/
int main(void)
{
LATE = 0x0000;
TRISE = 0xFFC0; // PWMs are outputs
CNEN1 = 0x00E0; // CN5,6 and 7 enabled
CNPU1 = 0x00E0; // enable internal pullups
IFS0bits.CNIF = 0; // clear CNIF
IEC0bits.CNIE = 1; // enable CN interrupt
SpeedError = 0;
SpeedIntegral = 0;
InitTMR3();
InitMCPWM();
InitADC10();
while(1)
{
while (!S2); // wait for start key hit
while (S2) // wait till key is released
DelayNmSec(10);
// read hall position sensors on PORTB
HallValue = PORTB & 0x0038; // mask RB3,4 & 5
HallValue = HallValue >> 3; // shift right to get value 1, 2 ... 6
OVDCON = StateLoTable[HallValue];// Load the overide control register
PWMCON1 = 0x0777; // enable PWM outputs
Flags.RunMotor = 1; // set flag
T3CON = 0x8030; // start TMR3
while (Flags.RunMotor) // while motor is running
if (!S2) // if S2 is not pressed

{
if (HallValue == 1) //IF in sector 1
{
HallValue = 0xFF; // force a new value as a sector
if (++Count == 5) // do this for 5 electrical revolutions or 1
// mechanical revolution for a 10 pole motor
{
Timer3 = TMR3;// read latest tmr3 value
TMR3 = 0;
Count = 0;
GetSpeed();// determine spped
}
}
}
else // else S2 is pressed to stop motor
{
PWMCON1 = 0x0700;// disable PWM outputs
OVDCON = 0x0000; // overide PWM low.
Flags.RunMotor = 0;// reset run flag
while (S2)// wait for key release
DelayNmSec(10);
}
} // end of while (1)
}
/*******************************************************************
Below is the code required to setup the ADC registers for :
1. 1 channel conversion (in this case RB2/AN2)
2. PWM trigger starts conversion
3. Pot is connected to CH0 and RB2
4. Manual Stop Sampling and start converting
5. Manual check of Conversion complete
*********************************************************************/
void InitADC10(void)
{
ADPCFG = 0xFFF8; // all PORTB = Digital;RB0 to RB2 = analog
ADCON1 = 0x0064; // PWM starts conversion
ADCON2 = 0x0000; // sample CH0 channel
ADCHS = 0x0002; // Connect RB2/AN2 as CH0 = pot.
ADCON3 = 0x0080; // Tad = internal RC (4uS)
IFS0bits.ADIF = 0; // clear flag
IEC0bits.ADIE = 1; // enable interrupt
ADCON1bits.ADON = 1; // turn ADC ON
}

/********************************************************************
InitMCPWM, intializes the PWM as follows:
1. FPWM = 16000 hz
2. Independant PWMs
3. Control outputs using OVDCON
4. Set Duty Cycle using PI algorithm and Speed Error
5. Set ADC to be triggered by PWM special trigger
*********************************************************************/
void InitMCPWM(void)
{
PTPER = FCY/FPWM - 1;
PWMCON1 = 0x0700; // disable PWMs
OVDCON = 0x0000; // allow control using OVD
PDC1 = 100; // init PWM 1, 2 and 3 to 100
PDC2 = 100;
PDC3 = 100;
SEVTCMP = PTPER; // special trigger is 16 period values
PWMCON2 = 0x0F00; // 16 postscale values
PTCON = 0x8000; // start PWM
}
/************************************************************************
Tmr3 is used to determine the speed so it is set to count using Tcy/256
*************************************************************************/
void InitTMR3(void)
{
T3CON = 0x0030; // internal Tcy/256 clock
TMR3 = 0;
PR3 = 0x8000;
}
/************************************************************************
GetSpeed, determins the exact speed of the motor by using the value in
TMR3 for every mechanical cycle.
*************************************************************************/
void GetSpeed(void)
{
if (Timer3 > 23000) // if TMR3 is large ignore reading
return;
if (Timer3 > 0)
Speed = RPMConstant/(long)Timer3;// get speed in RPM
ActualSpeed += Speed;
ActualSpeed = ActualSpeed >> 1;
if (++SpeedCount == 1)
{SpeedCount = 0;CalculateDC();}
}

/*****************************************************************************
CalculateDC, uses the PI algorithm to calculate the new DutyCycle value which
will get loaded into the PDCx registers.
****************************************************************************/
void CalculateDC(void)
{
DesiredSpeed = DesiredSpeed*3;
Flags.Minus = 0;
if (ActualSpeed > DesiredSpeed)
SpeedError = ActualSpeed - DesiredSpeed;
else
{
SpeedError = DesiredSpeed - ActualSpeed;
Flags.Minus = 1;
}
SpeedIntegral += SpeedError;
if (SpeedIntegral > 9000)
SpeedIntegral = 0;
DutyCycle = (((long)Ksp*(long)SpeedError + (long)Ksi*(long)SpeedIntegral) >> 12);
DesiredSpeed = DesiredSpeed/3;
if (Flags.Minus)
DutyCycle = DesiredSpeed + DutyCycle;
else DutyCycle = DesiredSpeed - DutyCycle;
if (DutyCycle < 100)
DutyCycle = 100;
if (DutyCycle > 1250)
{DutyCycle = 1250;SpeedIntegral = 0;}
PDC1 = DutyCycle;
PDC2 = PDC1;
PDC3 = PDC1;
}
//---------------------------------------------------------------------
// This is a generic 1ms delay routine to give a 1mS to 65.5 Seconds delay
// For N = 1 the delay is 1 mS, for N = 65535 the delay is 65,535 mS.
// Note that FCY is used in the computation. Please make the necessary
// Changes(PLLx4 or PLLx8 etc) to compute the right FCY as in the define
// statement above.
void DelayNmSec(unsigned int N)
{
unsigned int j;
while(N--)
for(j=0;j < MILLISEC;j++);
}