fl { POV_II PropForth5.5 2013/07/16 17:16:46 Serial-pararel-conversion by using shift-register(74HC595 2pcs) P0 -- Shift Clock P1 -- Data P2 -- Latch Clock PWM P3 -- Inverter -- MOSFET -- Motor LED-drum sensor P4 -- photo-sensor output } \ Send 16bits to shift-register \ ( n1 n2 n3 n4 -- ) n1:16bit-data n2:_lc n3:_data n4: _sc lockdict create a_shift_out forthentry $C_a_lxasm w, h127 h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z2WyPW1 l, zfiPZB l, z1SyLI[ l, z2WyPb1 l, zfiPeB l, z1SyLI[ l, z2WyPj1 l, zfiPmB l, z1SyLI[ l, zfyPOG l, z2WyPrG l, zgyPO1 l, z1jixZD l, z1bixZC l, z1[ixZC l, z3[yPvU l, z1bixZE l, z1[ixZE l, z1SyLI[ l, z1SV01X l, freedict \ Set delay \ ( n1 -- ) n1:delay-time[usec] lockdict create a_delay forthentry $C_a_lxasm w, h11B h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z1SyZKN l, z3[yPSJ l, z1SyLI[ l, z1SV01X l, z2WyPcb l, z20iPik l, z3ryPb0 l, z1SV000 l, freedict \ a cog special register [ifndef ctra h1F8 wconstant ctra ] \ a cog special register [ifndef ctrb h1F9 wconstant ctrb ] \ a cog special register [ifndef frqa h1FA wconstant frqa ] \ a cog special register [ifndef frqb h1FB wconstant frqb ] \ a cog special register [ifndef phsa h1FC wconstant phsa ] \ a cog special register [ifndef phsb h1FD wconstant phsb ] \ waitcnt ( n1 n2 -- n1 ) \ wait until n1, add n2 to n1 [ifndef waitcnt : waitcnt _xasm2>1 h1F1 _cnip ; ] \ ---------------- Constant ------------------------------------------------------------ \ Ports 0 wconstant _sc 1 wconstant _data 2 wconstant _lc 3 wconstant _pwm 4 wconstant _sense d1200000 constant 15msec d800000 constant 10msec \ pwm=100% d80000 constant 1msec d8000 constant 100usec d800 constant 10usec d80 wconstant 1usec d400000 constant max_pwm \ pwm=50% 20 wconstant Kp \ 1200rpm 9 wconstant Ki 0 wconstant Kd \ ---------------- Variables ----------------------------------------------------------- \ Time for 1-rotation variable 1rotT \ Time during 1-rotation at target rpm variable tgt_T \ Target rpm variable tgt_rpm \ Integral part variable i_part variable tgt_5% wvariable first variable error variable previous_error \ -------------------------------------------------------------------------------------- \ Set new value to wconstant : TO ' 2+ W! ; \ ---------------- Measurement for disk's rotate-speed ----------------------------------------- \ Get state of _sense \ ( -- n1 ) n1:t/f : senseState 1 _sense lshift ina COG@ and ; \ Time for 1-rotation \ ( -- ) : 1rot_Time 1 frqa COG! 1 frqb COG! h20000000 _sense or ctra COG! \ POS detector h30000000 _sense or ctrb COG! \ NEG detector \ Wait until _sense become high begin senseState until \ Clear phsb (low-pulse counter) 0 phsb COG! begin \ Wait until _sense become low begin senseState 0= until phsa COG@ \ Read hi-pulse ticks 0 phsa COG! \ Clear phsa (Hi-pulse counter) \ Wait until _sense become hi begin senseState until phsb COG@ \ Read lo-pulse ticks 0 phsb COG! \ Clear phsa (low-pulse counter) + 1rotT L! \ Time for disc's 1-rotation 0 until ; \ ---------------- PID ----------------------------------------------------------------- \ PID Control \ ( -- ) : pid _pwm pinout \ Set port to output 0 i_part L! \ Clear integral part 1 first W! \ Set first to initial 0 previous_error L! \ Clear previous_error 0 1rotT L! 1 frqa COG! 0 phsa COG! _pwm h10000000 or ctra COG! \ PWM/NCO mode on servo pin \ Wait until disk turn 3 pinhi begin 1rotT L@ until 3 pinlo cnt COG@ 10msec + \ cnt + 10msec begin 1rotT L@ tgt_T L@ - dup error L! d10000 / \ Proportional part Kp * \ ( Kp*% -- ) ." P:" dup . \ Integral part and Differenrial part first W@ 0= if \ Integral part error L@ d100000 / i_part dup L@ rot + swap L! \ i_part += i_part + error i_part L@ Ki * ." I:" dup . + \ ( p_part+i_part -- ) \ Differenrial part error L@ previous_error L@ - d10 / 100000 / \ Divide signed number from 0 to +-1000 Kd * error L@ previous_error L! ." D:" dup . + \ ( p_part+i_part+d_part -- ) then ." SUM:" dup . 10usec u* \ Multiply 100usec(1%) to % 0 max \ If minus, set 0 max_pwm min \ If over max_pwm, set max_pwm ." result:" dup . cr negate phsa COG! \ Set value to phsa 10msec waitcnt \ cnt + 10msec 1rotT L@ 0<> \ Ignore if qrotT is 0 if 1rotT L@ tgt_5% L@ < if \ If 1rot_T is above taget-speed's 5%, clear i_part and set first to 0 first W@ if 0 first W! 0 i_part L! then else \ When 1rotT is not above taget-speed's 5%, set first to 1 if first=0 first W@ 0= if 1 first W! then then then 1rotT L@ d8000 u/ ." 1rotation time:" . \ 0 until fkey? swap drop until 0 ctra COG! ; : test d60000 swap u/ 1msec u* dup tgt_T L! \ target's 1rotation ticks dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% c" 1rot_Time" 0 cogx pid drop 0 cogreset 0 1rotT L! ; \ ---------------- LEDs ---------------------------------------------------------------- \ Set LED-port to output \ ( -- ) : led_set_up _sc 3 0 do dup pinout 1+ loop drop ; \ Off all LEDs \ ( -- ) : leds_off 0 _lc _data _sc a_shift_out ; \ String buffer(26-characters) 2bytes(16dots) X 16bytes(16rows) per 1-charcter \ total 832bytes wvariable buffer d830 allot \ address saving next font-data inside buffer wvariable buf_ptr \ dummy wvariable dummy \ Output character { address column row n b31 b30 b29 . . . b1 b0 | n+4 0 0 0 0 0 | . | . \|/ n+d124 0 0 0 0 0 } \ bit15 of row_line --> character's top \ bit0 of row_line --> character's bottom variable row_line variable odd \ ( n -- ) n:character code : char_out_1 dup 1 and if 2 else 1 then odd L! \ Check even/odd hFE and d64 u* h8000 + \ Get ROM Font address d28 + \ Skip 7-line(7 Long) \ column=d16 d16 0 do dup d23 0 do i 2 <> if i 7 <> if i 9 <> if i d11 <> if i d13 <> if i d18 <> if i d20 <> if \ Skip lines row_line L@ 1 lshift row_line L! dup L@ odd L@ 2dup and if row_line L@ 1 or row_line L! then 2drop thens 4 + \ Add +4 to address loop drop \ Save font-data to buffer row_line W@ buf_ptr W@ W! 2 buf_ptr W+! { \ --- Print 16bit-data to terminal --- row_line L@ 1 d16 0 do 2dup and if h2A else bl then emit 1 lshift loop 2drop cr \ ------------------------------------ } odd L@ 2 lshift odd L! \ next colummn bit loop drop ; \ Save font-data for string(max 26chars) to buffer \ ( n1 -- ) n1:string address (top addr is string-length) \ Example c" ABC" disp_str_drum : disp_str_drum \ Clear buffer[832bytes] buffer d416 0 do dup 0 swap W! 2+ loop drop \ max characters of string is 26char dup C@ d26 min \ ( address str_length -- ) swap 1+ swap \ ( address+1 str_length -- ) buffer buf_ptr W! \ Set start-addr of buffer to buf_ptr \ Save string's font-data to buffer 0 do dup C@ char_out_1 1+ loop drop \ Set buf_ptr to buffer+d150 if string-size is less than d150(display area for LED-drum) buf_ptr W@ buffer - d300 < if buffer d300 + buf_ptr W! then ; \ Display data during 1 rotation \ (n1 -- n1) n1:buffer address : dispDrum dup \ ( addr addr -- ) buf_ptr W@ > \ Compare with last buffer address if buf_ptr W@ - buffer + then \ If addr > buf_ptr, set new addr \ ( addr -- ) dup \ ( addr addr -- ) 1 _sense lshift dup waitpne \ Detect base-pos for drum (Wait until _sense= Low) cnt COG@ 10msec + 1rotT L@ 1usec u/ d250 u/ \ Get dots of 1-rotation for do-loop swap 0 waitcnt drop \ Adjust display-start-pos 0 do \ 250usec X n dots = 50.5 to 50msec dup \ ( addr addr addr -- ) buf_ptr W@ = \ ( addr addr 1/0 -- ) if buf_ptr W@ - buffer + \ ( addr new-addr -- ) else dummy W@ - 0 + \ dummy ( addr addr -- ) then dup \ ( addr addr addr -- ) W@ _lc _data _sc a_shift_out \ Set LED-data to shift-register \ ( addr addr -- ) d110 a_delay \ Adjust delay-time[250usec] from _lc to next _lc 2+ \ ( addr addr+2 -- ) loop drop \ ( addr -- ) \ Off all leds leds_off ; \ Clear buffer : clr_buf buffer d416 0 do dup 0 swap W! 2+ loop drop ; : demo \ Init LED led_set_up leds_off \ Setup 0 dummy W! d50 1msec u* dup tgt_T L! \ target's 1rotation ticks (1200rpm) dup d20 u/ + tgt_5% L! \ target's 1rotation ticks + 5% c" 1rot_Time" 0 cogx c" pid" 1 cogx clr_buf begin 1rotT L@ tgt_5% L@ < until d3000 delms \ Move string to Left \ c" PropForth5.5 " disp_str_drum c" May the Forth be with you." disp_str_drum buffer d20 1 do \ Set string moving speed d10 0 do \ Repeat 10 times dispDrum j 2* + \ addr += outerloop*2 loop loop drop clr_buf \ Move string to Right c" May the Forth be with you." disp_str_drum buffer d20 1 do \ Set string moving speed d10 0 do \ Repeat 10 times dispDrum j 2* - \ addr -= outerloop*2 dup buffer < \ Compare with last buffer address if buffer swap - \ Get diference from top buffer addres to addr buf_ptr W@ swap - \ Get new address then loop loop drop 1 cogreset 0 cogreset ; \ HardWare test of LED-board \ ( -- ) : led_test led_set_up leds_off 1 d16 0 do st? dup _lc _data _sc a_shift_out 1 lshift d100 delms loop drop leds_off ; : tt begin led_test fkey? swap drop until ; { \ Send 16bits to shift-register \ ( n1 n2 n3 n4 -- ) n1:16bit-data n2:_lc n3:_data n4: _sc \ $C_treg1 -- _scm \ $C_treg2 -- _datam \ $C_treg3 -- _lcm \ $C_treg4 -- loop counter fl build_BootOpt :rasm \ get _scm mov $C_treg1 , # 1 shl $C_treg1 , $C_stTOS spop \ get _datam mov $C_treg2 , # 1 shl $C_treg2 , $C_stTOS spop \ get _lcm mov $C_treg3 , # 1 shl $C_treg3 , $C_stTOS spop \ shift 16bit-data to left shl $C_stTOS , # d16 \ set loop-count mov $C_treg4 , # d16 __1 shl $C_stTOS , # 1 wc \ set _data to Hi/Lo muxc outa , $C_treg2 \ set _scm to Hi or outa , $C_treg1 \ nop \ set _scm to Lo andn outa , $C_treg1 djnz $C_treg4 , # __1 \ set _lcm to Hi or outa , $C_treg3 \ nop \ set _lcm to lo andn outa , $C_treg3 spop jexit ;asm a_shift_out \ Set delay \ ( n1 -- ) n1:delay-time[usec] fl build_BootOpt :rasm __1 jmpret __delay_ret , # __delay djnz $C_stTOS , # __1 spop jexit \ Delay 1usec __delay mov $C_treg2 , # 68 add $C_treg2 , cnt waitcnt $C_treg2 , # 0 __delay_ret ret ;asm a_delay }