208 lines
6.1 KiB
Plaintext
208 lines
6.1 KiB
Plaintext
&H NOBOUNDSCHECK
|
|
DEF RSIPI_Test()
|
|
; =========================================================================
|
|
; RSIPI Comprehensive Test Program
|
|
; =========================================================================
|
|
; Tests all RSIPI Python library functionality:
|
|
; 1. RSI initialisation and connection
|
|
; 2. Cartesian correction (RSI_MOVECORR)
|
|
; 3. Tech variable exchange (Python <-> KRL)
|
|
; 4. $SEN_PREA variable reading
|
|
; 5. Digital I/O coordination
|
|
; 6. Handshake patterns (wait for Python, signal back)
|
|
;
|
|
; Matching Python script: rsi_config/rsipi_test.py
|
|
;
|
|
; Protocol (Tech.C11 = state from KRL, Tech.T11 = command from Python):
|
|
; State 0: Idle
|
|
; State 1: RSI connected, waiting for Python
|
|
; State 2: Running corrections (RSI_MOVECORR active)
|
|
; State 3: Corrections complete, reading SEN_PREA
|
|
; State 4: I/O test phase
|
|
; State 5: Complete
|
|
; State 99: Error
|
|
;
|
|
; Python commands via Tech.T11:
|
|
; 0: No command
|
|
; 1: Python ready, start corrections
|
|
; 2: Stop corrections
|
|
; 3: SEN_PREA values written, read them
|
|
; 4: Start I/O test
|
|
; 5: Shutdown
|
|
; =========================================================================
|
|
|
|
DECL INT ret, CONTID
|
|
DECL INT python_cmd
|
|
DECL REAL sen_val1, sen_val2, sen_val3
|
|
DECL E6POS start_pos
|
|
|
|
INI
|
|
|
|
; -- Store start position ------------------------------------------------
|
|
start_pos = $POS_ACT
|
|
|
|
; -- Move to a safe starting position ------------------------------------
|
|
; (Adjust these coordinates for your robot/cell)
|
|
PTP start_pos
|
|
|
|
; ========================================================================
|
|
; PHASE 1: Initialise RSI
|
|
; ========================================================================
|
|
|
|
; Load RSI signal flow configuration
|
|
ret = RSI_CREATE("RSIPI_Full.rsi", CONTID, TRUE)
|
|
IF (ret <> RSIOK) THEN
|
|
MsgNotify("RSI_CREATE failed", "RSIPI_Test")
|
|
HALT
|
|
ENDIF
|
|
|
|
; Activate RSI in RELATIVE mode at 4ms cycle
|
|
; Change to #ABSOLUTE / #IPO as needed
|
|
ret = RSI_ON(#RELATIVE, #IPO_FAST)
|
|
IF (ret <> RSIOK) THEN
|
|
MsgNotify("RSI_ON failed", "RSIPI_Test")
|
|
HALT
|
|
ENDIF
|
|
|
|
; Signal state 1: RSI connected, waiting for Python
|
|
$TECH.C[11] = 1
|
|
MsgNotify("RSI active - waiting for Python...", "RSIPI_Test")
|
|
|
|
; ========================================================================
|
|
; PHASE 2: Wait for Python to connect and signal ready
|
|
; ========================================================================
|
|
|
|
; Poll Tech.T11 for Python's "ready" command (value = 1)
|
|
python_cmd = 0
|
|
WHILE (python_cmd <> 1)
|
|
python_cmd = $TECH.T[11]
|
|
WAIT SEC 0.012 ; Check every 12ms
|
|
ENDWHILE
|
|
|
|
MsgNotify("Python connected - starting corrections", "RSIPI_Test")
|
|
|
|
; Send current position to Python via Tech.C12-C17
|
|
$TECH.C[12] = $POS_ACT.X
|
|
$TECH.C[13] = $POS_ACT.Y
|
|
$TECH.C[14] = $POS_ACT.Z
|
|
$TECH.C[15] = $POS_ACT.A
|
|
$TECH.C[16] = $POS_ACT.B
|
|
$TECH.C[17] = $POS_ACT.C
|
|
|
|
; ========================================================================
|
|
; PHASE 3: Sensor-guided motion (Python sends corrections)
|
|
; ========================================================================
|
|
|
|
; Signal state 2: corrections active
|
|
$TECH.C[11] = 2
|
|
|
|
; RSI_MOVECORR - robot is now purely controlled by Python corrections
|
|
; The robot will hold position and apply RKorr corrections from Python
|
|
; Python sends corrections via api.motion.update_cartesian()
|
|
; This blocks until Python sends stop command (Tech.T11 = 2)
|
|
; or until RSI is turned off
|
|
RSI_MOVECORR()
|
|
|
|
MsgNotify("Corrections phase complete", "RSIPI_Test")
|
|
|
|
; ========================================================================
|
|
; PHASE 4: Read $SEN_PREA values from Python
|
|
; ========================================================================
|
|
|
|
; Signal state 3: ready to read SEN_PREA
|
|
$TECH.C[11] = 3
|
|
|
|
; Wait for Python to write SEN_PREA values (command = 3)
|
|
python_cmd = 0
|
|
WHILE (python_cmd <> 3)
|
|
python_cmd = $TECH.T[11]
|
|
WAIT SEC 0.012
|
|
ENDWHILE
|
|
|
|
; Read values that Python wrote via MAP2SEN_PREA
|
|
sen_val1 = $SEN_PREA[1]
|
|
sen_val2 = $SEN_PREA[2]
|
|
sen_val3 = $SEN_PREA[3]
|
|
|
|
; Echo them back to Python via Tech.C18-C20
|
|
$TECH.C[18] = sen_val1
|
|
$TECH.C[19] = sen_val2
|
|
$TECH.C[110] = sen_val3
|
|
|
|
MsgNotify("SEN_PREA read complete", "RSIPI_Test")
|
|
|
|
; ========================================================================
|
|
; PHASE 5: Digital I/O test
|
|
; ========================================================================
|
|
|
|
; Signal state 4: I/O test phase
|
|
$TECH.C[11] = 4
|
|
|
|
; Wait for Python to start I/O test (command = 4)
|
|
python_cmd = 0
|
|
WHILE (python_cmd <> 4)
|
|
python_cmd = $TECH.T[11]
|
|
WAIT SEC 0.012
|
|
ENDWHILE
|
|
|
|
; Activate gripper (digital output 1)
|
|
$OUT[1] = TRUE
|
|
MsgNotify("Gripper ON - $OUT[1] = TRUE", "RSIPI_Test")
|
|
WAIT SEC 1.0
|
|
|
|
; Python should see Digout.o1 = TRUE in its send_variables
|
|
; Signal to Python that gripper is on
|
|
$TECH.C[11] = 41 ; Sub-state: gripper activated
|
|
|
|
; Wait for Python acknowledgement (command = 41)
|
|
python_cmd = 0
|
|
WHILE (python_cmd <> 41)
|
|
python_cmd = $TECH.T[11]
|
|
WAIT SEC 0.012
|
|
ENDWHILE
|
|
|
|
; Deactivate gripper
|
|
$OUT[1] = FALSE
|
|
MsgNotify("Gripper OFF - $OUT[1] = FALSE", "RSIPI_Test")
|
|
WAIT SEC 0.5
|
|
|
|
; Signal gripper off
|
|
$TECH.C[11] = 42 ; Sub-state: gripper deactivated
|
|
|
|
; Wait for Python acknowledgement
|
|
python_cmd = 0
|
|
WHILE (python_cmd <> 42)
|
|
python_cmd = $TECH.T[11]
|
|
WAIT SEC 0.012
|
|
ENDWHILE
|
|
|
|
; ========================================================================
|
|
; PHASE 6: Shutdown
|
|
; ========================================================================
|
|
|
|
; Signal state 5: complete
|
|
$TECH.C[11] = 5
|
|
MsgNotify("Test complete - shutting down RSI", "RSIPI_Test")
|
|
|
|
; Wait for Python to acknowledge shutdown (command = 5)
|
|
python_cmd = 0
|
|
WHILE (python_cmd <> 5)
|
|
python_cmd = $TECH.T[11]
|
|
WAIT SEC 0.012
|
|
ENDWHILE
|
|
|
|
; Clean up RSI
|
|
ret = RSI_OFF()
|
|
IF (ret <> RSIOK) THEN
|
|
MsgNotify("RSI_OFF warning", "RSIPI_Test")
|
|
ENDIF
|
|
|
|
ret = RSI_DELETE(CONTID)
|
|
|
|
; Return to start
|
|
PTP start_pos
|
|
|
|
MsgNotify("RSIPI_Test complete!", "RSIPI_Test")
|
|
|
|
END
|