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:
&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:
&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.