DAT '***********pasm cog****************** BCog org 0 ' mov AdPar,Par 'get the hub address of control PulseWidth list rdlong AdPWidth, AdPar add AdPar, #4 'to address of mode list rdlong AdMode, AdPar ' add AdPar, #4 'to address of theta list rdlong AdTheta, AdPar add AdPar, #4 'to address of commanded angle (or velocity)list rdlong AdCommand, AdPar add AdPar, #4 'to address of completed flag list rdlong AdComplete, AdPar add AdPar, #4 'to constant clk tics in 1 usec rdlong MicroSec, AdPar add AdPar, #4 'to pin numbers list rdlong AdPins, AdPar add AdPar, #4 'to Context list rdlong AdContext, AdPar add AdPar, #4 'to parameter list rdlong AdParameters, AdPar add AdPar, #4 'to context 2 (constant velocity mode) rdlong AdContext2, AdPar add AdPar, #4 'to velocity list rdlong AdVelocity, AdPar mov work1, #%00100 'pina follows phsa[31] shl work1, #26 'to control field mov phsa, #0 'no bogus startup pulse mov frqa, #1 'count by one mov ctra, work1 'start up counter a mov x, MicroSec 'tics in one microsecond mov y, C5000 ' call #multiply 'tics in 5 msec mov C5000, y 'save for later use mov TheServo, #0 'begin with zero mov Delta, #0 'dont store junk into context2 ' or dira, #$3f mov timer, cnt 'initialize 5.0 msec timer add timer, C5000 loop ' or outa, orangemask and TheServo, #3 wz 'see if servo number zero if_z jmp #InitAds 'go point to beginnings of lists add CAdPWidth, #2 'go on to next set of context (word) add CAdMode, #1 'byte add CAdTheta, #2 'word add CADCommand, #2 'word add CAdComplete, #1 'byte add CAdPins, #2 'word add CAdContext, #4 'long add CAdParameters, #4 'long add CAdContext2, #4 'long add CAdVelocity, #4 'long jmp #GetPar 'get context InitAds mov CAdPWidth, AdPWidth 'get pointers for servo zero mov CAdMode, AdMode mov CAdTheta, AdTheta mov CAdCommand, AdCommand mov CAdComplete, AdComplete mov CAdPins, AdPins mov CAdContext, AdContext mov CAdParameters, AdParameters mov CAdContext2, AdContext2 mov CAdVelocity, AdVelocity GetPar rdlong work1, CAdParameters 'get parameters for this servo mov maxError, work1 'break them out shr maxError, #24 and MaxError, #$FF mov gain, work1 'gain for this servo shr gain, #16 and gain, #$FF mov deltamin, work1 'dead zone shr deltamin, #8 and deltamin, #$FF mov deltamax, work1 'max delta from 1500 microseconds and deltamax, #$FF GetCon rdlong Work1, CAdContext 'get context mov PrevTheta, work1 '8..0: PrevTheta and PrevTheta, #511 '0..359 angle mov CountGood, Work1 '9..13: CountGood shr CountGood, #9 and CountGood, #$1f mov WSZFlag, Work1 '15: WSZFlag shr WSZFlag, #15 and WSZFlag, #1 wz rdlong work1, CAdContext2 'constant velocity context mov PrevDC, work1 'duty cycle from previous pass andn PrevDC, Hi16 'just low order word mov PrevDelta, Work1 'pulsewidth offset from previous pass shr PrevDelta, #16 'align andn PrevDelta, Hi16 'keep just offset nop 'may need to do more context here rdword work1, CAdPins wz 'get the pin numbers if_Z jmp #NoServo 'no servo here ServoN mov ContPin, Work1 'keep the control pin number for later shr ContPin, #8 'scaled appropriately and Work1, #$1F 'feedback pin number mov FBPinMask, #1 'form the mask for the feedback pin shl FBPinMask, Work1 waitpne FBPinMask, FBPinMask 'get pulse width: wait for feedback low waitpeq FBPinMask, FBPinMask 'then high mov work1, cnt 'beginning of high feedback pulse waitpne FBPinMask, FBPinMask 'wait for end of high pulse mov HiPer, cnt waitpeq FBPinMask, FBPinMask 'now wait for end of low pulse mov LowPer, cnt sub LowPer, HiPer 'calculate low period sub HiPer, work1 'and high period mov y, HiPer 'calculate total period add y, LowPer 'total period in tics (typically ~80K for 80MHz clock) shr y, #1 'keep within 16 bits mov x, HiPer 'set up dividend shl x, #11 'scale by 4096 (note divisor was shifted by one) call #divide 'calculate duty cycle andn x, Hi16 'discard remainder mov DutyCycle, x ' retain raw DC mov CVelocity, DutyCycle ' sub CVelocity, PrevDC 'calculate signed velocity abs work2, CVelocity 'unsigned current velocity cmp work2, PW1500 wc 'did we just cross zero (anything greater than about 100)? if_C jmp #NotXZ 'no, we did not test CVelocity, Bit31 wz 'see which direction we crossed zero if_Z sub CVelocity, VeloX0 'counterclockwise if_NZ add CVelocity, VeloX0 'clockwise correction NotXZ wrlong CVelocity, CAdVelocity 'save for GetVelocity 'CVelocity contains the current (signed) velocity sub x, DCMin wc 'translate current Duty Cycle to zero if_c mov x, #0 'must never ever even one time underflow mov y, #359 'scale call #multiply 'to mov x, y 'degrees mov y, dcRange call #divide ' andn x, hi16 'drop remainder wrword x, CAdtheta 'save actual angle for GetPos mov myTheta, x 'get my copy of actual theta rdbyte work1, CAdMode wz 'see what mode we are in if_Z mov WSZFlag, #0 'we are in CPW (commanded Pulse Width) mode if_z rdword CogPW, CAdPWidth 'get pulse width if CPW mode if_z jmp #HNPW 'we are in CPW mode mov CogPW, PW1500 'nominal pulse width: will do arithmatic in here test work1, #%1000 wz 'are we in constant velocity mode? if_NZ jmp #CVMode 'constant velocity Position cmp work1, #1 wz 'is this a new position command? if_z mov CountGood, #0 'it is: reset the goodness counter if_Z Mov PrevDelta, #0 'and ramp up from zero if_Z wrbyte two, CAdMode 'we have seen this position command sub PrevTheta, MyTheta 'check for having crossed zero abs PrevTheta, PrevTheta 'absolute value cmp PrevTheta, #180 wc 'was it a lot? eg or gt 180 degrees? if_NC xor WSZFlag, #1 'it was a lot; invert the flag rdword MyCommand, CAdCommand 'get the commanded angle min MyCommand, #1 'set limits max MyCommand, #358 mov error, MyTheta sub error, MyCommand wz 'signed error abs AbsError, error 'absolute value of error cmp AbsError, MaxError wz, wc 'desired accuracy if_C jmp #StopSvo 'error is less than n degrees. stop servo mov CountGood, #0 'certainly not done mov x, AbsError mov y, gain 'gain call #multiply 'leaves gain times error times 10 in y mov x, y 'now divide by ten mov y, #10 call #divide mov y, x 'result into y andn y, Hi16 'drop remainder mov Delta, y 'calculated delta in microseconds mov Work1, PrevDelta 'previous delta add Work1, Ramp 'previous + ramp amount max Delta, Work1 'ramp up a little at a time min Delta, DeltaMin 'but for sure get out of dead zone max Delta, DeltaMax 'maximum difference from 1500 test WSZFlag, #1 wz 'are we on wrong side of zero? if_nz jmp #WSZero 'yes cmp MyTheta, MyCommand wc 'we are right side of zero if_C sub CogPW, Delta 'drive CW if_NC add CogPW, Delta 'drive counterclockwise jmp #HNPW WSZero mov y, #31 'go slowly: distinctive pulse width cmp MyTheta, MyCommand wc 'reverse directions if_C add CogPW, Delta if_NC sub CogPW, Delta jmp #HNPW StopSvo add CountGood, #1 'another good pass cmp CountGood, #10 wz 'arbitrary number (200 msec) if_e wrbyte zero, CAdComplete 'signal host we are done max CountGood, #10 'no overflow into WSZFlag HNPW mov y, CogPW 'Have New Pulse Width in CogPW in usec mov x, microSec 'tics in usec call #multiply 'convert to clock tics in y mov work2, WSZFlag 'assemble context(1) for storage in hub shl work2, #15 'scale mov work1, CountGood shl work1, #9 or work1, MyTheta 'becomes PrevTheta for next pass or work1, work2 'wszFlag wrlong work1, CAdContext 'save context1 mov Work1, Delta 'set up context2 shl Work1, #16 'align (becomes PrevDelta) or Work1, DutyCycle 'insert current duty cycle wrlong work1, CAdContext2 'save context 2 ' andn outa, orangemask waitcnt timer, C5000 'remaining portion of 5 msec movs ctra, ContPin 'set up control pin mov work1, #1 '(re)enable it shl work1, ContPin or dira, work1 neg phsa, y 'start a pulse NxtSvo add TheServo, #1 'to next servo jmp #loop 'forever NoServo' andn outa, orangemask waitcnt timer, C5000 'just let its time be wasted jmp #NxtSvo CVMode test work1, #%0001 wz 'very first time here? if_NZ jmp #CVM1 'been here before mov PrevDelta, #0 'no previous delta. ramp up from zero wrbyte nine, CAdMode 'note been here before CVM1 rdword work2, CAdCommand 'get the commanded velocity test work2, Bit15 wz 'must we extend the sign? if_NZ or work2, Hi16 'yes:signed commanded velociy cmps CVelocity, work2 wz,wc 'compare actual to command if_Z jmp #CVM2 'the same: no adjustment needful if_C subs PrevDelta, #1 'adjust one way if_NC adds PrevDelta, #1 'or the other CVM2 mov Delta, PrevDelta add CogPW, Delta 'new pulse width ' wrword CogPW, CAdPWidth 'save for GetPulseWidth jmp #HNPW ' Divide x[31..0] by y[15..0] (y[16] must be 0) ' on exit, quotient is in x[15..0] and remainder is in x[31..16] divide shl y,#15 'get divisor into y[30..15] mov t,#16 'ready for 16 quotient bits :loop cmpsub x,y wc 'y =< x? Subtract it, quotient bit in c rcl x,#1 'rotate c into quotient, shift dividend djnz t,#:loop 'loop until done divide_ret ret 'quotient in x[15..0], 'remainder in x[31..16] ' Multiply x[15..0] by y[15..0] (y[31..16] must be 0) ' on exit, product in y[31..0] multiply shl x,#16 'get multiplicand into x[31..16] mov t,#16 'ready for 16 multiplier bits shr y,#1 wc 'get initial multiplier bit into c :loop if_c add y,x wc 'if c set, add multiplicand to product rcr y,#1 wc 'put next multiplier in c, shift prod. djnz t,#:loop 'loop until done multiply_ret ret 'return with product in y[31..0] {display mov slc, #32 'will display all 32 bits :loop shl slr, #1 wc muxc outa, bluemask or outa, violetmask nop andn outa, violetmask djnz slc, #:loop andn outa, bluemask display_ret ret } zero long 0 two long 2 nine long 9 C5000 long 5000 'number usec in 5.0 msec Bit31 long $8000_0000 Bit15 long $0000_8000 Hi16 long $FFFF_0000 'high word DcMin long 121 'duty cycle minimum DcRange long 3845 'duty cycle max PW1500 long 1500 Ramp long 5 'ramp up a little at a time (n uSec/20msec) VeloX0 long 3856 'crossing zero magic number {redmask long %1 'logic analyzer pin definitions orangemask long %10 yellowmask long %100 greenmask long %1000 BlueMask long %1_0000 violetmask long %10_0000} TheServo res 'which servo we are doing MicroSec res 'clock tics in one microsec AdPar res 'parameter list AdPWidth res 'address of control pulse width in hub list AdMode res 'address of mode (veleocity/angle) list Adtheta res 'address of angle feedback list AdCommand res 'address of commanded angle list AdComplete res 'address of complete flag list AdPins res 'address of pin list AdContext res 'adds of Context list AdParameters res 'addrs of parameters list AdContext2 res 'addrs of constant CVelocity context list AdVelocity res 'addrs of velocity feedback list CAdPWidth res 'address of current control pulse width in hub CAdMode res 'address of current mode (velocity/angle) CAdtheta res 'address of current angle feedback CAdCommand res 'address of current commanded angle CAdComplete res 'address of current complete flag CAdPins res 'address of currect pins CAdContext res 'address of current Context CAdParameters res 'address of current parameters CAdContext2 res 'current second context long CAdVelocity res 'current velocity Gain res 'parameters for current servo DeltaMin res DeltaMax res MaxError res Delta res 'current delta from 1500 CogPW res 'arithmatic for control pulse width MyTheta res 'my copy of actual angle MyCommand res 'my copy of commanded angle Error res 'signed error AbsError res 'absolute value of error HiPer res 'high period in clks LowPer res 'low period ContPin res 'control pin number FBPinMask res 'feedback pin mask timer res 'clock at end of next period work1 res 'generally useful work2 res CountGood res 'count "no changes needful" WSZFlag res 'wrong side of zero DutyCycle res 'current duty cycle PrevDC res 'previous DC CVelocity res 'current velocity: CW is positive, CCW is negative PrevDelta res 'delta from 1500 in velocity mode from before PrevTheta res 'theta from previous pass x res 'pasm divide/ multiply variables y res t res slc res 'showlong count slr res 'showlong register fit 320