{{ Demonstration of scaling Duty Cycle Written by Xander Soldaat }} OBJ ' dbg : "PASDebug" CON Motor1 = 8 Motor2 = 11 FWD = 1 RVS = 2 Motor1PWMPin = 8 Motor1FWDPin = 9 Motor1RVSPin = 10 Motor2PWMPin = 11 Motor2FWDPin = 12 Motor2RVSPin = 13 VAR long dutycycle1 long dutycycle2 long pwmfreq PUB start(_pwmfreq) | i dc1Addr := @dutycycle1 dc2Addr := @dutycycle2 pwmfreq := _pwmfreq pwmfAddr := @pwmfreq dira[Motor1FWDPin..Motor1RVSPin]~~ outa[Motor1FWDPin..Motor1RVSPin]~ dira[Motor2FWDPin..Motor2RVSPin]~~ outa[Motor2FWDPin..Motor2RVSPin]~ dutycycle1 := 0 dutycycle2 := 0 cognew(@entry, 0) PUB setDuty(Motor, percent) if Motor == Motor1 dutycycle1 := percent elseif Motor == Motor2 dutycycle2 := percent PUB setDir(Motor, direction) if direction == FWD outa[Motor + FWD]~~ outa[Motor + RVS]~ if direction == RVS outa[Motor + FWD]~ outa[Motor + RVS]~~ PUB coast outa[Motor1FWDPin..Motor1RVSPin]~ outa[Motor2FWDPin..Motor2RVSPin]~ PUB brake outa[Motor1FWDPin..Motor1RVSPin]~~ outa[Motor2FWDPin..Motor2RVSPin]~~ DAT org '********************************************************** ' ' PWM calculation routine ' entry ' --------- Debugger Kernel add this at Entry (Addr 0) --------- ' long $34FC1202,$6CE81201,$83C120B,$8BC0E0A,$E87C0E03,$8BC0E0A ' long $EC7C0E05,$A0BC1207,$5C7C0003,$5C7C0003,$7FFC,$7FF8 ' -------------------------------------------------------------- mov dira, diraval ' set APIN to output mov ctra, ctraval ' establish counter A mode and APIN mov frqa, #1 ' set counter to increment 1 each cycle mov ctrb, ctrbval ' establish counter B and mov frqb, #1 rdlong pwmf, pwmfAddr ' Get the PWM base frequency rdlong cpuf, #0 ' Get the CPU frequency (1st memory pos in HUB) ' Calculate the pwmbase time, divide cpufreq/pwmfreq ' Get 1/100th of the period mov multiplicand, pwmf mov multiplier, #100 jmpret multiply_ret, #multiply mov _t, product mov dividend, cpuf mov divisor, _t mov quotient, #0 jmpret divide_ret, #divide mov pwmfbase, quotient ' pwmfbase is quotient ' Now get the period, that's pwmbase * 100 mov multiplicand, pwmfbase mov multiplier, #100 jmpret multiply_ret, #multiply mov period, product mov time, cnt ' record current time add time, period ' establish next period :pwmloop mov dutyold, duty1 rdlong duty1, dc1Addr ' get an up to date duty cycle for motor 1 cmp dutyold, duty1 wz, wc ' If Z or C is set then they're the same IF_E jmp #:do_nothing ' They're the same, so skip this code below mov multiplicand, pwmfbase mov multiplier, duty1 jmpret multiply_ret, #multiply mov pulsewidth1, product :do_nothing mov dutyold, duty2 rdlong duty2, dc2Addr ' get an up to date duty cycle for motor 1 cmp dutyold, duty2 wz, wc ' If Z or C is set then they're the same IF_E jmp #:do_nothing_again ' They're the same, so skip this code below mov multiplicand, pwmfbase mov multiplier, duty2 jmpret multiply_ret, #multiply mov pulsewidth2, product :do_nothing_again neg phsa, pulsewidth1 neg phsb, pulsewidth2 waitcnt time, period ' wait until next period jmp #:pwmloop ' loop for next cycle '********************************************************** ' ' Divide two 32bit longs, only tested with unsigned. ' divide mov quotient, #0 ' Clear the quotient mov _t, #0 ' Clear the counter cmp divisor, #0 wz ' Check for division by 0 IF_Z jmp divide_ret ' return to main program cmp divisor, dividend wc, wz ' Check divisor > dividend IF_A jmp divide_ret ' return to main program :shift_divider_loop cmp divisor, dividend wc, wz ' Compare divisor and dividend IF_A jmp #:exit_shift_loop ' divisor is greater than dividend, so exit IF_BE add _t, #1 ' increment _t counter IF_B shl divisor, #1 wc, wz ' shift divisor to left, track C and Z IF_NZ_AND_NC jmp #:shift_divider_loop ' if no carry and divisor != 0, keep going :exit_shift_loop IF_A rcr divisor, #1 ' If divisor was shifted too much, shift carry back :divide_loop cmpsub dividend, divisor wc ' if divisor =< dividend, subtract it, set C rcl quotient,#1 ' rotate c into quotient shr divisor, #1 ' Shift divisor to right djnz _t,#:divide_loop ' loop until done :divide_ret jmp divide_ret ' return to main program '********************************************************** ' ' Multiply two factors, product may not exceed 32 bit ' multiply ' Check if multiplier > multiplicand, if so, switch them around cmp multiplier, multiplicand wc, wz IF_A mov product, multiplier IF_A mov multiplier, multiplicand IF_A mov multiplicand, product mov product, #0 ' Always clear product in case we run this twice :multiply_loop shr multiplier , #1 wc, wz ' Shift multiplier right by one IF_C add product, multiplicand ' If C =1, add multiplicand to product shl multiplicand, #1 ' Shift multiplicand left by 1 IF_NZ jmp #:multiply_loop ' Loop, but only if there are digits left in multiplier :multiply_ret jmp multiply_ret ' ' ' Variables ' diraval long |< Motor1PWMPin + |< Motor2PWMPin ' APIN direction (0) ctraval long %00100 << 26 + Motor1PWMPin ' NCO/PWM APIN=2 ctrbval long %00100 << 26 + Motor2PWMPin ' NCO/PWM APIN=3 period long 0 ' 800kHz period ( clkfreq / period ) time long 0 duty1 long 0 ' duty cycle for PWM1 in percent duty2 long 0 ' duty cycle for PWM2 in percent dc1Addr long 0 dc2Addr long 0 pulsewidth1 long 0 ' duty cycle for PWM1 in clkticks pulsewidth2 long 0 ' duty cycle for PWM2 in clkticks dutyold long 0 ' old value for duty cycle pwmf long 0 ' this is the full width of the carrier freq pwmfbase long 0 ' this is 1/100th of the total width cpuf long 0 ' CPU frequency pwmfAddr long 0 ' Address of PWM frequency var (pwmfreq in HUB) _t long 0 ' general purpose tmp long ' divisor long 0 ' used by the divide function dividend long 0 ' used by the divide function quotient long 0 ' holds the result of the divide function divide_ret long 0 ' address to which divide returns multiplier long 0 multiplicand long 0 product long 0 multiply_ret long 0 pwAddr1 long 0 pwAddr2 long 0 tmpAddr long 0