fl { Servo SG90 PropForth5.5 Modified "jm_servo8_adv.spin" 2015/02/28 21:55:48 3.3V 5V 5V MOSFET(2N7000) | | RED | ----------- 10kohm |RED | G | | orange ----------- P13 ----|S 2N7000 D|------------------|PWM DG90 | ----------- ----------- |BROWN GND 3.3V 5V 5V MOSFET(2N7000) | | | ----------- 10kohm |RED | G | | orange ----------- P14 ----|S 2N7000 D|------------------|PWM DG90 | ----------- ----------- |Brown GND } \ ------------------------------------------------------- \ Constants \ ------------------------------------------------------- \ Special register h1F8 wconstant ctra h1FA wconstant frqa h1FC wconstant phsa \ Time for servo d80 wconstant 1usec d1000 wconstant SV_MIN d2000 wconstant SV_MAX \ channel[max:8] 2 wconstant ch \ top pin number of contiguous servo group d13 wconstant pin \ ------------------------------------------------------- \ Variables \ ------------------------------------------------------- \ target[ch],delta[ch],usec[ch]: each channel has 2byte wvariable target 2 allot wvariable delta 2 allot wvariable usec 2 allot \ ------------------------------------------------------- \ Main \ ------------------------------------------------------- \ target position for each ch \ ( n1 -- n2 ) n1:ch n2:target_position's varriable address for each ch : tgt_pos 2 u* target + ; \ delta position for each ch \ ( n1 -- n2 ) n1:ch n2:delta's varriable address for each ch : delta_val 2 u* delta + ; \ current position for each ch \ ( n1 -- n2 ) n1:ch n2:current_position's varriable address for each ch : cur_pos 2 u* usec + ; \ Set servo-motor's pin to output \ ( n1 -- ) n1:top pin number of contiguous servo group : setup ch 0 do dup pinout 1+ loop drop ; \ Check if position is inside region \ ( n1 -- n2 ) \ n1:servo position(microsecond) \ n2:SV_MIN <= n2 <= SV_MAX : pos_check SV_MIN W@ max SV_MAX W@ min ; \ Set selected servo(ch) to new position \ ( n1 n2 -- ) \ n1:ch [0,1,2,3,4,5,6,7] \ n2:newpos specified in microseconds : setpos pos_check swap 2dup \ ( n2 n1 n2 n1 ) tgt_pos W! \ ( n2 n1 ) Save target_position to sprcified ch dup \ ( n2 n1 n1 ) \ delta[ch] = 0 delta_val 0 swap W! \ ( n2 n1 ) Save delta=0 to specified ch \ usec[ch] = newpos cur_pos W! \ ( -- ) Save new_position to specified ch ; \ Move selected servo from current position to newpos in n1 frames \ ( n1 n2 n3 -- ) \ n1:frame [1frame is 20msec(servo frame timing)] \ n2:ch \ n3:newpos : movpos pos_check 2dup \ ( n1 n2 n3 n2 n3 ) \ save newpos to target[ch] swap tgt_pos W! \ ( n1 n2 n3 ) Save target_position to sprcified ch rot \ ( n2 n3 n1 ) dup 1 = if drop \ ( n2 n3 ) setpos else >r \ ( n2 n3 ) Push frame over \ ( n2 n3 n2 ) cur_pos W@ - \ Calculate (difference from newpos and usec) abs d10 u* \ ( n2 n3 calc_value ) r> \ Pop frame u/ \ ( n2 n3 calc_value ) Calculate 0.1usec/frame 5 + d10 u/ 1 max \ ( n2 compared_result ) round up to 1usec/frame swap \ ( compared_resulte n2 ) delta_val W! \ ( -- ) Update delta to specified ch then ; \ Returns position (current output command to servo) of channel \ ( n1 -- n2 ) \ n1:ch n2:ch's position(microseconds) : position cur_pos W@ ; \ Returns True when channel is at desired target \ ( n1 -- n2 ) \ n1:ch n2:t/f : at_target dup cur_pos W@ swap tgt_pos W@ = ; \ Wait until servo channel reaches at target position \ ( n1 -- ) n1:ch : wait_tgt begin dup at_target until drop ; \ Runs 2 servos \ ( n1 -- ) n1:top pin number of contiguous servo group : delta_servo dup setup 1 frqa COG! 0 phsa COG! cnt COG@ d200000 + \ cnt + 2.5msec (2.5msec x 8 = 20msec) swap begin \ run 8 slot(20msec) 8 0 do \ active channel? i ch < if \ Set PWM/NCO mode on servo pin dup i + h10000000 or ctra COG! \ Set negative value to phsa i cur_pos W@ 1usec u* negate phsa COG! i cur_pos W@ i tgt_pos W@ < if i cur_pos W@ i delta_val W@ + i tgt_pos W@ min i cur_pos W! else i cur_pos W@ i tgt_pos W@ > if i cur_pos W@ i delta_val W@ - i tgt_pos W@ max i cur_pos W! then then then swap d200000 waitcnt \ cnt + 2.5msec swap 0 ctra COG! loop 0 until \ fkey? swap drop until drop ; \ Start servo cog \ ( -- ) : servo_init ch 0 do i d1500 setpos \ servo1 initial position[0degree] i d1500 setpos \ servo2 initial position[0degree] loop c" pin delta_servo" 0 cogx ; : disp_pos ch 0 do ." ch:" i . ." target:" i tgt_pos W@ . ." delta:" i delta_val W@ . ." usec:" i cur_pos W@ . cr loop cr ; : test servo_init 50 0 2000 movpos begin disp_pos 0 cur_pos W@ d2000 = until disp_pos ; wvariable flag \ Display servo's current position \ ( -- ) : disp_ST7565S init_LCD 0 font_type W! \ propfont 0 x W! 0 y W! c" 1: ms" lcd_string 0 x W! 1 y W! c" 2: ms" lcd_string begin \ ch0 2 x W! 0 y W! 0 cur_pos W@ lcd_dec \ ch1 2 x W! 1 y W! 0 cur_pos W@ lcd_dec flag W@ \ fkey? swap drop until poweroff ; \ Using ST7565S(128x64)_0.2.f \ ( -- ) : demo 0 flag W! servo_init c" disp_ST7565S" 1 cogx begin 300 0 2000 movpos 300 1 2000 movpos 0 wait_tgt 1 wait_tgt 500 0 1000 movpos 200 1 1000 movpos 0 wait_tgt 1 wait_tgt fkey? swap drop until 50 0 1500 movpos 50 1 1500 movpos d1000 delms 1 flag W! 0 cogreset 1 cogreset ;