fl hex { servo motor(multi drop type) driver for KMR-P4(4-legs-robot) 2013/02/17 11:38:03 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 Arrangement for each servo ID --- --- | | | | | 2 | | 4 | | | | | --- --- --- --- | | | | | 1 | | 3 | | | | | --- Front --- --------- | | | | | Body | Top Of View | | | | --------- --- --- | | | | | 5 | | 7 | | | | | --- --- --- --- | | | | | 6 | | 8 | | | | | --- --- } \ 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 \ Delta for each servo variable delta d28 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 \ Send serial-command \ ( -- ) : serCom buffer rx_byte W@ tx_byte W@ serm bitticks hdserialTxRx+ ; \ 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 ; \ ************************************************************ \ *** 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 *** \ ************************************************************ \ 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? ; \ 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 ; \ 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 0 delta i 4 u* + L! loop ; \ Step up servo position to current position \ ( -- ) : servo d100 0 do \ Add delta to current 8 0 do delta i 4 u* + L@ 0 <> if current i 2 u* + W@ delta i 4 u* + L@ + current i 2 u* + W! then loop \ ." 1_CURRENT:" current h10 dump \ Copy current to target 8 0 do delta i 4 u* + L@ 0 <> if current i 2 u* + W@ target i 2 u* + W! \ ." i=" i . then loop \ ." 2_TARGET:" target h10 dump \ ." 3_Delta" delta h20 dump \ Move each servo sv_move loop \ Clear delta-array 8 0 do 0 delta i 4 u* + L! loop ; \ ************************************************************ \ *** Float leg patern *** \ ************************************************************ \ Float Front-Left leg \ ( -- ) : float_FL \ Set delta to value's 1/100 -5 delta d28 + L! -5 delta 4 + L! servo \ Set delta to value's 1/100 5 delta 4 + L! servo ; \ Float Front-Right leg \ ( -- ) : float_FR \ Set delta to value's 1/100 5 delta d20 + L! 5 delta d12 + L! servo \ Set delta to value's 1/100 -5 delta d12 + L! servo ; \ Float Rear-Right leg \ ( -- ) : float_RR \ Set delta to value's 1/100 5 delta 4 + L! 5 delta d28 + L! servo \ Set delta to value's 1/100 -5 delta d28 + L! servo ; \ Float Rear-Left leg \ ( -- ) : float_RL \ Set delta to value's 1/100 -6 delta d12 + L! -5 delta d20 + L! servo \ Set delta to value's 1/100 5 delta d20 + L! servo ; \ Move all legs to Left \ ( -- ) : Left-round init_pos float_FL d10 delta L! d5 delta d28 + L! servo float_RR d10 delta d24 + L! -5 delta 4 + L! servo float_FR d10 delta 8 + L! -5 delta d20 + L! servo float_RL d10 delta d16 + L! 5 delta d12 + L! servo ; \ Rotate robot-body to right \ ( -- ) : R_round d-10 delta L! d-10 delta 8 + L! d-10 delta d16 + L! d-10 delta d24 + L! servo ; : demo init_pos begin Left-round R_round st? 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+ }