fl hex { servo motor(multi drop type) driver for KMR-P4(4-legs-robot) 2013/02/11 11:15:06 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 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_str1 -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 wvariable eeprom_str2 -2 allot s, Comminication s, TempLimit s, CurrentLimit wvariable eeprom_str3 -2 allot 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 ; \ Parity error msg : Parity? ." Parity Error" cr ; \ Send serial-command \ ( -- ) : serCom buffer rx_byte W@ tx_byte W@ serm bitticks hdserialTxRx+ ; \ Move servo to desured position \ Send 2 times position-command \ ( n1 n2 -- ) n1:position n2:servo ID : position h80 + buffer C! dup 7 rshift dup rot2 buffer 1+ C! h7F and dup buffer 2+ C! 3 tx_byte W! 3 rx_byte W! serCom \ Parity error? hFF = if Parity? else d200 delms \ Wit for getting position 2 buffer + C! 1 buffer + C! h81 buffer C! serCom \ Prity error? hFF = if Parity? then then 3 1 do buffer i + C@ loop swap 7 lshift or decimal . hex cr ; \ Move servo to desured position ( Not getting current 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? hFF = if Parity? then 3 1 do buffer i + C@ loop swap 7 lshift or decimal . hex cr ; \ Read parameters \ ( n1 n2 -- n3 ) \ n1:item 0:eeprom 1: stretch 2:speed 3:current 4:temperature \ n2:servo ID \ n3:parity error : readVal hA0 + buffer C! 2 tx_byte W! dup buffer 1+ C! 0 = if d66 rx_byte W! else 3 rx_byte W! then serCom ; : setVal hC0 + buffer C! 3 tx_byte W! dup buffer 1+ C! 0 = if d66 rx_byte W! else 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 EPROM 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_str1 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_str1 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_str1 i print_item W@ . cr 2+ else i d32 < if \ Communication - CurrentLimit i index eeprom_str2 i d26 - print_item 1+ C@ . cr 2+ else i d50 < if \ Factrysetting i index d12 eeprom_str1 dispStr Tab W@ . cr 2+ else \ response - Char-stretch3 i index eeprom_str3 i d50 - print_item i d54 = if W@ . else dup C@ 4 lshift swap 1+ C@ or . then cr 2+ thens 2 +loop drop cr ; \ 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? hFF = if Parity? then buffer C@ h1F and ; \ 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? hFF = if Parity? then buffer C@ h1F and ; : init d7500 swap pos ; : move d7000 swap pos ; : bufclr d66 0 do 0 buffer i + C! loop ; \ Set all servo to neutral position \ ( -- ) : init_pos d7500 9 1 do dup i pos loop drop ; : demo1 init_pos d6000 2 pos d9000 4 pos d6000 6 pos d9000 8 pos ; : demo2 init_pos d9000 2 pos d6000 4 pos d9000 6 pos d6000 8 pos ; 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 __delay d_8800 ;asm hdserialTxRx+ }