; driver for MDP5011

; (c) copyright GP Eckersley
; released under terms of
; GNU General Public License Version 3 as published by
;    the Free Software Foundation


;  Id: pic18pwm.a18,v 1.13 2010/12/28 21:13:57 daddio Exp $

; works, no voltage control 000, 
; bugs pictalk  001 

;  Id: pic18pwm.a18,v 1.16 2010/12/29 22:35:50 daddio Exp $
; fixed stack set

;  Id: pic18pwm.a18,v 1.18 2010/12/30 03:16:05 daddio Exp $
; set scaling for 16 v dc supply

;  Id: pic18pwm.a18,v 1.23 2011/01/01 11:39:57 daddio Exp $
;  Tested with 16 volt supply runs reqttm=0x330 @ 20 Torr
; to run turn on eninc and enfb in aflags

;  Id: pic18pwm.a18,v 1.26 2011/01/03 23:22:11 daddio Exp $
;   starts with no overcuurent or glitches  if sequenced aflags 0.4,0e

;  Id: pic18pwm.a18,v 1.29 2011/01/10 09:08:11 daddio Exp $

; macros for add/acc/shift

;  Id: pic18pwm.a18,v 1.33 2011/01/10 11:38:32 daddio Exp $

; reasonably accurate version

;  Id: pic18pwm.a18,v 1.36 2011/01/14 11:19:01 daddio Exp $
; slews to 116 without much overshoot
;  $Id: pic18pwm.a18,v 1.37 2011/01/14 13:09:34 daddio Exp $


; Improvements needed
; Back-EMF is too course
; PWM to fractional values
; Startup sequence
; Led speed indicator
; Analog inputs

; list e=2
; list p=16c84
;list p=16c84

 include <picinc18f.a18>

;   internal ram allocations

; Following are general purpose storage
mpad = 020
rsm macro lbl,cnt
lbl equ mpad
mpad = mpad+cnt
  endm

 udata
tmp0 res 1
tmp1 res 1
tmp2 res 1
tmp3 res 1
tmp4 res 1
tmp5 res 1
tmp6 res 1
tmp7 res 1
savew res 1
savest res 1
savefsr res 2
sampleptr res 1


clock_time equ 80 ; 8 microsec; / 64 ;14 is about minimum, 40(064) is  64 * 16 = 4 microsec/cycle
; This is the main interrupt cycle time  T_int
;  

power_flags res 1

vvtmp0 res 1 ; Stores data to be sent
vvtmp1 res 1 ; Following are general purpose storage
vvtmp2 res 1
vvtmp3 res 1
vvtmp4 res 1
vvtmp5 res 1
vvtmp6 res 1
pntb equ 3         ;     no of bits for current table address 
pntc equ 1<<pntb   ;     no of current points
ivals res pntc
offsang res 1
rateinc res 3
angle res 4
rate res 4 ; highest byte only for carries
mangle res 4
turndun equ 0   ; completed turn flag in aflags
prvinp equ 1    ; prev tacho flag
eninc equ 2     ; enable rate inc
enfb equ 3      ; enable speed/turntime feedback
autostart equ 4 ; run start sequence automatically
motoroff equ 5  ; stop
aflags res 1
scanstate res 1
clock_0 res 4 ; clock tick counter , incremented every clock_time

turnclock0 res 3  
turnclock1 res 3  
ttm res 3  
reqttm res 3  
errttm res 3

backemf res 1 ; back emf in units of Vsupply/0x40

tick1 res 1
cycnt res 1
ivv res 1         ; initial voltage value
actvv res 1       ; measured dc voltage value
drvvv res 1      ; output pwm drive value
indrvvv res 4      ; output pwm drive integrator value
vovers res 1
uprmargin res 1    ; max positive - backemf
lwrmargin res 1    ; max negative - backemf
pwmmax equ 040     ; pwm cycle count

mpyr res 2
mpand res 3
mulres res 6
intfb res 1     ; scaling for integrating feedback feedback 
bug1 res 1
adcres res 16


 code
pwron equ 0 ; = 1 to enable supply 

pwrtrig = 2 ; bit in port c to fire pwr. sup. flip/flop


pls macro 
  bsf ledport2,ledbit
;  nop
  bcf ledport2,ledbit
 endm

ledport2=porta
ledtris2=trisa
ledbit = 3
;ledport2=porte
;ledtris2=trise
;ledbit = 1

ledtris=trise
ledport=porte
; pg1
onboardled=2

pwmctris=trisc
pwmcport=portc
pwmcout=2

pwmtris=trisc   ; this can change to rb3 if selected in cfg.a18
pwmport=portc
pwmout=1


rsttris=trisd
rstport=portd
rstout=4

 org 0
 goto  boot ; Normal start vector for the 16C84 org 4
 if 1==1
 nop
 nop
 goto int01

 nop
 nop
 nop
 nop
 nop
 nop
 else
 org  8       ; low priority I.V.
 goto int01
 org 018
 endif
; goto int01  ; low priority I.V.


bitout = 5
ledbit2 = 2
b5 = 5
b2 = 2
b1 = 1

savctxt macro
 movwf savew
 swapf status,w
; pg0
 movwf savest
 endm

rgetctxt macro
 swapf savest,w
 movwf status
 swapf savew,f
 swapf savew,w
 pls
 retfie
 endm

adcmask2 equ (6<<adcs0)+(1<<acqt0)+(0<<adfm)  ; clock select 1muS - 2 cycle acq. -
adcmask0 equ (1<<go)+(1<<adon) ; channel select 4 bits masked to chs0
 
int01
intdec ;     None as yet

 pls
 savctxt
; pg0
 bcf pir1,tmr2if
 movlw 0
 incf clock_0,f
 addwfc clock_0+1,f
 addwfc clock_0+2,f
 addwfc clock_0+3,f
 jbs clock_0,0,doadc 

maxtime=1 
 jbs angle+3,maxtime,angsat  ; angle reached limit 2**(16+maxtime+1)
 clrwdt
 movlw 0
;  incf turnclock1
; addwfc turnclock1+1,f
; addwfc turnclock1+2,f

carryb equ tmp5
radish
 movfw rate+0
 addwf angle+0,f
 movfw rate+1
 addwfc angle+1,f
 movfw rate+2
 addwfc angle+2,f
; clrf angle+3
; btfss angle+3,7  ; limit at 080h
 skcc
 incf angle+3,f

ptin equ 1
 jbs aflags,prvinp,pset
 jbc porte,ptin,not1
 bsf aflags,prvinp
angsat
 bsf aflags,turndun
 movff angle+0,mangle+0
 movff angle+1,mangle+1
 movff angle+2,mangle+2
 movff angle+3,mangle+3
 clrf angle+0
 clrf angle+1
 clrf angle+2
 clrf angle+3
 movff clock_0+0,turnclock0+0
 movff clock_0+1,turnclock0+1
 movff clock_0+2,turnclock0+2
 goto not1
pset
  skbs porte,ptin
  bcf aflags,prvinp
not1

 


 rgetctxt

doadc ; every t*2=16 mus here
 movff fsr0l,savefsr
 movff fsr0h,savefsr+1
 movfw sampleptr
 andlw 7
 addwf sampleptr,w
 incf sampleptr,f
 call getnc 
 movwf tmp0
; movwf tmp1
 rlcf tmp0,w
 lfsr 0,adcres 
 andlw 7<<1
 addwf fsr0l,f
 skcc
 incf fsr0h,f
 movff adresl,postinc0
 movff adresh,indf0
 rrcf tmp0,f
 rrcf tmp0,w
 iorlw (1<<go)+(1<<adon)
 movwf adcon0 ; set channel,go,adcon
 
 
 
 movff savefsr,fsr0l
 movff savefsr+1,fsr0h
 rgetctxt

getnc addwf pcl,f
  retlw 010  ; hs nibble=next mux-ptr; ls nibble=current result address
  retlw 021
  retlw 032
  retlw 043
  retlw 054
  bra rr1
  bra rr1
  bra rr1
rr1
  clrf sampleptr
  retlw 05
  

boot
 clrwdt   ; Clear watchdog timer
 clrf bsr
 	; Initialize the stack pointer
	lfsr 1, _stack_end
	lfsr 2, _stack_end
;;;	; 1st silicon does not do this on POR
;;;	clrf TBLPTRU, 0
;;;	; Initialize the flash memory access configuration.
;;;	; This is harmless for non-flash devices, so we do it on all parts.
;;;	bsf 0xa6, 7, 0 ; EECON1.EEPGD = 1, TBLPTR accesses program memory
;;;	bcf 0xa6, 6, 0 ; EECON1.CFGS = 0, TBLPTR accesses program memory
	


 bcf ledtris,onboardled
 bcf ledtris2,ledbit


 movlf 0c0,intcon  ; 0e0 for tmr0
 clrf intcon2
 clrf intcon3


 bcf rsttris,rstout
 bsf rstport,rstout

;=================   initialize pwm1  =================

;  bsf pwmtris,pwmout
cytime equ 080 ; cycle time in units=fclock/4, eg 64mhz,080 is 128/16=8 microsecond/cycle (125kHz)
  movlf cytime-1,pr2      
  movlf 0c,ccp2con  ; pwm mode upper 2 bits of pw set to 0
  movlf 10,ccpr2l
  bcf pir1,tmr2if
  movlf 0,pir1
  movlf 0,pir2
  movlf 4,t2con ; timer on (4) ; p167
  bsf t2con,tmr2on
wtovflow skbc pir1,tmr2if
  goto wtovflow
;  bcf pwmtris,pwmout


;;  initialize pwm2
;  bsf pwmctris,pwmcout
  movlf 0c,ccp1con  ; pwm mode upper 2 bits of pw set to 0
  movlf pwmmax,ccpr1l
  movlf 1,pstrcon
;  bcf pwmctris,pwmcout
;  clrf trisc
  movlf 0ff-((1<<pwmout)+(1<<pwmcout)),trisc

;==========================================
;  ===== Interrupt on each PWM cycle ======
  clrf pie2
  movlf 1<<tmr2ie,pie1
;  ========================================
;  ====         Start ADC =================

 movlf 01f,ansel ; An0 to an4 on
 movlf adcmask2,adcon2 
 clrf adcon1
 clrf sampleptr
 movlf adcmask0,adcon0 
;===========================================
motpol equ 0
tacho equ 1

  movlf 094,offsang    ; sensor to mag axis
  movlf 060,rate
  movlf 01,rate+1
  movlf 0,rate+2
  movlf 0,aflags
;===========================================
  movlf 0004,intfb   ; initial slow value

;===========================================
  movlf 086,bug1  ; version check
;===========================================

;  table made by genitab.py
 movlf 00,ivals+0
 movlf 032,ivals+1
 movlf 063,ivals+2
 movlf 090,ivals+3
 movlf 0b7,ivals+4
 movlf 0d8,ivals+5
 movlf 0f0,ivals+6
 movlf 0ff,ivals+7

;===========================================
 if 1==0
   movlf 03a,actvv ; 16v;40v 0x91;  later get from adc 255 is 70v
;   movlf 010,ivv ; initial voltage scaling
 else
   movlf 091,actvv ; 16v;40v 0x91;  later get from adc 255 is 70v
;   movlf 08,ivv ; initial voltage scaling
 endif 
 movlw 0b5
 mulwf actvv
 movff prodh,vovers  ; per unit volts/rate ; 03f / upper bits of rate
; movlf 029,vovers ; per unit volts/rate ; 03f / upper bits of rate
; for reqttm=0x600 , rate measured at 0x5520; backemf=0x19
; rrate=0x5520/(16e-6*(1<<24));rttm=1.0/(0x600*8e-6);print rrate,rttm

;===========================================


;============ Set time/speed ===============
; 0x600 is 81 hz,0x116 is 450 Hz
;ttm=hex(int(1.0/(450*8e-6)+0.5));print ttm

  movlf 0,reqttm+0
  movlf 06,reqttm+1
  movlf 0,reqttm+2

;===========================================

;


 extern _hndshkio

 goto _hndshkio

 global _scanit  
 global _scanit2 ; meassage entry point  

_scanit2
  bcf ledport,onboardled
  nop
  nop
  bsf ledport,onboardled
;  return
  
tss macro cod,tgt
  movfw scanstate
  sublw cod
  jz tgt
  endm

_scanit
  clrwdt
  tss 0,startscan
  tss 1,runscan

startscan  
 movff clock_0+1,tick1
 clrf cycnt
 incf scanstate,f
 goto scandun



dubprod macro 
 movfw prodl
 addwf prodl,f
 movfw prodh
 addwfc prodh,f
 endm

negn  macro prma,num
;  negate n bytes
lctr = 0  
  while lctr < num
    comf prma+lctr,f
lctr = lctr+1
  endw
  incf prma+0,f
lctr = 1  
  while lctr < num
    skcc
    incf prma+lctr,f
lctr = lctr+1
  endw
  endm



subn  macro prma,prmb,prmc,num
;  add n bytes
lctr = 1  
  movfw prmb+0
  subwf prma+0,w
  movwf prmc+0
  while lctr < num
    movfw prmb+lctr
    subwfb prma+lctr,w
    movwf prmc+lctr
lctr = lctr+1
  endw
  endm

addn  macro prma,prmb,prmc,num
;  add n bytes
lctr = 1  
  movfw prmb+0
  addwf prma+0,w
  movwf prmc+0
  while lctr < num
    movfw prmb+lctr
    addwfc prma+lctr,w
    movwf prmc+lctr
lctr = lctr+1
  endw
  endm


accn  macro prma,prmb,num
;  add n bytes
lctr = 1  
  movfw prma+0
  addwf prmb+0,f
  while lctr < num
   movfw prma+lctr 
   addwfc prmb+lctr,f
lctr = lctr+1
  endw
  endm

 
movn  macro prma,prmb,num
;  move n bytes
lctr = num-1  
  while lctr>=0
   movff prma+lctr,prmb+lctr
lctr = lctr-1
  endw
  endm

 
  
shrn  macro prma,num
; signed shift of num bits to the right
lctr = num-1  
  rlcf prma+lctr,w  ; sign bit to carry 
  while lctr>=0
   rrcf prma+lctr,f
lctr = lctr-1
  endw
  endm


shln  macro prma,num
;  shift of num bits to the left
lctr = 0
  clc 
  while lctr < num
   rlcf prma+lctr,f
lctr = lctr+1
  endw
  endm



mulh1 macro totres,prma,prmact,prmb,lprmb,result,addsub,addsubc
prmbct = 0
    while prmbct<lprmb
      movfw prma+prmact
      mulwf prmb+prmbct
      movfw prodl
resptr = prmact+prmbct      
      addsub result+resptr,f
resptr = resptr+1      
      movfw prodh
      addsubc result+resptr,f
resptr = resptr+1
  mulh2 totres,result,addsubc
prmbct = prmbct+1
    endw
  endm


tcall macro prma
  prma
  endm

mulh2 macro totres,result,addsubc
      if resptr<=totres
        movlw 0
      endif  
      while resptr < totres
        addsubc result+resptr,f
resptr = resptr+1      
      endw  
  endm

clrbn macro prma,totlen  
pctr = 0
 while pctr<totres
   clrf result+pctr
pctr+=1
 endw
 endm

mulmn macro   prma,lprma,prmb,lprmb,result
totres = lprma+lprmb; length of result
 clrbn result,totres
 mulaccmn prma,lprma,prmb,lprmb,result
 endm

mulaccdecmn macro prma,lprma,prmb,lprmb,result,addsub,addsubc
; multiply and accumulate/decrement
prmact = 0
 while prmact<lprma  
;  directly nested loops don't work, so use a nested macro call
    mulh1 totres,prma,prmact,prmb,lprmb,result,addsub,addsubc
prmact = prmact+1
  endw
  endm


mulaccmn macro prma,lprma,prmb,lprmb,result
; multiply and accumulate
 mulaccdecmn prma,lprma,prmb,lprmb,result,addwf,addwfc
  endm

muldecmn macro prma,lprma,prmb,lprmb,result
; multiply and accumulate
 mulaccdecmn prma,lprma,prmb,lprmb,result,subwf,subwfb
  endm



getvinv ;   Invert voltage to get pu/volt 
  movfw actvv  ; 2000h/actvv
  mulwf tmp2
  movlw 0
  subwf prodl,f
  movlw 020
  subwfb prodh,f
  movlw 0ff
  skcs
  movlw 1
  addwf tmp2,f 
;==  Initialise voltage control limits  ====
 swapf tmp2,w
 andlw 0f
 movwf ivv
 addwf ivv,f

 movff ivv,lwrmargin   
 clc
 rlcf lwrmargin,f 
 rlcf lwrmargin,f 
 movff lwrmargin,uprmargin    

 movlw 049
 mulwf tmp2
 movff prodh,vovers  ; voltage is in pu/speed

;===========================================
 return


runscan

 call getvinv


;==================== Output PWM   =================
polarity equ 0
 movfw offsang
 addwf angle+2,w   ; fraction of 1 turn
 movwf tmp3
 movfw tmp3
 btfsc tmp3,6
 comf  tmp3,w
 movwf tmp1
 rlcf tmp1,f
 swapf tmp1,w
 andlw 7
 lfsr 0,ivals
 addwf fsr0l,f 
 skcc
 incf fsr0h,f
 movff indf0,tmp4
 movfw indf0
 mulwf drvvv,w
 movff prodh,tmp6
 movlw 040
 jbs tmp3,7,posout
 comf prodh,f ; polarity
 incf prodh,f
posout
 addwf prodh,w
 movwf ccpr1l

 jbc aflags,turndun,noinc3
 bcf aflags,turndun
; here on end of turn
; time difference turn to turn
  movfw turnclock1+0
  subwf turnclock0+0,w
  movwf ttm+0
  movfw turnclock1+1
  subwfb turnclock0+1,w
  movwf ttm+1
  movfw turnclock1+2
  subwfb turnclock0+2,w
  movwf ttm+2
  movff turnclock0+0,turnclock1+0
  movff turnclock0+1,turnclock1+1
  movff turnclock0+2,turnclock1+2

  



;==================== update voltage to suit speed ==================
  
  movlw 1<<5 ; shift in upper 3 bits ; use mul as 2 byte shifter
  mulwf rate+2
  movff prodl,tmp7
  movlw 1<<5 ; shift in upper 5 bits
  mulwf rate+1
  movfw prodh
  addwf tmp7,w
  movwf tmp7   ; 0x10 at ttm= 0x3f0
  mulwf vovers
  dubprod
  dubprod
  dubprod
  dubprod
  movff prodh,backemf

  subn ttm,reqttm,errttm,3
; err is negative for overspeed
 

;====================================================================

;;; mulmn mpand,3,mpyr,2,mulres
; for testing multiply only - disable - seems ok Thu Jan 13 17:57:23 EST 2011
;====================================================================

 jbs aflags,eninc,dorate
 movff ivv,drvvv
 bra noinc3

mulafrac ; multiply mpand[3] * mpyr[1] and shift 4 up
 mulmn mpand,3,mpyr,1,mulres
 shln mulres,4
 shln mulres,4
 shln mulres,4
 shln mulres,4
 return

mulafracs ; multiply mpand[3]-signed * mpyr[1] and shift 4 up
  skbs mpand,7
  bra mulafrac
  negn mpand,3
  call mulafrac
  negn mpand,3
  return

mulaccf macro errval,scalev,accto
; fractional accumulate errval[3]*scalev[1]/0x10 onto accto
  movn errval,mpand,3
  movff svalev,mpand
  call mulafracs
  addn mpyres,accto,4
  endm


dorate
; update req. speed if eninc flag on
 movff rate+1,rateinc+0
 movff rate+2,rateinc+1
 movff rate+3,rateinc+2
 movf rateinc+1,w
 jnz nomin
 movf rateinc+2,w
 jnz nomin
 movfw rateinc+0
 skbs rateinc+0,7
 addlw 010 ; floor acceleration value
 movwf rateinc+0
nomin 
 movff mangle+2,tmp0
 movf mangle+3,f
 jz nocom

  movfw mangle+3
  
  jbc  mangle+3,maxtime,scaledown
  clrf rate+2
  movlf 1,rate+1
  movlf 0c0,rate+0
  goto rst11

scaledown 
;;  incf tmp2,f
;;
;;  comf tmp0,f
;;       movlf 0fe,tmp0 ; *3/4
;  call scaleb
 comf rateinc+0,f
 comf rateinc+1,f
 comf rateinc+2,f
; incf rateinc+0,f

nocom
 movfw rateinc+0
 addwf rate+0,f
 movfw rateinc+1
 addwfc rate+1,f
 movfw rateinc+2
 addwfc rate+2,f
;; skcc
;; bcf aflags,eninc
rst11
 clrf rate+3

  jbs aflags,enfb,dofb  ; if no fb control, reset integrator
  clrf indrvvv+0 
  clrf indrvvv+1 
  clrf indrvvv+2
  movfw ivv  
  movwf indrvvv+3
  bra fbdone
toofast1  
  negn mpand,3
  comf vvtmp1+2,w
  jnz toofast3
  comf vvtmp1+1,w
  jnz toofast3
  comf vvtmp1,w
  clc
  andlw 0fe
  jz nottoof
  andlw 0fc
  jz s001
  andlw 0f8
  jz s002
  andlw 0f0
  jz s003

toofast3
s004  rlcf mpyr,f
s003  rlcf mpyr,f
s002  rlcf mpyr,f
s001  rlcf mpyr,f
s000  rlcf mpyr,f
nottoof  call mulafrac
  negn mulres,4
 bra ifbdone
slewlim
maxslew equ 012
 clrf mulres+2,f
 clrf mulres+3,f
 movlw maxslew
 subwf mulres+1,w
 jcc ifbdone
 clrf mulres+0,f
 movlf maxslew,mulres+1 
 bra ifbdone
dofb
  movn errttm,vvtmp1,3
  clrf vvtmp0
  movn errttm,mpand,3
  movff intfb,mpyr
  jbs errttm+2,7,toofast1  ; make errttm+2
  call mulafrac
  tstfsz ttm+2
  bra ifbdone
dzone equ 02 ; limit slew for ttm less than this
  movlw dzone
  subwf ttm+1,w
  jcc slewlim

ifbdone 
  
  shrn vvtmp0,4
  shrn vvtmp0,4
 


;  accn vvtmp0,indrvvv,4
  accn mulres,indrvvv,4
;  shl3 errttm
  shrn errttm,3
  movfw backemf
  addwf uprmargin,w
  movwf vvtmp3 
  sublw pwmmax-1
  jcs nouprclip
  movlf pwmmax-1,vvtmp3
nouprclip  
  movfw lwrmargin
  subwf backemf,w
  movwf vvtmp4
  skbc vvtmp4,7
  clrf vvtmp4
  skbc vvtmp4,7   ; output must >=0
  clrf vvtmp4

  movfw indrvvv+3
  addwf errttm+2,w
  movwf drvvv
  cpfsgt vvtmp4  ; test lower limit < w
  bra tstupl
  subwf vvtmp4,w
  addwf indrvvv+3,f
  bra adjdun
tstupl
  cpfslt vvtmp3
  bra adjdun
  cpfseq vvtmp3
  bra ddd
  bra adjdun
ddd  subwf vvtmp3,w
  addwf indrvvv+3,f
adjdun
  movwf vvtmp5

  

fbdone


noinc3
 

 movfw clock_0+1
 subwf tick1,w   
 jnz scandun        ; every 2048 muSec
 incf tick1,f
 jnz  scandun
 incf tick1+1,f
; comes here on average every 4096/3276.8 microseconds with 16/20 mHx xtal, missing slots are queued  
  incf cycnt,f
;  incf portd,f
;
; pid loop 


 movfw cycnt
 andlw 3
 jnz scandun




scandun
   return


 udata_shr 0a00
_stack res 0eff-0a00
_stack_end


 end