Code Review for KRC2 2005ed and Other Questions

Nation · July 17, 2020 at 10:59 PM

Gents,

I'm outside of my comfort zone, and working on a small job programming a KRC2 2005 edition with a KR 30 arm. It is running V5.5.10. This robot is essentially a demo robot that will live in a conference room, and mirror the user's right hand motions which are streamed in via RS232. Hand coordinates are produced by a C# application interfacing with a Azure Kinect depth camera. The robot has no tool on it.

I've got it up and running and figured I would ask the experts here for their opinion on ball of code I've managed to produce in the last few days. The core of it built around @SkyeFire's code in this thread. Just to get the warm and fuzzies that I am not doing anything drastically wrong.

Here is the .scr module:

Code

&ACCESS RVP
&REL 64
&PARAM EDITMASK = *
DEF serial( )
DECL BOOL Pass
DECL INT I
DECL POS Shift


MOVEHOME:
;FOLD PTP P7 Vel=25 % PDAT7 Tool[0] Base[0];%{PE}%R 5.5.31,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P7, 3:, 5:25, 7:PDAT7
$BWDSTART=FALSE
PDAT_ACT=PPDAT7
FDAT_ACT=FP7
BAS(#PTP_PARAMS,25)
PTP XP7
;ENDFOLD


; Set base as the PTP point above.
$BASE = $POS_ACT
$TOOL = $NULLFRAME


; Set Velocites
$VEL.CP=1
$VEL.ORI1=400
$VEL.ORI2=400
$ACC.CP=10
$ACC.ORI1=100
$ACC.ORI2=100


REREAD:
; Init receiving string
Pass = strclear(Result[])
Offset=0


;Get a position from the host computer.
COPEN (:SER_3, Handle) ; open serial port COM3
Mode = #ABS ; CREAD waits for data or timeout
WAIT FOR ($DATA_SER3 <> 0) ; wait for data in COM3 buffer
; read serial buffer into RX string
CREAD (Handle, RX_State, Mode, Timeout, Offset, "%s", Result[])
CCLOSE (Handle, CH_State) ; close serial channel


;Occasionally a malformed string comes across. Need to investigate.
IF STRLEN(Result[])<20 THEN
GOTO REREAD
ENDIF


;Replace Commas with spaces so SREAD will work.
FOR I = 1 TO STRLEN(Result[])
IF (Result[I]==",") THEN
Result[I]=" "
ENDIF
ENDFOR


Offset = 0 ; Start SREAD from first string character
SREAD (Result[], CH_State, Offset, "%7f %7f %7f", X_Result, Y_Result, Z_Result) ; extract X Y and Z values from string


;Cap the values passed in. Probably a better way to do this.
IF (X_Result>500) THEN
X_Result=500
ENDIF
IF (X_Result<-500) THEN
X_Result=-500
ENDIF
IF (Y_Result>500) THEN
Y_Result=500
ENDIF
IF (Y_Result<-500) THEN
Y_Result=-500
ENDIF
IF (Z_Result>500) THEN
Z_Result=500
ENDIF
IF (Z_Result<-500) THEN
Z_Result=-500
ENDIF


;Apply the camera position. Kinda hacky.
Shift = $POS_ACT
Shift.x = Y_result
Shift.y = -X_result
Shift.z = Z_result
Shift.a = 0
Shift.b = 0
Shift.c = 0


;Move to commanded position.
PTP Shift


GOTO REREAD


END

Display More

and the .dat file:

Code

&ACCESS RVP
&REL 21
&PARAM EDITMASK = *
DEFDAT SERIAL PUBLIC




DECL INT Handle=3
DECL INT offset=0
DECL REAL Timeout=5.0
DECL STATE_T TX_State={RET1 #CMD_OK,MSG_NO 0,HITS 1,LENGTH 0}
DECL STATE_T RX_State={RET1 #DATA_BLK,MSG_NO 0,HITS 1,LENGTH 21}
DECL STATE_T ch_State={RET1 #DATA_END,MSG_NO 0,HITS 3,LENGTH 0}
DECL MODUS_T Mode=#ABS
DECL CHAR Result[100]
Result[]=" "


DECL REAL X_Result=92.0999985
DECL REAL Y_Result=401.899994
DECL REAL Z_Result=790.700012


DECL BASIS_SUGG_T LAST_BASIS={POINT1[] "P7 ",POINT2[] "P7 ",CP_PARAMS[] "CPDAT0 ",PTP_PARAMS[] "PDAT7 ",CONT[] " ",CP_VEL[] "2.0 ",PTP_VEL[] "25 ",SYNC_PARAMS[] "SYNCDAT ",SPL_NAME[] "S0 "}
DECL E6POS XP1={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT1={VEL 100.0,ACC 100.0,APO_DIST 100.0}
DECL E6POS XP2={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP2={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT2={VEL 100.0,ACC 100.0,APO_DIST 100.0}
DECL E6POS XP3={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP3={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT3={VEL 100.0,ACC 100.0,APO_DIST 100.0}
DECL E6POS XP4={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP4={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT4={VEL 100.0,ACC 100.0,APO_DIST 100.0}
DECL E6POS XP5={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP5={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT5={VEL 100.0,ACC 100.0,APO_DIST 100.0}
DECL INT LAST_XP_POINT=7
DECL E6POS XP6={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP6={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT6={VEL 100.0,ACC 100.0,APO_DIST 100.0}
DECL E6POS XP7={X 751.742126,Y 486.357605,Z 1312.63306,A -80.4673462,B 87.5570297,C -113.480103,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP7={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE}
DECL PDAT PPDAT7={VEL 100.0,ACC 100.0,APO_DIST 100.0}
ENDDAT

Display More

One thing I am running into (in teach high speed) is that when the robot reaches the shift point, it engages the brakes, then releases them when it jumps back up and reads in another position. Is there a way to prevent that behavior? AKA keep the brakes released.

Also, if anyone has any suggestions on how to speed up the code so the robot is more responsive I am all ears. I've had to slow the streaming down on the computer side to about 1Hz. Otherwise I get buffer overflow errors. Also if anyone knows how to clear the buffer, that would be helpful.

hermann · July 18, 2020 at 9:22 AM

Don't open/close serial port in every loop, just open once at beginning.

Change the format of the string that you don't have to convert strings to commas.

The behavior of the brakes seems curious, normally they should be released at least for about 10 seconds after one movement.

Mentat · July 20, 2020 at 11:03 AM

Code

$BASE = $POS_ACT

Is a bold and daring line- I would change it to some constant, maybe the same homepos.

Don't use GOTO, KRL provides functionality to write functions, procedures, loops, if and case statements- use those instead.

Limiting the values like this leaves them overly dependent on the homepos; also instead of a cube I would limit to a sphere- less jarring, at least for me.

MOM · July 22, 2020 at 6:54 PM

Hi,

I also have a Suggestion:

Do not send your data as string, send them in binary format and receive them like this:

CREAD(HANDLE,SR_T,MR_T,TIMEOUT,OFFSET_CR,"%r%.6r",IPOSNO,POSV[],PARMS[])

This will also speed up the communication and will not cause any problem by converting to float or double (you have to check first), afterwards it will be working pervectly.

With this I never had problems as you described.

Regards

MOM

Nation · November 3, 2020 at 10:49 PM

Finally back on this job, as I've has some free time appear in my schedule.

Quote from Nation

One thing I am running into (in teach high speed) is that when the robot reaches the shift point, it engages the brakes, then releases them when it jumps back up and reads in another position. Is there a way to prevent that behavior? AKA keep the brakes released

Found out that the previous owner of this robot had set $BRK_DEL_PRO to 200. That was causing the odd brake behavior.

Currently working on getting it up and running in auto.

Emben22 · November 4, 2020 at 9:41 AM

Had the same issue of buffer overflow when we were working on robotic Air Hockey .

We used rsi and a krc4 agilus.

we did 2 things to fix it.

1. we clear the buffer after receiving a data. (which i dont know how to do it on krc2)

2. from C# code we are limiting the sent data. its either the the first point or the last point detected from the camera.(robot is runs at full speed).

Nation · November 6, 2020 at 9:07 PM

While it is pretty easy to get the robot up and running in AUT, I'd like to have it so the end user only has to press a single start button. I was hoping to do this by monitoring the fence circuit and then resetting the messages and starting the program once it is clear. In Fanuc world this is fairly easy, but the more I dig into this, it looks like at the minimum I would have to wire some outputs to inputs.

Is there a way to map the EXT AUT signals to a virtual signal?

**panic mode** · November 6, 2020 at 9:44 PM

i wish this was possible in IOSYS.INI like with [IOLINKING]

would like that on KRC4 too...

Code Review for KRC2 2005ed and Other Questions

Advertising from our partners

Anyware Robotics is hiring!

How to see your Job Posting (search or recruit) here in Robot-Forum.com