fl hex { servo motor(multi drop type) driver for KMR-P4(4-legs-robot) 2013/02/15 13:41:40 PropForth5.0/5.2 Vcca(3.3V) Vccb(5V) |1 |20 Propeller(3.3V) ----------- 1| |19 P0 ------| A0 B0 |------------ Servo signal 10| |11 GND ------|GND OE |-------GND ------------ levelshift-module(FXMA108) RS232 115000bps parity:Even Stopbit:1bit time ------------------------------------------------------------------->> ------- --------- ------------ | | | | | | | | ----- ------------------------------------- idle start bit0 bit1 bit2 bit3 bit4 bit5 bit6 bit7 Parity stop idle bit bit bit (always "0") (always "1") ParityBit is 0 when data is h03 ParityBit is 1 when data is h07 } \ allocate string \ ( -- ) : s, parsenw dup C@ 1+ bounds dup rot2 do C@++ c, loop drop ; \ Buffer for transmitting data and reseiving data wvariable buffer d64 allot \ Target position for each servo wvariable target d14 allot \ Current position for each servo wvariable current d14 allot \ position for each servo wvariable setting d14 allot lockdict create hdserialTxRx+ forthentry $C_a_lxasm w, h14A h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z2WibeB l, z1SyLI[ l, z2WibmB l, z1SyLI[ l, z2WiPZB l, z1SyLI[ l, z2WiPeB l, z1SyLI[ l, z2WibuB l, z2WiPmB l, z1bix\6 l, z1bixo6 l, ziPRE l, z1XFPRB l, z1bvPS0 l, z1biPT4 l, zfyPO1 l, z2WyPrB l, z2WiQ55 l, z20iQ7k l, z3riQ55 l, z1YVPO1 l, z1vix\6 l, zbyPO1 l, z3[yPva l, z20yPj1 l, z3[yP[V l, z1[ixo6 l, z2WiPo7 l, z2WyPO0 l, z2WyPr9 l, z2WiQ55 l, zbyQ02 l, z20iQ55 l, z3jFbo6 l, z3nFbo6 l, z20iQ7k l, z3riQ55 l, z1YFbql l, zbyPO1 l, z1vyPS0 l, z3[yPvr l, zFPRE l, z1XFPRB l, z2Wvc3y l, z20yPj1 l, z3[yPfj l, z2WiPT8 l, z1SV01X l, z80 l, 0 l, 0 l, 0 l, 0 l, z29W l, freedict \ Strings for eeprom items wvariable eeprom_str -2 allot s, BackupCharacter s, StretchGain s, --Speed-- s, --Punch-- s, -DeadBand- s, -Dumping- s, SafeTimer s, Flag(Hex) s, UpperPulseLimit s, dummy s, LowerPulseLimit s, dummy s, FactrySetting s, Comminication s, TempLimit s, CurrentLimit s, Response s, UserOffset s, FactrySetting s, -ServoID- s, Char-stretch1 s, Char-stretch2 s, Char-stretch3 \ byte number wvariable tx_byte wvariable rx_byte \ bitticks for d115000cps clkfreq d115000 u/ constant bitticks \ serial port 0 wconstant ser 0 >m constant serm \ Display string \ ( n1 n2 -- ) n1:string index n2:string's top address : dispStr swap dup 0 <> if 0 do dup C@ + 1+ loop else drop then .cstr ; \ Print error msg when hFF \ ( n -- ) n:hFF/0 : Parity? hFF = if ." Parity Error" cr then ; \ Send serial-command \ ( -- ) : serCom buffer rx_byte W@ tx_byte W@ serm bitticks hdserialTxRx+ ; \ Move servo to desired position \ ( n1 n2 -- ) n1:position n2:servo ID : pos h80 + buffer C! dup 7 rshift buffer 1+ C! h7F and buffer 2+ C! 3 tx_byte W! 3 rx_byte W! serCom \ Parity error? Parity? { 3 1 do buffer i + C@ loop swap 7 lshift or decimal . hex cr } ; \ Move 1-servo to target position with checking position \ ( n1 n2 -- ) n1:position n2:servo ID : move 2dup pos begin 2dup pos 1 delms over buffer 2+ C@ buffer 1+ C@ 7 lshift or - abs d100 < if 1 else 0 then until 2drop ; \ Move 8-servos to target position with checking position \ ( -- ) : sv_move \ Issue command for moving each servo to each target 8 0 do target i 2 u* + W@ 0 <> if target i 2 u* + W@ i 1+ pos then loop \ Check each servo's position begin 8 0 do target i 2 u* + W@ 0 <> if target i 2 u* + W@ i 1+ 2dup pos \ Get position over buffer 2+ C@ buffer 1+ C@ 7 lshift or - abs d200 < \ Set zero if servo reach to its target position if 0 target i 2 u* + W! then 2drop then loop \ Break if all target value are zero 0 8 0 do target i 2 u* + W@ + loop 0= if 1 else 0 then until ; \ ************************************************************ \ *** Utility *** \ ************************************************************ \ Read parameters \ ( n1 n2 -- n3 ) \ n1:item 0:eeprom 1: stretch 2:speed 3:current 4:temperature \ n2:servo ID \ n3:parity error(hFF) : readVal hA0 + buffer C! 2 tx_byte W! dup buffer 1+ C! 0 = if \ eeprom d66 rx_byte W! else \ stretch, current, temperature 3 rx_byte W! then serCom ; \ Print Tab \ ( -- ) : Tab 9 emit ; \ Print index \ ( n -- ) n:index number : index ." " 1+ decimal . hex Tab ; \ Print string \ ( n1 n2 -- ) n1:string's address n2:string index : print_item 2 u/ swap dispStr Tab ; \ Get eeprom-data from servo \ ( n -- ) n:servo ID : eeprom 0 over readVal drop cr ." -- servoID:" . ." eeprom data[64byte] --" hex buffer 2+ d64 dump cr ." index item value" cr buffer 2+ d64 0 do dup i d16 < if \ BackUpCharacter - Flag i index eeprom_str i print_item dup C@ 4 lshift swap 1+ C@ or i d14 = if hex . decimal else . then 2+ cr else i d24 < if \ UpperPulseLimit - LowerPulseLimit i index eeprom_str i print_item decimal W@ dup 8 rshift swap hF and 4 lshift or 8 lshift over 2+ W@ dup 8 rshift swap hF and 4 lshift or or . ." (decimal)" 4+ i 2+ seti cr else hex i d24 = if \ FactrySetting i index eeprom_str i print_item W@ . cr 2+ else i d32 < if \ Communication - CurrentLimit i index eeprom_str i print_item 1+ C@ . cr 2+ else i d50 < if \ Factrysetting i index d12 eeprom_str dispStr Tab W@ . cr 2+ else \ response - Char-stretch3 i index \ eeprom_str3 i d50 - print_item eeprom_str i d18 - print_item i d54 = if W@ . else dup C@ 4 lshift swap 1+ C@ or . then cr 2+ thens 2 +loop drop cr decimal ; \ MUST CONNECT ONLY 1-SERVO \ Read servoID \ (-- n2 ) n1:current ID-number : readID hE0 buffer C! 4 1 do 0 i buffer + C! loop 4 tx_byte W! 1 rx_byte W! serCom \ Parity error? Parity? \ Get servoID buffer C@ h1F and ; \ MUST CONNECT ONLY 1-SERVO \ Set ServoID \ ( n1 -- n2 ) n1:ID-number (0 - d31) n2:current ID-number : setID hE0 + buffer C! 4 1 do 1 i buffer + C! loop 4 tx_byte W! 1 rx_byte W! serCom \ Parity error? Parity? \ Get servoID buffer C@ h1F and ; \ ************************************************************ \ *** Servo command *** \ ************************************************************ \ Set all servos to initial position \ ( -- ) : init_pos 8 0 do d7500 target i 2 u* + W! loop sv_move 8 0 do d7500 current i 2 u* + W! loop 8 0 do d7500 setting i 2 u* + W! loop ; \ Clear target-array \ ( -- ) : clr_target 8 0 do 0 target i + W! loop ; : up \ Set increment for servo to d10 ( loop-count=d100 total_inc=d1000 ) d100 0 do 8 0 do \ Up i 1+ 2 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then i 1+ 4 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then i 1+ 6 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then i 1+ 8 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then \ Rotate left i 1+ 1 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then i 1+ 3 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then i 1+ 5 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then i 1+ 7 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then loop \ Copy setting_array to target_aray 8 0 do setting i 2 u* + W@ target i 2 u* + W! loop \ Move servo sv_move loop ; : down \ Set decrement for servo to d10 ( loop-count=d100 total_dec=d1000 ) d100 0 do 8 0 do \ Down i 1+ 2 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then i 1+ 4 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then i 1+ 6 = if setting i 2 u* + W@ d10 + setting i 2 u* + W! then i 1+ 8 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then \ rotate right i 1+ 1 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then i 1+ 3 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then i 1+ 5 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then i 1+ 7 = if setting i 2 u* + W@ d10 - setting i 2 u* + W! then loop \ Copy setting_array to target_aray 8 0 do setting i 2 u* + W@ target i 2 u* + W! loop \ Move servo sv_move loop ; : demo1 begin up down down up fkey? swap drop until ; decimal { fl \ hdseialTxRx+ \ ( n1 n2 n3 n4 n5 -- n6 ) \ n1:buffer address n2:rx byte number n3:tx byte number n4:bitmask n5:bitticks \ n6:0/hFF (hFF:parity error) build_BootOpt :rasm mov __bitticks , $C_stTOS spop mov __sermask , $C_stTOS spop mov $C_treg1 , $C_stTOS spop mov $C_treg2 , $C_stTOS spop mov __buffer , $C_stTOS mov $C_treg3 , $C_stTOS \ ------------------------------ \ Transmit \ ------------------------------ \ $C_treg1 -- transmit data counter \ $C_treg3 -- buffer address counter \ $C_treg4 -- transmit data bit counter (11bits) \ $C_stTOS -- transmit data \ serpin high or outa , __sermask or dira , __sermask __TxByteloop \ Get transmit-data to $C_stTOS rdbyte $C_stTOS , $C_treg3 \ StartBit + data(8bit) + ParityBit + StopBit = 11bit test $C_stTOS , $C_stTOS wc \ Adding ParityBit if_c or $C_stTOS , # h100 \ Adding StopBit or $C_stTOS , __h200 \ Adding StartBit shl $C_stTOS , # 1 \ first loop tick count count mov $C_treg4 , # d_11 mov $C_treg5 , __bitticks add $C_treg5 , cnt __TxBitloop waitcnt $C_treg5 , __bitticks \ Check if bit0($C_stTOS) is 1 or 0 test $C_stTOS , # 1 wz \ Set tx-bit to Hi if bit0($C_stTOS) is 1 muxnz outa , __sermask shr $C_stTOS , # 1 djnz $C_treg4 , # __TxBitloop \ Increment buffer address counter add $C_treg3 , # 1 djnz $C_treg1 , # __TxByteloop \ ------------------------------ \ Receive \ ------------------------------ \ $C_treg2 -- receive data counter \ $C_treg3 -- buffer address counter \ $C_treg4 -- receive data bit counter (9bits) \ $C_stTOS -- receive data \ serpin as input andn dira , __sermask \ Copy buffer address to $C_treg3 mov $C_treg3 , __buffer __RxByteloop mov $C_stTOS , # 0 mov $C_treg4 , # d_9 mov $C_treg5 , __bitticks shr $C_treg5 , # 2 add $C_treg5 , __bitticks \ wait for a hi to lo transition waitpeq __sermask , __sermask waitpne __sermask , __sermask \ first loop tick count add $C_treg5 , cnt __RxBitloop waitcnt $C_treg5 , __bitticks \ data(8bit) + ParityBit + StopBit test __sermask , ina wz shr $C_stTOS , # 1 \ Set rx-bit to Hi if sermask bit is 1 muxnz $C_stTOS , # h_100 djnz $C_treg4 , # __RxBitloop \ Copy data to buffer wrbyte $C_stTOS , $C_treg3 \ Parity check test $C_stTOS , $C_stTOS wc if_c mov __parity , # h_FF add $C_treg3 , # 1 djnz $C_treg2 , # __RxByteloop mov $C_stTOS , __parity jexit __h200 h200 __bitticks 0 __sermask 0 __buffer 0 __parity 0 ;asm hdserialTxRx+ }