fl hex { Driver for uM-FPUv3.1 PropForth5.2 16/10/2012 09:47:12 } \ ------- FPU ---------------- \ uM-FPU Function Definitions 1 wconstant GetConst 2 wconstant calcInverse 3 wconstant inv_kinema \ Variables Definitions 7 wconstant arm_e 8 wconstant arm_f 9 wconstant re A wconstant rf B wconstant pwm90 C wconstant x0 D wconstant y0 E wconstant z0 F wconstant x0_tmp 10 wconstant y0_tmp 11 wconstant theta1 12 wconstant theta2 13 wconstant theta3 14 wconstant sv1 15 wconstant sv2 16 wconstant sv3 1E wconstant ok \ FPU Instruction 00 wconstant NOOP F0 wconstant SYNC 5C wconstant syncChar F3 wconstant VERSION F2 wconstant READSTR 1D wconstant FREAD0 FC wconstant READVAR 95 wconstant LREADA 97 wconstant LREAD0 98 wconstant LREADBYTE 99 wconstant LREADWORD 5B wconstant LOADWORD 01 wconstant SELECTA 20 wconstant FSET 25 wconstant FDIV 29 wconstant FSET0 2E wconstant FDIV0 1F wconstant FTOA 7E wconstant FCALL 17 wconstant FWRITEA DB wconstant EESAVEA 5A wconstant LOADUBYTE 02 wconstant SELECTX 08 wconstant COPYA \ Propeller uM-FPUv3.1 3 wconstant _CLK \ P3 ----> SCLK 4 wconstant _DIN \ P4 ----> DIN 5 wconstant _DOUT \ P5 <---- DOUT d16 wconstant statusLED d17 wconstant successLED 1 _DOUT lshift constant _doutm : clk_out_l _CLK pinlo ; : clk_out_h _CLK pinhi ; : din_out_l _DIN pinlo ; : din_out_h _DIN pinhi ; \ Shift-Out data[byte,word,long] \ ( n1 n2 n3 -- ) n1:data[byte,word,long] n2:mask_data n3:loop counter : shift_out 0 do 2dup and if din_out_h else din_out_l then clk_out_h 1 rshift clk_out_l loop 2drop ; \ Send command to FPU (shiftout) \ ( n1 -- ) n1:command : >fpu 80 8 shift_out ; \ Send command and data(1byte) to FPU (shiftout) \ ( n1 n2 -- ) n1:data n2:command : 2>fpu 2 0 do >fpu loop ; \ Send command and data(3byte) to FPU (shiftout) \ ( n1 n2 n3 n4 -- ) n1;data3 n1;data2 n1;data1 n1;command : 3>fpu 3 0 do >fpu loop ; \ Send command and data(4byte) to FPU (shiftout) \ ( n1 n2 n3 n4 n5-- ) n1:data4 n2;data3 n3;data2 n4;data1 n5;command : 4>fpu 4 0 do >fpu loop ; \ Shift-In \ ( n -- b1 ) Receive n bytes from device, where n is 1 (byte), 2 (word), or 4 (long). : fpu> 0 swap 8 * 0 do 1 lshift ina COG@ _doutm and 0> if 1+ then clk_out_h clk_out_l loop ; \ ( -- t/f) Sets pin directions and resets fpu chip - If success, true. : reset_FPU \ Set port to output _CLK pinout _DIN pinout statusLED pinout successLED pinout successLED pinlo statusLED pinlo din_out_h \ Send $FF to reset uM-FPUv3.1 A 0 do FF >fpu loop din_out_l 20 delms \ Send sync-character to verify communication SYNC >fpu 1 fpu> syncChar = if 1 statusLED pinhi else 0 statusLED pinlo then ; \ ( -- ) Wait for fpu to be ready. : wait_FPU begin ina COG@ _doutm and 0= until ; : print_FPU_str READSTR >fpu \ Print character until receiving data is h00 begin 1 fpu> \ Receive 1byte dup 0= if drop 1 else emit 0 \ . 0 then until ; : FPU_info VERSION >fpu \ Copy version string to string buffer wait_FPU \ Wait print_FPU_str space \ Get register0 FREAD0 >fpu 4 fpu> ." version code:" hex . cr \ Get clock tics/ms READVAR >fpu D >fpu LREAD0 >fpu 4 fpu> ." Frequency:" decimal . ." kHz" cr \ Get internal mode READVAR >fpu B >fpu LREAD0 >fpu 4 fpu> dup hex ." $" . cr ." Communication:" dup 2 and if ." SPI" else ." I2C" then cr ." Mode:" dup 4 and if ." PIC mode" else ." IEEE" then cr ." Debug:" 8 and if ." Enabled" else ." Not Enabled" then cr decimal ; : separate_2 h100 u/mod ; \ Set constants to kinamatics \ ( n1 n2 n3 n4 n5 -- ) n1:pulse-width at 90degree n2:rf n3:re n4:arm_f n5:arm_e : kinama_const arm_e SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu arm_f SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu re SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu rf SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu pwm90 SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu GetConst FCALL 2>fpu ; \ Initialize and get informations. \ ( -- ) : init_FPU reset_FPU if FPU_info d860 d50 d172 d190 d106 kinama_const else ." fault" cr then ; \ ( z0 y0 x0 -- ) : inv_kinematics x0 SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu x0_tmp COPYA 2>fpu y0 SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu y0_tmp COPYA 2>fpu z0 SELECTA 2>fpu separate_2 LOADWORD 3>fpu FSET0 >fpu inv_kinema FCALL 2>fpu ; \ Get each servo's puse-width-time \ ( -- n1 n2 n3 ) n1:servo1 n2:servo2 n3:servo3 : getAngle sv1 SELECTA 2>fpu LREADA >fpu 4 fpu> sv2 SELECTA 2>fpu LREADA >fpu 4 fpu> sv3 SELECTA 2>fpu LREADA >fpu 4 fpu> ; \ Check if Invert-kinematic calculation is success \ ( -- n1 ) n1:t/f : calc_success? ok SELECTA 2>fpu LREADBYTE >fpu 1 fpu> ; \ Check invert-kinematics calculation \ ( z y x -- sv1 sv2 sv3 ) : check_FPU inv_kinematics wait_FPU calc_success? 2 = if successLED pinhi ." sv3 sv2 sv1" cr getAngle . . . cr else successLED pinlo ." Cannot calculate" cr then ; decimal { init_FPU d-185 d69 d69 check_FPU d860 d50 d172 d190 d106 kinama_const \ Set constants d-180 d30 d-50 inv_kinematics \ Set x0,y0,z0 \ 294.8432msec : time cnt COG@ d-180 d30 d-50 inv_kinematics waitFPU cnt COG@ swap - . ; \ 1.014msec : test cnt COG@ 88 80 8 shift_out cnt COG@ swap - . ; } { \ sample eeprom-write \ This code execute first only one time. : set_eeprom hE SELECTA 2>fpu FWRITEA >fpu hD9 hB3 hDD h3F 4>fpu \ 1.7320531 sqrt3 0 EESAVEA 2>fpu FWRITEA >fpu hD7 hB3 h5D h3F 4>fpu \ 0.8660254 sin120 1 EESAVEA 2>fpu FWRITEA >fpu h00 h00 h00 hBF 4>fpu \ -0.5 cos120 2 EESAVEA 2>fpu FWRITEA >fpu hD7 hB3 hDD h3F 4>fpu \ 1.732051 tan60 3 EESAVEA 2>fpu FWRITEA >fpu h00 h00 h00 h3F 4>fpu \ 0.5 sin30 4 EESAVEA 2>fpu FWRITEA >fpu h3A hCD h13 h3F 4>fpu \ 0.5773503 tan30 5 EESAVEA 2>fpu ; : test begin LREADWORD >fpu 2 fpu> . 200 delms fkey? swap drop until ; : test1 begin LREADBYTE >fpu 1 fpu> . 200 delms fkey? swap drop until ; }