( Inverse kinematics servo code for Lynx Motion L6 arm and gripper ( Author assumes no responsability to damage to servos or anything eles ( as a result of using this code. ( Portions of this code taken from New Micros' website with their consent ( however, they assume no liability either. ( this code assumes a working knowledge of the IsoPod line of products ( ( ********************************* SETUP ( This code was written for the arm to be configured in a specific way. ( ( the links and joints are named as follows, starting at the ground. ( RBASE is the base rotation. ( BASE is the height offset from the shoulder to the ground. ( SHOULDER is the joint at the base of the arm. ( HUMERUS is the first link of the arm, between ELBOW and SHOULDER. ( ELBOW is the next joint. ( ULNA is the next link, between ELBOW and WRIST. ( WRIST is the next joint. ( HAND is the last length, from WRIST to tip. ( left arm right arm ( RBASE PWMA0 PWMB0 ( SHOULDER PWMA1 PWMB1 ( ELBOW PWMA2 PWMB2 ( WRIST PWMA3 PWMB3 ( RGRIP PWMA4 PWMB4 ( GRIP PWMA5 PWMB5 ( ( RBASE is centered in it's travel, so that when the arm is laying down ( and the processer is behind it, it is at 0 degrees. ( looking from above clockwise is positive. ( The shoulder servos when pointed straight up are are at 90 degrees. ( when tilted toward the object, and level, they are at 0 degrees. ( The Elbow, when set so that the HUMERUS and ULNA are parallel, ( is at 0 degrees, with negitive values curling the arm inward. ( The wrist is similarly positioned at 0 degrees as well. ( ( RGRIP and GRIP are not part of the inverse kinematics ( Grip is calibrated in such a way that a command of 0 just closes it, ( and a command of 1500 opens it to 1.5" ( ( The constants ULNA HUMERUS BASE and ULNA are defined in this code ( in units of inches, but that was only a matter of convenience. ( Modifications to the arm can be accomodated my changing these values ( ( Scaling is determined by 4 values. ( the physical units of the arms endpoints, and the command pulses. ( there are four tables asociated with the scaling of the servos. ( ( this table is the internal value used to command the servo ( ARM-JOINT-CON PULMIN ( PULSE COMMAND AT MINIMUM RANGE ( base shoulder elbow wrist rgrip grip ( 3900 P, 3500 P, 3500 P, 2600 P, 3000 P, 4900 P, ( LEFT ( 3900 P, 3500 P, 3500 P, 2600 P, 3000 P, 4900 P, ( RIGHT ( ( this is the real world value associated with it ( in this instance, 1/10th's of a degree and for the grip ( 1/1000ths of an inch. Thus 900=90 degrees and 1500=1.5 inches ( ARM-JOINT-CON SRVMIN ( MINIMUM RANGE OF SERVO'S COMMAND ( base shoulder elbow wrist rgrip grip ( -900 P, 1800 P, -1530 P, 700 P, -800 P, 0 P, ( LEFT ( -900 P, 1800 P, -1530 P, 700 P, -800 P, 0 P, ( RIGHT ( ( This is the table of the maximum command pulse of the servo ( ARM-JOINT-CON PULMAX ( PULSE COMMAND AT MAX RANGE ( base shoulder elbow wrist rgrip grip ( 11000 P, 12500 P, 12500 P, 11500 P, 11800 P, 7700 P, ( 11000 P, 12500 P, 12500 P, 11500 P, 11800 P, 7700 P, ( ( THis is the maximum real world units associated with the ( the servo ( ARM-JOINT-CON SRVMAX ( MAXIMUM RANGE OF SERVO'S COMMAND ( base shoulder elbow wrist rgrip grip ( 900 P, 0 P, 270 P, -1100 P, 900 P, 1780 P, ( 900 P, 0 P, 270 P, -1100 P, 900 P, 1780 P, ( please note that the minimum and maximum command pulses are ( unsigned, whereas the real world units are signed ( ( if you comment out the line ( 0 ' ERASEPROG LFA PF! ( CLOSE THE DICTIONARY LINKAGE BELOW SCRUB ( you can upload software so that you can determine the values for ( the tables. ( With all your servos unplugged, or plugged in, but with a current ( limited supply upload the code with the one line commented out. ( now type MAIN and then STOP-TIMER ( now you can easily read and modify the positions of the servos directly ( By teling the system the arm and joit you are interested in, ( you can retrieve the current setting ( LEFT RBASE CMDPOS ? ( will tell you the setting of the left arm's base servo, in the timescale ( that the PWM registers expect to see. ( With care, you can jam the values into the servos directly by doing the ( following: ( 11000 PWMA0 PWM-OUT ( this would write the value of 11000 to PWMA0, which happens to be ( the RBASE of the left arm. ( So by carefully putting values into the joints directly, and measuring ( the values of your joints with a protractor, you can set these tables with ( your own values. ( ( THe next table controls the initial positions, in user units, of the ( starting positions of the servos. ( ( ARM-JOINT-CON RSTDEG ( POWER UP CMD DEG ( base shoulder elbow wrist rgrip grip ( 0 P, 1350 P, -1350 P, -450 P, 0 P, 1500 P, ( LEFT ( 0 P, 1350 P, -1350 P, -450 P, 0 P, 1500 P, ( RIGHT ( ( The next table sets up the acceleration and maximum velocity of the ( motion profiler. These are expressed in terms of the command pulse that ( drives the servos ( ( CREATE #0 ( base shoulder elbow wrist rgrip grip ( 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, ( CMDPOS L NOT USED ( 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, ( CMDPOS R NOT USED ( 800 P, 800 P, 800 P, 800 P, 800 P, 800 P, ( CMDVEL L ( 800 P, 800 P, 800 P, 800 P, 800 P, 800 P, ( CMDVEL R ( 8 P, 8 P, 8 P, 8 P, 8 P, 8 P, ( CMDACC L ( 8 P, 8 P, 8 P, 8 P, 8 P, 8 P, ( CMDACC R ( the values here are relitively snappy. when setting up, start up with ( values of 100 for velocity and 1 for acceleration ( ( Oncy you have properly parameterized the arm, command over SCI0 are ( straight forward. ( LEFT RBASE 0 CMDDEG ! WILL CENTER THE BASE ( LEFT SHLDR 900 CMDDEG ! WILL CENTER THE SHOULDER ( And so forth ( ( Inverse kinematics can also be used as well. ( The IK is a bit of frankencode, cut is serves the purpose. I choose to ( Implement the code in floating point, since it would have involved more ( processing power ( to have entry in Integer. ( There are three parameters that are needed for an inverse kinematics ( move. ( THe angle of the hand to the ground, called FIWRIST ( Float In Wrist ( THe height of the tip of the gripper when closed, FIHeight ( And the radius, of the gripper's tip from the base, FIRADIUS ( For example ( 0.0E0 FIWRIST F! 10.0E0 FIHEIGHT F! 10.0E0 FIRADIUS F! DO-IK ( will move the tip to a radius of 10 inches and a height of 10 inches ( If the arm is commanded to an impossible location, it will not move ( and it will show it's "diffuculty" by setting the value of IK-ERROR ( to some negitive, non zero value. Each joint of the arm will set a ( different bit ( in IK-ERROR. ( IK-ERROR ? will show you this ( ( In addition to the inverse kinematics, forward kinematics are available ( ( The word FORWARD-SOLVE will transfer the COMMANDED, not CURRENT join ( settings to the float variables FOFINALRADIUS and FOFINALHEIGHT ( By re-writing this, you could use it to solve for triangulation solutions ( with distance measureing devices. ( FOFINALHEIGHT F? FOFINALRADIUS F? will show these values. ( SCRUB 0 RAM ! ( : EEWORD ; ( SET UP SERIAL BUFFERS DECIMAL VARIABLE TXB0 80 4 + ALLOT EEWORD VARIABLE RXB0 80 4 + ALLOT EEWORD VARIABLE TXB1 80 4 + ALLOT EEWORD VARIABLE RXB1 80 4 + ALLOT EEWORD : INIT-SER 9600 SCI0 BAUD SCI0 TXB0 84 TXBUFFER SCI0 RXB0 84 RXBUFFER 9600 SCI1 BAUD SCI1 TXB1 84 TXBUFFER SCI1 RXB1 84 RXBUFFER ; EEWORD INIT-SER ( You do this serial stuff first, because it will speed download as well ( Servos: ( BLUE HS55'S 725 TO 2600 ( ( S2003 613 2800 ( ******************* BEGINNING OF SERVO SCALING CODE ( PWM RATE OUT: 40MHz IP CLK /16 PRESCALE = .4uS PWM CLK TICK ( * 32768 CNT OF PWM MAX = 13.1mS OR 76.3Hz VARIABLE ARM EEWORD VARIABLE JOINT EEWORD : ARM-SET P@ ARM ! ; EEWORD 0 DUP ARM-SET LEFT EEWORD ( 0 1+ ARM-SET RIGHT EEWORD ( 1 : JOINT-SET P@ JOINT ! ; EEWORD 0 DUP JOINT-SET RBASE EEWORD ( 0 1+ DUP JOINT-SET SHLDR EEWORD ( 1 1+ DUP JOINT-SET ELBOW EEWORD ( 2 1+ DUP JOINT-SET WRIST EEWORD ( 3 1+ DUP JOINT-SET RGRIP EEWORD ( 4 1+ JOINT-SET GRIP EEWORD ( 5 : ARM-JOINT-VAR P@ JOINT @ + ARM @ IF 6 + THEN ; EEWORD ARM-JOINT-VAR CMDDEG EEWORD ( COMANDED DEGREES HERE CONSTANT *CMDPOS EEWORD ARM-JOINT-VAR CMDPOS EEWORD ( KEEP IN THIS ORDER v ARM-JOINT-VAR CMDVEL EEWORD ( KEEP IN THIS ORDER v ARM-JOINT-VAR CMDACC EEWORD ( KEEP IN THIS ORDER v ARM-JOINT-VAR CURPOS EEWORD ( KEEP IN THIS ORDER v ARM-JOINT-VAR CURVEL EEWORD ( KEEP IN THIS ORDER v ARM-JOINT-VAR CURACC EEWORD ( KEEP IN THIS ORDER v ARM-JOINT-VAR DECCNT EEWORD : ARM-JOINT-CON JOINT @ + ARM @ IF 6 + THEN ; EEWORD ARM-JOINT-CON RSTDEG ( POWER UP CMD DEG ( base shoulder elbow wrist rgrip grip 0 P, 1350 P, -1350 P, -450 P, 0 P, 1500 P, ( LEFT 0 P, 1350 P, -1350 P, -450 P, 0 P, 1500 P, ( RIGHT EEWORD ( PWM-OUT RANGE FOR A GIVEN SERVO ARM-JOINT-CON PULMIN ( PULSE COMMAND AT MINIMUM RANGE ( base shoulder elbow wrist rgrip grip 3900 P, 3500 P, 3500 P, 2600 P, 3000 P, 4900 P, 3900 P, 3500 P, 3500 P, 2600 P, 3000 P, 4900 P, EEWORD ARM-JOINT-CON PULMAX ( PULSE COMMAND AT MAX RANGE ( base shoulder elbow wrist rgrip grip 11000 P, 12500 P, 12500 P, 11500 P, 11800 P, 7700 P, 11000 P, 12500 P, 12500 P, 11500 P, 11800 P, 7700 P, EEWORD ( ANGLE RANGE x10 FOR A GIVEN PWM-OUT RANGE ( base shoulder elbow wrist rgrip grip ARM-JOINT-CON SRVMIN ( MINIMUM RANGE OF SERVO'S COMMAND -900 P, 1800 P, -1530 P, 700 P, -800 P, 0 P, -900 P, 1800 P, -1530 P, 700 P, -800 P, 0 P, EEWORD ARM-JOINT-CON SRVMAX ( MAXIMUM RANGE OF SERVO'S COMMAND 900 P, 0 P, 270 P, -1100 P, 900 P, 1780 P, 900 P, 0 P, 270 P, -1100 P, 900 P, 1780 P, EEWORD ( CONVERSION FACTOR FOR THAT MOVEMENT ARM-JOINT-CON PUL/SRV ( PULSE SPAN / INPUT SPAN, CALCULATED LEFT RBASE PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, SHLDR PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, ELBOW PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, WRIST PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, RGRIP PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, GRIP PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, RIGHT RBASE PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, SHLDR PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, ELBOW PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, WRIST PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, RGRIP PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, GRIP PULMAX P@ PULMIN P@ - 1000 SRVMAX P@ SRVMIN P@ - */ P, EEWORD ( ******************* BEGINNING OF SETTINGS CODE : P>DMOVE BEGIN >R OVER P@ OVER ! SWAP 1+ SWAP 1+ R> 1- DUP 0= UNTIL DROP 2DROP ; EEWORD ( USER PARAMETERS FOR VELOCITY AND ACCELERATION CREATE #0 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, ( CMDPOS L NOT USED 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, 5000 P, ( CMDPOS R NOT USED 800 P, 800 P, 800 P, 800 P, 800 P, 800 P, ( CMDVEL L 800 P, 800 P, 800 P, 800 P, 800 P, 800 P, ( CMDVEL R 8 P, 8 P, 8 P, 8 P, 8 P, 8 P, ( CMDACC L 8 P, 8 P, 8 P, 8 P, 8 P, 8 P, ( CMDACC R EEWORD #0 *CMDPOS 36 P>DMOVE ( ******************* BEGINNING OF SETTINGS CODE : LMTPOS PULMIN P@ MAX PULMAX P@ MIN ; EEWORD : LMTANG SRVMIN P@ SRVMAX P@ MIN MAX SRVMIN P@ SRVMAX P@ MAX MIN ; EEWORD ( ******************* BEGINNING OF OO PWM CODE ( get address of context-last variable GPIO CONTEXT @ CONSTANT context-last EEWORD NO-CONTEXT : IS-CONTEXT ( nfa -- ) context-last ! context-last CONTEXT ! ; EEWORD : INDEXED-OBJLIT ( i -- ) R> SWAP OVER P@ + P@ OBJREF ! CELL+ >R ; EEWORD : INDEXED-OBJLITERAL ( a -- compiling ) ( i a -- executing ) STATE @ IF COMPILE INDEXED-OBJLIT P, ( compiling: OBJLIT,a ELSE + P@ OBJREF ! ( executing: just store it THEN ; IMMEDIATE EEWORD : INDEXED-OBJECT DUP P@ IS-CONTEXT ( set the private context CELL+ [COMPILE] INDEXED-OBJLITERAL ( set object table pointer ; EEWORD : }} ; EEWORD : {{ BEGIN ' CFA DUP [ ' }} CFA ] LITERAL - WHILE 3 + P, REPEAT DROP ; EEWORD PWMOUT INDEXED-OBJECT ]L-PWMAx {{ PWMA0 PWMA1 PWMA2 PWMA3 PWMA4 PWMA5 }} EEWORD PWMOUT INDEXED-OBJECT ]R-PWMBx {{ PWMB0 PWMB1 PWMB2 PWMB3 PWMB4 PWMB5 }} EEWORD : RCMOVE CURPOS @ LMTPOS JOINT @ ARM @ IF ]R-PWMBx ELSE ]L-PWMAx THEN PWM-OUT ; EEWORD : INIT-RC 0 ARM ! 0 JOINT ! RSTDEG CMDDEG 12 P>DMOVE #0 *CMDPOS 36 P>DMOVE ( CMDPOS, CMDVEL, CMDACC #0 *CMDPOS 36 + 36 P>DMOVE ( CURPOS, CURVEL, CURACC 32767 PWMA0 PWM-PERIOD ( 0x7FFF 76 Hz UPDATE RATE 32767 PWMB0 PWM-PERIOD ( 0x7FFF 76 Hz UPDATE RATE ; EEWORD ( ******************* BEGINNING OF PROFILED MOVE CODE : MOVEDONE? CMDPOS @ CURPOS @ = ; EEWORD : MOVEDIR? CMDPOS @ CURPOS @ < ; EEWORD : REMAINING? CMDPOS @ CURPOS @ - ABS ; EEWORD : DECEL? CURVEL @ CMDACC @ /MOD SWAP IF 1+ THEN DECCNT ! CURVEL @ DECCNT @ * 2 /MOD + REMAINING? < NOT ; EEWORD : ATSPEED? CURVEL @ CMDVEL @ = ; EEWORD : OVERSPEED? CMDVEL @ ABS CURVEL @ ABS U< ; EEWORD : PROFILE MOVEDONE? NOT IF ( COMPUTE NECESSARY ACC CMDACC @ DECEL? OVERSPEED? OR IF NEGATE ELSE ATSPEED? IF DROP 0 THEN THEN CURACC ! ( COMPUTE NECESSARY VELOCITY CURVEL @ CURACC @ + 0 MAX CMDVEL @ MIN CURVEL ! ( COMPUTE NECESSARY POSITION REMAINING? CURVEL @ U< IF CMDPOS @ CURPOS ! ELSE CURVEL @ MOVEDIR? IF NEGATE THEN CURPOS +! THEN CURPOS @ LMTPOS CURPOS ! ELSE 0 CURVEL ! 0 CURACC ! 0 DECCNT ! THEN ; EEWORD : MAKEPULSE CMDDEG @ LMTANG DUP CMDDEG ! PUL/SRV P@ 1000 */ PULMAX P@ PUL/SRV P@ SRVMAX P@ 1000 */ - + CMDPOS ! ; EEWORD : VELMOVE ARM @ JOINT @ TCFTICKS @ 1 AND IF LEFT ELSE RIGHT THEN RBASE MAKEPULSE PROFILE RCMOVE SHLDR MAKEPULSE PROFILE RCMOVE ELBOW MAKEPULSE PROFILE RCMOVE WRIST MAKEPULSE PROFILE RCMOVE RGRIP MAKEPULSE PROFILE RCMOVE GRIP MAKEPULSE PROFILE RCMOVE JOINT ! ARM ! ; EEWORD : GO LMTPOS CMDPOS ! ; EEWORD : RGO CMDDEG @ + LMTPOS CMDDEG ! ; EEWORD ( RELATIVE GO ( ******************* BEGINNING OF CODE FOR fp SERVO STUFF DECIMAL PI F2* FCONSTANT 2PI EEWORD 360.0E0 2PI F/ FCONSTANT DR EEWORD VARIABLE IK-ERROR EEWORD : R>D DR F* ; EEWORD ( RADIANS TO DEGREES ) : D>R [ 1E0 DR F/ ] FLITERAL F* ; EEWORD ( DEGREES TO RADIANS ) DECIMAL ( ***************** ( F MEANS FLOAT, I IS INPUT, O IS OUTPUT, C IS CALCULATED FVARIABLE FIBASE EEWORD ( BASE ANGLE IN RADIANS FVARIABLE FIWRIST EEWORD ( ANGLE OF WRIST TO GROUND IN RADIANS FVARIABLE FIHEIGHT EEWORD ( INPUT HEIGHT Y FVARIABLE FIRADIUS EEWORD ( INPUT RADIUS X FVARIABLE FCS-W EEWORD ( LENGTH OF SHOULDER TO WRIST FVARIABLE FCANGLE1 EEWORD ( ANGLE BETWEEN FCS-W AND GROUND IN RADIANS FVARIABLE FCANGLE2 EEWORD ( ANGLE BETWEEN HUMERUS AND GROUND IN RADIANS FVARIABLE FCOFSY1 EEWORD ( INTERMEDIATE OFFSET Y1 USED IN WRIST FVARIABLE FCOFSX1 EEWORD ( INTERMEDIATE OFFSET X1 USED IN WRIST FVARIABLE FCOFSY2 EEWORD ( INTERMEDIATE OFFSET Y2 USED IN WRIST FVARIABLE FCOFSX2 EEWORD ( INTERMEDIATE OFFSET X2 USED IN WRIST FVARIABLE FOSHOULDER EEWORD ( ANGLE OF SHOULDER IN RADIANS FVARIABLE FOELBOW EEWORD ( ANGLE OF ELBOW IN RADIANS FVARIABLE FOWRIST EEWORD ( ANGLE OF WRIST IN RADIANS ( ***************** ( NOT NECESSARY FOR INVERSE KINEMATICS, BUT MAY BE USEFUL TO DETECT ( DANGEROUS SITUATIONS AND ALSO FOR SENSORS FVARIABLE FOELBOWHEIGHT EEWORD FVARIABLE FOWRISTHEIGHT EEWORD FVARIABLE FOTIPHEIGHT EEWORD FVARIABLE FOELBOWRADIUS EEWORD FVARIABLE FOWRISTRADIUS EEWORD FVARIABLE FOTIPRADIUS EEWORD FVARIABLE FOFINALHEIGHT EEWORD FVARIABLE FOFINALRADIUS EEWORD ( **************** : INIT-IK 0 IK-ERROR ! ( SET ERROR TO 0 FOR IK CALCS 0.0E0 FIWRIST F! ( WRIST TO GROUND DEFAULT TO 0 8.0E0 FIHEIGHT F! ( 6" UP 8.0E0 FIRADIUS F! ( 6" OUT ; EEWORD ( ***************** 4.75E0 FCONSTANT FHUMERUS EEWORD 4.75E0 FCONSTANT FULNA EEWORD 3.25E0 FCONSTANT FBASEHEIGHT EEWORD 6.00E0 FCONSTANT FHANDLENGTH EEWORD ( ***************** : ANGCHK FDUP PI FNEGATE F< FDUP PI F+ F0= OR IF 2PI F+ THEN PI FOVER F< IF 2PI F- THEN ; EEWORD : FATAN2NEW ( F: Y \ X ... F in radians between -pi and +pi FDUP F0= ( x=0? IF FDROP FDUP F0= 0= ( y=0? IF F0< PI IF FNEGATE THEN F2/ THEN EXIT THEN FOVER F0< ( y<0) FDUP F0< ( x<0) F/ FATAN IF ( x<0) PI IF ( y<0) F- ELSE F+ THEN ELSE DROP THEN ; EEWORD : FASIN ( F:r1 -- r2 ) 1E0 FOVER FDUP F* ( SINr1 1 SINr1^2 F- FSQRT ( SINr1 1-SINr1^2^.5 FATAN2NEW ; EEWORD : FACOS ( F:r1 -- r2 ) 1E0 FOVER FDUP F* ( COSr1 1 COSr1^2 F- FSQRT ( COSr1 1-COSr1^2^.5 FSWAP FATAN2NEW ; EEWORD ( ******************* BEGINNING OF FORWARD KINEMATICS STUFF : PJFK ( PUT JOINTS FORWARD KINEMATICS SHLDR CMDDEG @ S>F 10.0E0 F/ D>R FOSHOULDER FDUP F! ELBOW CMDDEG @ S>F 10.0E0 F/ D>R FOELBOW FDUP F! WRIST CMDDEG @ S>F 10.0E0 F/ D>R FOWRIST FDUP F! ; EEWORD : MAKEELBOW FOSHOULDER F@ FDUP FSIN FHUMERUS F* FOELBOWHEIGHT F! FCOS FHUMERUS F* FOELBOWRADIUS F! ; EEWORD : MAKEWRIST FOSHOULDER F@ FOELBOW F@ F+ FDUP FSIN FULNA F* FOWRISTHEIGHT F! FCOS FULNA F* FOWRISTRADIUS F! ; EEWORD : MAKETIP FOSHOULDER F@ FOELBOW F@ F+ FOWRIST F@ F+ FDUP FSIN FHANDLENGTH F* FOTIPHEIGHT F! FCOS FHANDLENGTH F* FOTIPRADIUS F! ; EEWORD : MAKEALL ARM @ JOINT @ MAKEELBOW MAKEWRIST MAKETIP FBASEHEIGHT FOELBOWHEIGHT F@ F+ FOWRISTHEIGHT F@ F+ FOTIPHEIGHT F@ F+ FOFINALHEIGHT F! FOELBOWRADIUS F@ FOWRISTRADIUS F@ F+ FOTIPRADIUS F@ F+ FOFINALRADIUS F! JOINT ! ARM ! ; EEWORD : FORWARD-SOLVE ( GEN. FORWARD KINEMATICS SOLUTION AND PRINT PJFK MAKEALL ; EEWORD ( ******************* BEGINNING OF INVERSE KINEMATICS STUFF : MAKEOFFSETS ( SEE J7 & K7 OF SPREADSHEET FIWRIST F@ D>R FDUP FSIN FHANDLENGTH F* FCOFSY1 F! FCOS FHANDLENGTH F* FCOFSX1 F! ( SEE J7 & K7 OF SPREADSHEET FIHEIGHT F@ FCOFSY1 F@ F- FBASEHEIGHT F- FCOFSY2 F! FIRADIUS F@ FCOFSX1 F@ F- FCOFSX2 F! ; EEWORD : MAKES-W FCOFSY2 F@ FDUP F* FCOFSX2 F@ FDUP F* F+ FSQRT FCS-W F! ; EEWORD : MAKEA1 FCOFSY2 F@ FCOFSX2 F@ FATAN2NEW FCANGLE1 F! ; EEWORD : MAKEA2 FHUMERUS FDUP F* FULNA FDUP F* F- FCS-W F@ FDUP F* F+ FHUMERUS F2* FCS-W F@ F* F/ FACOS FCANGLE2 F! ; EEWORD : MAKESHLDROUT FCANGLE1 F@ FCANGLE2 F@ F+ FOSHOULDER F! ; EEWORD : MAKEELBOWOUT FHUMERUS FDUP F* FULNA FDUP F* F+ FCS-W F@ FDUP F* F- FHUMERUS F2* FULNA F* F/ FACOS FOELBOW PI FSWAP F- FNEGATE F! ; EEWORD : MAKEWRISTOUT FIWRIST F@ D>R FOELBOW F@ F- FOSHOULDER F@ F- FOWRIST F! ; EEWORD : MAKE-IK MAKEOFFSETS MAKES-W MAKEA1 MAKEA2 MAKESHLDROUT MAKEELBOWOUT MAKEWRISTOUT ; EEWORD : REPORT-IK FOSHOULDER F? FOELBOW F? FOWRIST F? ; EEWORD : REPORT-IK-DEG ." SHOULDER= " FOSHOULDER F@ R>D F. CR ." ELBOW= " FOELBOW F@ R>D F. CR ." WRIST= " FOWRIST F@ R>D F. CR ; EEWORD : TEST-IK 6.0E0 FIHEIGHT F! 6.0E0 FIRADIUS F! 0.0E0 FIWRIST F! MAKE-IK REPORT-IK ; EEWORD : TEST-IK-2 6.0E0 FIHEIGHT F! 6.0E0 FIRADIUS F! 45.0E0 D>R FIWRIST F! MAKE-IK REPORT-IK-DEG ; EEWORD : FSHOULDER-OK? ( MAKES -1 IN IK-ERROR SHLDR FOSHOULDER F@ R>D 10.0E0 F* F>D DROP DUP LMTANG = NOT IK-ERROR +! ; EEWORD : FELBOW-OK? ( MAKES -2 IN IK-ERROR ELBOW FOELBOW F@ R>D 10.0E0 F* F>D DROP DUP LMTANG = NOT 2* IK-ERROR +! ; EEWORD : FWRIST-OK? ( MAKES -4 IN IK-ERROR WRIST FOWRIST F@ R>D 10.0E0 F* F>D DROP DUP LMTANG = NOT 2* 2* IK-ERROR +! ; EEWORD : LENGTH-OK? ( MAKES -8 IN IK-ERROR FCS-W F@ FULNA FHUMERUS F+ F< NOT 2* 2* 2* IK-ERROR +! ; EEWORD : ALL-OK? 0 IK-ERROR ! FSHOULDER-OK? FELBOW-OK? FWRIST-OK? LENGTH-OK? ; EEWORD : DO-IK JOINT @ ARM @ ( REQUIRES FIHEIGHT FIRADIUS FIWRIST TO BE SET, OR BAD JUJU MAKE-IK ( MORE ERROR CHECKING FOR JOINT LIMITS SHOULD BE DONE HERE ( SINCE SOME IK CALCS WILL RESULT IN BAD JOINT POS. ALL-OK? IK-ERROR @ NOT IF ( WRIST CMDDEG @ S>F 10.0E0 D>R F/ FIWRIST F! FOSHOULDER F@ R>D 10.0E0 F* F>D DROP SHLDR CMDDEG ! FOELBOW F@ R>D 10.0E0 F* F>D DROP ELBOW CMDDEG ! FOWRIST F@ R>D 10.0E0 F* F>D DROP WRIST CMDDEG ! THEN ARM ! JOINT ! ; EEWORD : SQJ7 ( SQUIRT INTO JOINTS 0.0E0 FIWRIST F! ( FORCING AN ORIENTATION TO GROUND DO-IK ; EEWORD ( ******************* BEYOND THIS, ARE TOOLS FOR CHECK OUT : LABEL CR ." JOINT# CURPOS CURVEL CURACC DECCNT CMDPOS CMDVEL CMDACC" ." PULMIN PULMAX" ; EEWORD : ?7 @ 7 .R ; EEWORD : ?U7 @ 7 U.R ; EEWORD : SETTINGS? CR JOINT ?U7 CURACC CURVEL CURPOS ?U7 ?U7 ?7 DECCNT ?7 CMDPOS ?U7 CMDVEL ?U7 CMDACC ?U7 PULMIN P@ 7 .R PULMAX P@ 7 .R ; EEWORD : X LABEL BEGIN MOVEDONE? ?TERMINAL OR NOT WHILE SETTINGS? REPEAT SETTINGS? ( AT LEAST ONCE, OR ONE LAST TIME ; EEWORD : T LABEL ARM @ JOINT @ LEFT RBASE SETTINGS? SHLDR SETTINGS? ELBOW SETTINGS? WRIST SETTINGS? RGRIP SETTINGS? GRIP SETTINGS? RIGHT RBASE SETTINGS? SHLDR SETTINGS? ELBOW SETTINGS? WRIST SETTINGS? RGRIP SETTINGS? GRIP SETTINGS? JOINT ! ARM ! ; EEWORD ( ******************* ALTERNATE SERIAL COMMAND STREAM 128 CONSTANT EVADDR EEWORD ( EVERY ADDRESS 129 CONSTANT LEFTADDR EEWORD 130 CONSTANT RIGHTADDR EEWORD 38 CONSTANT MYADDR EEWORD ( A TEST ADDRES USING "&" SO CAN TYPE FROM KEYBOARD HERE 80 ALLOT CONSTANT MBUFF EEWORD VARIABLE MBUFFX EEWORD ( index into message buffer VARIABLE RXCHAR EEWORD ( last character received, for state tests : EVALUATE ( addr n -- ) TIB @ >R >IN @ >R #TIB @ >R 2DUP + 0 SWAP C! ( append a null terminator, required by INTERPRET #TIB ! TIB ! 0 >IN ! INTERPRET R> #TIB ! R> >IN ! R> TIB ! ; EEWORD : MESSAGEPROCESS MBUFF @ 129 = IF LEFT THEN MBUFF @ 130 = IF RIGHT THEN MBUFF 1+ MBUFFX @ 2 - EVALUATE ; EEWORD : GET-CHAR SCI1 RX RXCHAR C! ( GET CHAR RXCHAR C@ SCI1 TX ( ECHO IF NEEDED RXCHAR C@ MBUFF MBUFFX @ + C! ( PUT IN NEXT SPOT IN BUFFER MBUFFX @ 79 U< ( LESS THAN LAST SPOT IN BUFFER? IF MBUFFX @ 1+ ( JUST BUMP POINTER ELSE 0 ( WRAP TO BEGINNING OF BUFFER THEN MBUFFX ! ( SAVE POINTER ; EEWORD : ADDR-CHAR? RXCHAR C@ 128 U< NOT RXCHAR C@ MYADDR = OR ( TEST LINE ; EEWORD : RESTART-BUFF RXCHAR C@ MBUFF C! ( PUT CHAR AT BEGINNING OF BUFFER 1 MBUFFX ! ( SET POINTER TO NEXT SPOT IN BUFFER ; EEWORD : ENDMSG? RXCHAR C@ 13 = ( CR ; EEWORD MACHINE RCVR EEWORD ON-MACHINE RCVR APPEND-STATE AWAIT-ADDR EEWORD APPEND-STATE CHECK-ADDR EEWORD APPEND-STATE AWAIT-DATA EEWORD APPEND-STATE STORE-DATA EEWORD ( wait for valid address character: 80 to FF IN-STATE AWAIT-ADDR CONDITION SCI1 RX? CAUSES REDLED ON ( TEST LINE GET-CHAR REDLED OFF ( TEST LINE THEN-STATE CHECK-ADDR THIS-TIME IN-EE IN-STATE CHECK-ADDR CONDITION ADDR-CHAR? NOT CAUSES ( THEN-STATE AWAIT-ADDR THIS-TIME IN-EE IN-STATE CHECK-ADDR CONDITION ADDR-CHAR? CAUSES RESTART-BUFF THEN-STATE AWAIT-DATA THIS-TIME IN-EE IN-STATE AWAIT-DATA CONDITION SCI1 RX? CAUSES GET-CHAR THEN-STATE STORE-DATA THIS-TIME IN-EE IN-STATE STORE-DATA CONDITION ENDMSG? NOT CAUSES ( THEN-STATE AWAIT-DATA THIS-TIME IN-EE IN-STATE STORE-DATA CONDITION ENDMSG? CAUSES YELLED OFF MESSAGEPROCESS YELLED ON THEN-STATE AWAIT-ADDR NEXT-TIME IN-EE ( ******************* THE MACHINE CHAIN MACHINE-CHAIN ARM-CHAIN VELMOVE RCVR END-MACHINE-CHAIN EEWORD : ERASEPROG SCRUB ; EEWORD : WORDS WORDS ; EEWORD : ? ? ; EEWORD 10 SPACES ( THIS HELPS DOWNLOADER SYNC : F? F? ; EEWORD : STORE ! ; EEWORD : FSTORE F! ; EEWORD : RIGHT RIGHT ; EEWORD : LEFT LEFT ; EEWORD : RBASE RBASE ; EEWORD : SHLDR SHLDR ; EEWORD : ELBOW ELBOW ; EEWORD : WRIST WRIST ; EEWORD : RGRIP RGRIP ; EEWORD : GRIP GRIP ; EEWORD : CMDDEG CMDDEG ; EEWORD : CMDPOS CMDPOS ; EEWORD : CMDVEL CMDVEL ; EEWORD : CMDACC CMDACC ; EEWORD : CURPOS CURPOS ; EEWORD : CURVEL CURVEL ; EEWORD : CURACC CURACC ; EEWORD : DECCNT DECCNT ; EEWORD : FIWRIST FIWRIST ; EEWORD : FIHEIGHT FIHEIGHT ; EEWORD : FIRADIUS FIRADIUS ; EEWORD : FOWRIST FOWRIST ; EEWORD : MAKEALL MAKEALL ; EEWORD : DO-IK DO-IK ; EEWORD : FOFINALHEIGHT FOFINALHEIGHT ; EEWORD : FOFINALRADIUS FOFINALHEIGHT ; EEWORD : IK-ERROR IK-ERROR ; EEWORD : MAIN INIT-IK INIT-RC INIT-SER MBUFF 80 ERASE 0 MBUFFX ! 0 RXCHAR ! AWAIT-ADDR SET-STATE CR ." L6 Arm Command Language" CR EVERY 50000 CYCLES SCHEDULE-RUNS ARM-CHAIN ; EEWORD ( 0 ' ERASEPROG LFA PF! ( CLOSE THE DICTIONARY LINKAGE BELOW SCRUB NO-CONTEXT ( HEX 7C00 DECIMAL AUTOSTART MAIN ( V0.1 - V0.5 HEX 3C00 DECIMAL AUTOSTART MAIN ( V0.6 - SAVE-RAM