diff --git a/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsix b/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsix index 9b40139..590aab6 100644 --- a/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsix +++ b/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsix @@ -6,8 +6,14 @@ - - + + + + + + + + @@ -65,24 +71,162 @@ - + + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -2400,8 +2544,14 @@ - - + + + + + + + + @@ -2455,12 +2605,141 @@ - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml b/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml index 22bedd1..1c90970 100755 --- a/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml +++ b/workspaces/kuka/KSS_above_v8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml @@ -10,25 +10,43 @@ - - - - + + + + + + + + + + - + - - - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/KSS_above_v8.5/backup.bat b/workspaces/kuka/KSS_above_v8.5/backup.bat new file mode 100644 index 0000000..13f1891 --- /dev/null +++ b/workspaces/kuka/KSS_above_v8.5/backup.bat @@ -0,0 +1,73 @@ +@echo off +setlocal enabledelayedexpansion + +REM ============================================ +REM KUKA KSSv8.6 b_ctrldbox Backup Script +REM Copies the CURRENT on-controller files that +REM deploy.bat would overwrite into a timestamped +REM backup folder, for manual comparison later. +REM ============================================ + +echo. +echo ============================================ +echo KUKA KSSv8.6 b_ctrldbox Backup +echo ============================================ +echo. + +REM ============================================ +REM PATH CONFIGURATION - must match deploy.bat +REM ============================================ +set "DST_RSI_CONFIG=C:\KRC\ROBOTER\Config\User\Common\SensorInterface" +set "DST_EKI_CONFIG=C:\KRC\ROBOTER\Config\User\Common\EthernetKRL" +set "DST_PROGRAM=C:\KRC\ROBOTER\KRC\R1\Program\b_ctrldbox" + +REM ============================================ +REM TIMESTAMP +REM ============================================ +for /f "skip=1" %%x in ('wmic os get localdatetime') do if not defined MyDate set "MyDate=%%x" +set "TIMESTAMP=%MyDate:~0,8%_%MyDate:~8,6%" + +set "BACKUP_ROOT=%~dp0Backup\%TIMESTAMP%" + +echo Backup folder: +echo %BACKUP_ROOT% +echo. + +call :BackupFolder "RSI config files" "%DST_RSI_CONFIG%" "%BACKUP_ROOT%\Config\User\Common\SensorInterface" +call :BackupFolder "EKI config files" "%DST_EKI_CONFIG%" "%BACKUP_ROOT%\Config\User\Common\EthernetKRL" +call :BackupFolder "Program files (RSI+EKI)" "%DST_PROGRAM%" "%BACKUP_ROOT%\KRC\R1\Program\b_ctrldbox" + +echo. +echo ============================================ +echo Backup finished. +echo Backed up files are in: +echo %BACKUP_ROOT% +echo ============================================ +pause +goto :eof + +REM ============================================ +REM FUNCTION: BackupFolder +REM Arguments: %1=Description, %2=Source (current on-controller), %3=Destination (backup) +REM ============================================ +:BackupFolder +set "desc=%~1" +set "src=%~2" +set "dst=%~3" + +echo. +echo ============================================ +echo BACKUP: %desc% +echo FROM: %src% +echo TO: %dst% +echo ============================================ + +if not exist "%src%\*" ( + echo Source folder does not exist or is empty - nothing to back up. + goto :eof +) + +if not exist "%dst%\" mkdir "%dst%" +xcopy /Y /E /I "%src%\*" "%dst%\" +echo Backup completed. +goto :eof diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/EthernetKRL/b_ctrldbox_EkiKSSinterface.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/EthernetKRL/b_ctrldbox_EkiKSSinterface.xml new file mode 100755 index 0000000..d85554d --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/EthernetKRL/b_ctrldbox_EkiKSSinterface.xml @@ -0,0 +1,50 @@ + + + + Client + + + Submit + + + 10.28.23.240 + 54600 + TCP + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi new file mode 100644 index 0000000..ab7028a --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi.diagram b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi.diagram new file mode 100644 index 0000000..1e07f65 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi.diagram @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi.xml new file mode 100644 index 0000000..3230632 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi.rsi.xml @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi_eth.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi_eth.xml new file mode 100644 index 0000000..9421227 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/b_ctrldbox_rsi_eth.xml @@ -0,0 +1,29 @@ + + + 10.23.23.28 + 28283 + KROSHU + FALSE + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi new file mode 100644 index 0000000..9f134b9 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi @@ -0,0 +1,163 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi.diagram b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi.diagram new file mode 100644 index 0000000..fb8064b --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi.diagram @@ -0,0 +1,266 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi.xml new file mode 100644 index 0000000..eea5065 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi.rsi.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi_eth.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi_eth.xml new file mode 100644 index 0000000..0ee9fea --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/ext_axis/b_ctrldbox_rsi_eth.xml @@ -0,0 +1,31 @@ + + + 10.23.23.28 + 28283 + KROSHU + FALSE + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi new file mode 100644 index 0000000..fcd9194 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi @@ -0,0 +1,407 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi.diagram b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi.diagram new file mode 100644 index 0000000..ce7211b --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi.diagram @@ -0,0 +1,652 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi.xml new file mode 100644 index 0000000..05df083 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi.rsi.xml @@ -0,0 +1,229 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml new file mode 100644 index 0000000..705064c --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/Config/User/Common/SensorInterface/gpios/b_ctrldbox_rsi_eth.xml @@ -0,0 +1,52 @@ + + + 10.23.23.28 + 28283 + KROSHU + FALSE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/command_handler.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/command_handler.dat new file mode 100755 index 0000000..a28917b --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/command_handler.dat @@ -0,0 +1,48 @@ +&ACCESS R +DEFDAT command_handler PUBLIC + ; FOLD Custom enumerations + GLOBAL CONST INT STATE_COUNT = 3 + GLOBAL ENUM DriverState INITIALIZED, INACTIVE, ACTIVE + + GLOBAL CONST INT EVENT_COUNT = 13 + GLOBAL ENUM EventType NONE, INVALID, STARTED, STOPPED, CANCELLED, RESET_OK, ERROR, CONNECTED, SWITCH_OK, DRIVES_TURNED_ON, DRIVES_TURNED_OFF, CYCLE_TIME_CHANGED, STATUS_UPDATED + + GLOBAL CONST INT COMMAND_COUNT = 9 + GLOBAL ENUM CommandType CMD_NONE, CMD_CON, CMD_START, CMD_RESET, CMD_CANCEL, CMD_CHANGE, CMD_INVALID, CMD_DRIVES_ON, CMD_DRIVES_OFF, CMD_CHANGE_CYCLE_TIME + + GLOBAL CONST INT CONTROL_MODE_COUNT = 3 + GLOBAL ENUM ControlMode INACTIVE, JOINT_POSITION, CART_POSITION + ; ENDFOLD + + ; FOLD Custom structure types + GLOBAL STRUC Command INT cmd_int, INT control_mode, INT cycle_time + ; ENDFOLD + + ; FOLD State variables + DECL DriverState current_driver_state = #INITIALIZED + ; ENDFOLD + + ; FOLD Constants for mapping driver states to valid commands + CONST INT INITIALIZED_VALID_COUNT = 5 + DECL CONST CommandType INIT_VALID_COMMANDS[16] + INIT_VALID_COMMANDS[1] = #CMD_CON + INIT_VALID_COMMANDS[2] = #CMD_CHANGE + INIT_VALID_COMMANDS[3] = #CMD_DRIVES_ON + INIT_VALID_COMMANDS[4] = #CMD_DRIVES_OFF + INIT_VALID_COMMANDS[5] = #CMD_CHANGE_CYCLE_TIME + + CONST INT INACTIVE_VALID_COUNT = 6 + DECL CONST CommandType INACTIVE_VALID_COMMANDS[16] + INACTIVE_VALID_COMMANDS[1] = #CMD_START + INACTIVE_VALID_COMMANDS[2] = #CMD_CHANGE + INACTIVE_VALID_COMMANDS[3] = #CMD_CANCEL + INACTIVE_VALID_COMMANDS[4] = #CMD_DRIVES_ON + INACTIVE_VALID_COMMANDS[5] = #CMD_DRIVES_OFF + INACTIVE_VALID_COMMANDS[6] = #CMD_CHANGE_CYCLE_TIME + + CONST INT ACTIVE_VALID_COUNT = 2 + DECL CONST CommandType ACTIVE_VALID_COMMANDS[16] + ACTIVE_VALID_COMMANDS[1] = #CMD_CANCEL + ACTIVE_VALID_COMMANDS[2] = #CMD_RESET + ; ENDFOLD +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/command_handler.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/command_handler.src new file mode 100755 index 0000000..c29072d --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/command_handler.src @@ -0,0 +1,180 @@ +&ACCESS R +DEF command_handler() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEF ROS_HandleCommand(cmd: IN) + DECL Command cmd + DECL CommandType cmd_enum + + cmd_enum = ROS_IntToCommandType(cmd.cmd_int) + + IF cmd_enum == #CMD_NONE THEN + ; If nothing was received, send status update, if necessary + ROS_UpdateStatus() + RETURN + ENDIF + + ; Report error if the command was invalid + IF NOT IsCommandValid(cmd_enum) THEN + ROS_SetLastEvent(#INVALID) + ROS_Error("Invalid transition requested") + RETURN + ENDIF + + SWITCH cmd_enum + CASE #CMD_CON + ROS_SetLastEvent(#CONNECTED) + CASE #CMD_START + StartProgram() + CASE #CMD_CANCEL + CancelProgram() + CASE #CMD_RESET + ResetProgram() + CASE #CMD_CHANGE + ChangeControlMode(cmd) + CASE #CMD_DRIVES_ON + $DRIVES_ENABLE = TRUE + CONFIRM 0 + ROS_SetLastEvent(#DRIVES_TURNED_ON) + CASE #CMD_DRIVES_OFF + $DRIVES_ENABLE = FALSE + ROS_SetLastEvent(#DRIVES_TURNED_OFF) + CASE #CMD_CHANGE_CYCLE_TIME + ChangeCycleTime(cmd) + DEFAULT + ROS_SetLastEvent(#INVALID) + ENDSWITCH +END + +DEFFCT BOOL IsCommandValid(cmd: IN) + DECL CommandType cmd + + ; Actual check + SWITCH current_driver_state + CASE #INITIALIZED + RETURN IsCommandValidInInit(cmd) + CASE #INACTIVE + RETURN IsCommandValidInInactive(cmd) + CASE #ACTIVE + RETURN IsCommandValidInActive(cmd) + ENDSWITCH +ENDFCT + +; Checks if the specified command enum is a valid command in the initialized state +DEFFCT BOOL IsCommandValidInInit(cmd: IN) + DECL CommandType cmd + INT idx + + FOR idx = 1 TO INITIALIZED_VALID_COUNT + IF INIT_VALID_COMMANDS[idx] == cmd THEN + RETURN TRUE + ENDIF + ENDFOR + + RETURN FALSE +ENDFCT + +; Checks if the specified command enum is a valid command in the inactive state +DEFFCT BOOL IsCommandValidInInactive(cmd: IN) + DECL CommandType cmd + INT idx + + FOR idx = 1 TO INACTIVE_VALID_COUNT + IF INACTIVE_VALID_COMMANDS[idx] == cmd THEN + RETURN TRUE + ENDIF + ENDFOR + + RETURN FALSE +ENDFCT + +; Checks if the specified command enum is a valid command in the active state +DEFFCT BOOL IsCommandValidInActive(cmd: IN) + DECL CommandType cmd + INT idx + + FOR idx = 1 TO ACTIVE_VALID_COUNT + IF ACTIVE_VALID_COMMANDS[idx] == cmd THEN + RETURN TRUE + ENDIF + ENDFOR + + RETURN FALSE +ENDFCT + +; Resets the command handler state. Should be called whenever the external client disconnects. +GLOBAL DEF ROS_ResetCommandHandler() + current_driver_state = #INITIALIZED +END + +DEF StartProgram() + CONFIRM 0 + IF ROS_StartProgram() THEN + current_driver_state = #Active + ROS_SetLastEvent(#STARTED) + ELSE + ROS_SetLastEvent(#ERROR) + ENDIF +END + +DEF CancelProgram() + CONFIRM 0 + IF ROS_CancelProgram() THEN + current_driver_state = #Initialized + ROS_SelectControlMode(#INACTIVE) + ROS_SetLastEvent(#CANCELLED) + ELSE + ROS_SetLastEvent(#ERROR) + ENDIF +END + +DEF ResetProgram() + CONFIRM 0 + IF ROS_ResetProgram() THEN + current_driver_state = #INACTIVE + ROS_SetLastEvent(#RESET_OK) + ELSE + ROS_SetLastEvent(#ERROR) + ENDIF +END + +DEF ChangeControlMode(command_struc: IN) + DECL Command command_struc + DECL ControlMode control_mode + + control_mode = ROS_IntToControlMode(command_struc.control_mode) + IF control_mode == #INACTIVE THEN + ROS_SetLastEvent(#ERROR) + RETURN + ENDIF + + ROS_SelectControlMode(control_mode) + current_driver_state = #INACTIVE + ROS_SetLastEvent(#SWITCH_OK) +END + +DEF ChangeCycleTime(command_struc: IN) + DECL Command command_struc + + IF NOT ROS_SelectCycleTime(command_struc.cycle_time) THEN + ROS_SetLastEvent(#ERROR) + RETURN + ENDIF + + ROS_SetLastEvent(#CYCLE_TIME_CHANGED) +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/driver.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/driver.src new file mode 100755 index 0000000..d548495 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/driver.src @@ -0,0 +1,43 @@ +&ACCESS R +def driver() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEF SPS_Init() + EKI_ServerStart() +END + +GLOBAL DEF SPS_LoopCall() + DECL Command cmd + + ; Check for connection/disconnection events + ROS_HandleConnection() + + IF IsClientConnected() THEN + ; Parse commands into Command structure + EKI_ReceiveData(cmd) + + ; Handle parsed command + ROS_HandleCommand(cmd) + + ; Publish state transition or error events + ROS_PublishEvent() + ELSE + ; Wait for a tick if there is no active connection to avoid CPU errors + WAIT SEC 0.012 + ENDIF +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/eki_communication.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/eki_communication.dat new file mode 100755 index 0000000..94c3579 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/eki_communication.dat @@ -0,0 +1,12 @@ +&ACCESS R +DEFDAT eki_communication PUBLIC + GLOBAL CONST CHAR INTERFACE_NAME[64] + INTERFACE_NAME[] = "b_ctrldbox_EkiKSSinterface" + CONST CHAR INTERFACE_VERSION[16] + INTERFACE_VERSION[] = "1.0.0" + CONST INT ROS_EKI_CONN_FLAG = 105 + CONST INT ROS_EKI_RECV_FLAG = 110 + + DECL BOOL connection_flag = FALSE + DECL EventType last_event = #NONE +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/eki_communication.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/eki_communication.src new file mode 100755 index 0000000..21ef468 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/eki_communication.src @@ -0,0 +1,156 @@ +&ACCESS R +DEF eki_communication() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEF EKI_ServerStart() + DECL EKI_STATUS ret + + ResetComponents() + + ; Init connection flag - will be set TRUE internally by EKI after connection is made + $FLAG[ROS_EKI_CONN_FLAG] = FALSE + ; Init data recv flag - will be set TRUE internally by EKI after data is received + $FLAG[ROS_EKI_RECV_FLAG] = FALSE + + ret = EKI_Init(INTERFACE_NAME[]) + ret = EKI_Open(INTERFACE_NAME[]) +END + +GLOBAL DEF EKI_ServerReset() + DECL EKI_STATUS ret + ret = EKI_Clear(INTERFACE_NAME[]) + EKI_ServerStart() +END + +DEF ResetComponents() + ROS_ResetCommandHandler() + ROS_ResetProgramHandler() + ROS_ResetStatus() +END + +; Tries to read received elements from buffer. +GLOBAL DEF EKI_ReceiveData(command: OUT) + DECL Command command + DECL EKI_STATUS ret + + ; Init cmd + command.cmd_int = 5 ; By default use the none command + command.control_mode = -1 + command.cycle_time = -1 + + ; Verify a valid connection + IF NOT IsClientConnected() THEN + WAIT SEC 0.012 + RETURN + ENDIF + + ; Skip processing if no data received. + IF NOT DidReceiveData() THEN + WAIT SEC 0.012 + RETURN + ENDIF + + ; Reset flag immediately after msg notification + $FLAG[ROS_EKI_RECV_FLAG] = FALSE + + ret = EKI_CheckBuffer(INTERFACE_NAME[], "External/@REQTYPE") + + IF ret.buff > 0 THEN + ret = EKI_GetInt(INTERFACE_NAME[], "External/@REQTYPE", command.cmd_int) + ret = EKI_GetInt(INTERFACE_NAME[], "External/@ControlMode", command.control_mode) + ret = EKI_GetInt(INTERFACE_NAME[], "External/@CycleTime", command.cycle_time) + ELSE + ROS_Error("No valid data received") + WAIT SEC 0.012 + ENDIF +END + +GLOBAL DEF ROS_HandleConnection() + IF IsClientConnected() <> connection_flag THEN + connection_flag = IsClientConnected() + IF IsClientConnected() THEN + ROS_Info("External client connected") + ELSE + ROS_Info("External client disconnected") + EKI_ServerReset() + ENDIF + ENDIF +END + +GLOBAL DEF ROS_PublishEvent() + DECL EKI_STATUS ret + INT event_int + CHAR x_path_send[32] + + ; Before publishing a response to a command, select the relevant part of the XML + x_path_send[] = "Robot/Response" + + IF last_event <> #NONE THEN + IF NOT IsClientConnected() THEN + ROS_Error("External client is not connected when trying to send event") + RETURN + ENDIF + + event_int = ROS_EventTypeToInt(last_event) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Response/@EventID", event_int) + + ; Only list commands that need special handling + SWITCH last_event + CASE #CONNECTED + ROS_WriteInitData() + x_path_send[] = "Robot/Init" + CASE #SWITCH_OK + ret = EKI_SetString(INTERFACE_NAME[], "Robot/Response", ROS_GetSelCtrlModeStr()) + CASE #STATUS_UPDATED + x_path_send[] = "Robot/Status" + ENDSWITCH + + ; Send the message + ret = EKI_Send(INTERFACE_NAME[], x_path_send[]) + + ; Reset event + ROS_SetLastEvent(#NONE) + ret = EKI_SetString(INTERFACE_NAME[], "Robot/Response", " ") + ENDIF +END + +GLOBAL DEF ROS_SetLastEvent(event: IN) + DECL EventType event + last_event = event +END + +GLOBAL DEFFCT BOOL IsClientConnected() + RETURN $FLAG[ROS_EKI_CONN_FLAG] +ENDFCT + +DEFFCT BOOL DidReceiveData() + RETURN $FLAG[ROS_EKI_RECV_FLAG] +ENDFCT + +GLOBAL DEF ROS_WriteInitData() + DECL EKI_STATUS eki_ret + + ROS_WriteStatus() + + ; Set general parameters + eki_ret = EKI_SetString(INTERFACE_NAME[], "Robot/Init/@VER", INTERFACE_VERSION[]) + eki_ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Init/@NumAxes", $NUM_AX) + eki_ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Init/@NumExternalAxes", $EX_AX_NUM) + eki_ret = EKI_SetString(INTERFACE_NAME[], "Robot/Init/@Model", $ROBTRAFO[]) + eki_ret = EKI_SetString(INTERFACE_NAME[], "Robot/Init/@RobVer", $V_R1MADA[]) +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/logging.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/logging.dat new file mode 100644 index 0000000..084dd88 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/logging.dat @@ -0,0 +1,4 @@ +&ACCESS R +DEFDAT logging PUBLIC + BOOL DEBUG_ON = FALSE +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/logging.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/logging.src new file mode 100644 index 0000000..15e6b92 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/logging.src @@ -0,0 +1,64 @@ +&ACCESS R +DEF logging() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEF ROS_Info(msg[]: IN) + CHAR msg[] + MsgNotify(msg[], "INFO") +END + +GLOBAL DEF ROS_Debug(msg[]: IN) + CHAR msg[] + IF DEBUG_ON THEN + MsgNotify(msg[], "DEBUG") + ENDIF +END + +GLOBAL DEF ROS_Error(msg[]: IN) + CHAR msg[] + MsgNotify(msg[], "ERROR") +END + +GLOBAL DEF ROS_Info1String(msg[]: IN, str[]: IN) + CHAR msg[] + CHAR str[] + MsgNotify(msg[], "INFO",, str[]) +END + +GLOBAL DEF ROS_Debug1String(msg[]: IN, str[]: IN) + CHAR msg[] + CHAR str[] + IF DEBUG_ON THEN + MsgNotify(msg[], "DEBUG",, str[]) + ENDIF +END + +GLOBAL DEF ROS_Error1String(msg[]: IN, str[]: IN) + CHAR msg[] + CHAR str[] + MsgNotify(msg[], "ERROR",, str[]) +END + +GLOBAL DEF ROS_Debug2Strings(msg[]: IN, str1[]: IN, str2[]: IN) + CHAR msg[] + CHAR str1[] + CHAR str2[] + IF DEBUG_ON THEN + MsgNotifyTextPar(msg[], "DEBUG", 0, str1[], str2[]) + ENDIF +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/mappings.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/mappings.dat new file mode 100644 index 0000000..e9305a2 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/mappings.dat @@ -0,0 +1,76 @@ +&ACCESS R +DEFDAT mappings PUBLIC + ; FOLD Custom structure types + STRUC AxisTypeTuple INT axis_type_int, CHAR axis_type_string[32] + STRUC ControlModeTuple ControlMode control_mode, CHAR control_mode_string[32], INT control_mode_int + STRUC CycleTimeTuple ROS_CycleTime cycle_time, CHAR cycle_time_string[32], INT cycle_time_int + STRUC DriverStateTuple DriverState driver_state, CHAR driver_state_string[32] + STRUC ProgramConfiguration ControlMode control_mode, ROS_CycleTime cycle_time, CHAR program[64] + ; ENDFOLD + + ; FOLD Constants for mapping axis type integers to strings and vice versa + CONST INT AXIS_TYPE_COUNT = 3 + DECL CONST AxisTypeTuple AXIS_TYPE_TUPLES[3] + AXIS_TYPE_TUPLES[1] = { axis_type_int 1, axis_type_string[] "LINEAR" } + AXIS_TYPE_TUPLES[2] = { axis_type_int 3, axis_type_string[] "ROTATIONAL" } + AXIS_TYPE_TUPLES[3] = { axis_type_int 5, axis_type_string[] "ENDLESS" } + ; ENDFOLD + + ; FOLD Constants for mapping commands + DECL CONST CommandType COMMAND_TYPES[9] + COMMAND_TYPES[1] = #CMD_CON + COMMAND_TYPES[2] = #CMD_START + COMMAND_TYPES[3] = #CMD_RESET + COMMAND_TYPES[4] = #CMD_CANCEL + COMMAND_TYPES[5] = #CMD_CHANGE + COMMAND_TYPES[6] = #CMD_NONE + COMMAND_TYPES[7] = #CMD_DRIVES_ON + COMMAND_TYPES[8] = #CMD_DRIVES_OFF + COMMAND_TYPES[9] = #CMD_CHANGE_CYCLE_TIME + ; ENDFOLD + + ; FOLD Constants for mapping control modes to strings/integers and vice versa + DECL CONST ControlModeTuple CONTROL_MODE_TUPLES[3] + CONTROL_MODE_TUPLES[1] = { control_mode #INACTIVE, control_mode_string[] "Inactive Control", control_mode_int 0 } + CONTROL_MODE_TUPLES[2] = { control_mode #JOINT_POSITION, control_mode_string[] "Joint Position Control", control_mode_int 1 } + CONTROL_MODE_TUPLES[3] = { control_mode #CART_POSITION, control_mode_string[] "Cartesian Position Control", control_mode_int 5 } + ; ENDFOLD + + ; FOLD Constants for mapping strings to ROS_CycleTime values and vice versa + DECL CONST CycleTimeTuple CYCLE_TIME_TUPLES[3] + CYCLE_TIME_TUPLES[1] = { cycle_time #NONE_CYCLE_TIME, cycle_time_string[] "UNSPECIFIED", cycle_time_int 0 } + CYCLE_TIME_TUPLES[2] = { cycle_time #RSI_4MS, cycle_time_string[] "RSI_4MS", cycle_time_int 1 } + CYCLE_TIME_TUPLES[3] = { cycle_time #RSI_12MS, cycle_time_string[] "RSI_12MS", cycle_time_int 2 } + ; ENDFOLD + + ; FOLD Constants for mapping driver states to strings and vice versa + DECL CONST DriverStateTuple DRIVER_STATE_TUPLES[3] + DRIVER_STATE_TUPLES[1] = { driver_state #INITIALIZED, driver_state_string[] "INITIALIZED" } + DRIVER_STATE_TUPLES[2] = { driver_state #INACTIVE, driver_state_string[] "INACTIVE" } + DRIVER_STATE_TUPLES[3] = { driver_state #ACTIVE, driver_state_string[] "ACTIVE" } + ; ENDFOLD + + ; FOLD Constants for easier mapping of event types to integers + DECL CONST EventType EVENT_TYPES[13] + EVENT_TYPES[1] = #STARTED + EVENT_TYPES[2] = #STOPPED + EVENT_TYPES[3] = #CANCELLED + EVENT_TYPES[4] = #RESET_OK + EVENT_TYPES[5] = #ERROR + EVENT_TYPES[6] = #CONNECTED + EVENT_TYPES[7] = #SWITCH_OK + EVENT_TYPES[8] = #INVALID + EVENT_TYPES[9] = #NONE + EVENT_TYPES[10] = #DRIVES_TURNED_ON + EVENT_TYPES[11] = #DRIVES_TURNED_OFF + EVENT_TYPES[12] = #CYCLE_TIME_CHANGED + EVENT_TYPES[13] = #STATUS_UPDATED + ; ENDFOLD + + ; FOLD Constants for mapping program configurations to programs to run + CONST INT PROGRAM_CONFIG_COUNT = 2 + DECL CONST ProgramConfiguration PROGRAM_CONFIGS[2] + PROGRAM_CONFIGS[1] = { control_mode #JOINT_POSITION, cycle_time #RSI_4MS, program[] "R1/rsi_joint_pos_4ms()" } + PROGRAM_CONFIGS[2] = { control_mode #JOINT_POSITION, cycle_time #RSI_12MS, program[] "R1/rsi_joint_pos_12ms()" } + ; ENDFOLD +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/mappings.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/mappings.src new file mode 100644 index 0000000..095fd64 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/mappings.src @@ -0,0 +1,213 @@ +&ACCESS R +DEF mappings() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +; Converts a boolean to an integer +GLOBAL DEFFCT INT ROS_BoolToInt(boolean: IN) + BOOL boolean + + IF boolean THEN + RETURN 1 + ENDIF + + RETURN 0 +ENDFCT + +; Returns the current operation mode as an integer +GLOBAL DEFFCT INT ROS_OperationModeToInt() + IF $T1 THEN + RETURN 1 + ENDIF + + IF $T2 THEN + RETURN 2 + ENDIF + + IF $AUT THEN + RETURN 3 + ENDIF + + ; $EXT + RETURN 4 +ENDFCT + +; Return the axis type string associated with the given integer +GLOBAL DEFFCT CHAR [32] ROS_AxisTypeIntToString(axis_type_int: IN) + INT axis_type_int + INT idx + + FOR idx = 1 TO AXIS_TYPE_COUNT + IF AXIS_TYPE_TUPLES[idx].axis_type_int == axis_type_int THEN + RETURN AXIS_TYPE_TUPLES[idx].axis_type_string[] + ENDIF + ENDFOR + + ; Unreachable + RETURN "INVALID" +ENDFCT + +; Returns the command type associated with the given integer +GLOBAL DEFFCT CommandType ROS_IntToCommandType(command_type_int: IN) + INT command_type_int + INT idx + + FOR idx = 1 TO COMMAND_COUNT + IF command_type_int == idx - 1 THEN + RETURN COMMAND_TYPES[idx] + ENDIF + ENDFOR + + RETURN #CMD_INVALID +ENDFCT + +; Returns the control mode associated with the given integer +GLOBAL DEFFCT ControlMode ROS_IntToControlMode(control_mode_int: IN) + INT control_mode_int + INT idx + + FOR idx = 1 TO CONTROL_MODE_COUNT + IF CONTROL_MODE_TUPLES[idx].control_mode_int == control_mode_int THEN + RETURN CONTROL_MODE_TUPLES[idx].control_mode + ENDIF + ENDFOR + + RETURN #INACTIVE +ENDFCT + +; Returns the string representation of the specified control mode +GLOBAL DEFFCT CHAR [32] ROS_ControlModeToString(control_mode: IN) + DECL ControlMode control_mode + INT idx + + FOR idx = 1 TO CONTROL_MODE_COUNT + IF CONTROL_MODE_TUPLES[idx].control_mode == control_mode THEN + RETURN CONTROL_MODE_TUPLES[idx].control_mode_string[] + ENDIF + ENDFOR + + ; Unreachable + RETURN "NOT_FOUND" +ENDFCT + +; Returns the integer representation of the specified control mode +GLOBAL DEFFCT INT ROS_ControlModeToInt(control_mode: IN) + DECL ControlMode control_mode + INT idx + + FOR idx = 1 TO CONTROL_MODE_COUNT + IF CONTROL_MODE_TUPLES[idx].control_mode == control_mode THEN + RETURN CONTROL_MODE_TUPLES[idx].control_mode_int + ENDIF + ENDFOR + + ; Unreachable + RETURN -1 +ENDFCT + +; Return the cycle time associated with the given integer +GLOBAL DEFFCT ROS_CycleTime ROS_IntToCycleTime(cycle_time_int: IN) + INT cycle_time_int + INT idx + + FOR idx = 1 TO CYCLE_TIME_COUNT + IF CYCLE_TIME_TUPLES[idx].cycle_time_int == cycle_time_int THEN + RETURN CYCLE_TIME_TUPLES[idx].cycle_time + ENDIF + ENDFOR + + RETURN #NONE_CYCLE_TIME +ENDFCT + +; Returns the string representation of the specified cycle time +GLOBAL DEFFCT CHAR [32] ROS_CycleTimeToString(cycle_time: IN) + DECL ROS_CycleTime cycle_time + INT idx + + FOR idx = 1 TO CYCLE_TIME_COUNT + IF CYCLE_TIME_TUPLES[idx].cycle_time == cycle_time THEN + RETURN CYCLE_TIME_TUPLES[idx].cycle_time_string[] + ENDIF + ENDFOR + + ; Unreachable + RETURN "NOT_FOUND" +ENDFCT + +; Returns the integer representation of the specified cycle time +GLOBAL DEFFCT INT ROS_CycleTimeToInt(cycle_time: IN) + DECL ROS_CycleTime cycle_time + INT idx + + FOR idx = 1 TO CYCLE_TIME_COUNT + IF CYCLE_TIME_TUPLES[idx].cycle_time == cycle_time THEN + RETURN CYCLE_TIME_TUPLES[idx].cycle_time_int + ENDIF + ENDFOR + + ; Unreachable + RETURN -1 +ENDFCT + +; Returns the string represenation of the specified driver state +GLOBAL DEFFCT CHAR [32] ROS_DriverStateToString(driver_state: IN) + DECL DriverState driver_state + INT idx + + FOR idx = 1 TO STATE_COUNT + IF DRIVER_STATE_TUPLES[idx].driver_state == driver_state THEN + RETURN DRIVER_STATE_TUPLES[idx].driver_state_string[] + ENDIF + ENDFOR + + ; Unreachable + RETURN "INVALID" +ENDFCT + +; Returns the integer representation of the specified event type +GLOBAL DEFFCT INT ROS_EventTypeToInt(event_type: IN) + DECL EventType event_type + INT idx + + FOR idx = 1 TO EVENT_COUNT + IF event_type == EVENT_TYPES[idx] THEN + RETURN idx - 1 + ENDIF + ENDFOR + + ; Unreachable + RETURN -1 +ENDFCT + +; Maps the specifed control mode and cycle time to a program name if available +GLOBAL DEFFCT CHAR [64] ROS_GetProgramName(control_mode: IN, cycle_time: IN) + DECL ControlMode control_mode + DECL ROS_CycleTime cycle_time + INT idx + BOOL control_mode_ok + BOOL cycle_time_ok + + FOR idx = 1 TO PROGRAM_CONFIG_COUNT + control_mode_ok = PROGRAM_CONFIGS[idx].control_mode == control_mode + cycle_time_ok = PROGRAM_CONFIGS[idx].cycle_time == cycle_time + IF control_mode_ok AND cycle_time_ok THEN + RETURN PROGRAM_CONFIGS[idx].program[] + ENDIF + ENDFOR + + RETURN "INVALID" +ENDFCT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/program_handler.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/program_handler.dat new file mode 100755 index 0000000..cd4a4af --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/program_handler.dat @@ -0,0 +1,16 @@ +&ACCESS R +DEFDAT program_handler PUBLIC + ; FOLD Custom enumerations + GLOBAL CONST INT CYCLE_TIME_COUNT = 3 + GLOBAL ENUM ROS_CycleTime NONE_CYCLE_TIME, RSI_4MS, RSI_12MS + ; ENDFOLD + + CONST INT INTEGER_MAX = 2147483647 + DECL STATE_T statement_state + DECL MODUS_T command_mode = #SYNC + + ; FOLD Program handler state variables + DECL ControlMode selected_control_mode = #INACTIVE + DECL ROS_CycleTime selected_cycle_time = #RSI_4MS + ; ENDFOLD +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/program_handler.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/program_handler.src new file mode 100755 index 0000000..3cb7a3d --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/program_handler.src @@ -0,0 +1,206 @@ +&ACCESS R +DEF program_handler() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEFFCT BOOL ROS_StartProgram() + CHAR program_name[64] + CHAR s[256] + INT k + + program_name[] = ROS_GetProgramName(selected_control_mode, selected_cycle_time) + + ; Check to see if a program is already selected - should not happen + IF $PRO_STATE1 <> #P_FREE THEN + ; If a program is already selected, log error and return + ROS_Error1String("Program %1 is already selected", $PRO_NAME1[]) + RETURN FALSE + ENDIF + + ; Select the new program + s[] = "RUN/" + k = strAdd(s[], program_name[]) + + CWRITE($CMD, statement_state, command_mode, s[]) + IF NOT WaitForStateTransition(#P_FREE, 1000) THEN + ROS_Error1String("Timeout selecting program %1", program_name[]) + RETURN FALSE + ENDIF + + RETURN TRUE +ENDFCT + +GLOBAL DEFFCT BOOL ROS_CancelProgram() + ROS_Debug("Cancelling program...") + + ; Check to see if no program is selected + IF $PRO_STATE1 == #P_FREE THEN + RETURN TRUE + ENDIF + + IF $PRO_STATE1 == #P_ACTIVE THEN + ROS_Debug("Sending stop command") + CWRITE($CMD, statement_state, command_mode, "STOP 1") + IF NOT WaitForStateTransition(#P_ACTIVE, 1000) THEN + ROS_Error("Timeout stopping program") + RETURN FALSE + ENDIF + ENDIF + + ; Cancel the program + ROS_Debug("Sending cancel command") + CWRITE($CMD, statement_state, command_mode, "CANCEL 1") + IF NOT WaitForState(#P_FREE, 1000) THEN + ROS_Error("Timeout cancelling program") + RETURN FALSE + ENDIF + + RETURN TRUE +ENDFCT + +GLOBAL DEFFCT BOOL ROS_ResetProgram() + ; If program is already reset, just return + IF $PRO_STATE1 == #P_RESET THEN + RETURN TRUE + ENDIF + + ; Check for no program selected + IF $PRO_STATE1 == #P_FREE THEN + ROS_Info("No program selected") + RETURN FALSE + ENDIF + + ; If the program is running, stop it + IF $PRO_STATE1 == #P_ACTIVE THEN + CWRITE($CMD,statement_state,command_mode, "STOP 1") + IF NOT WaitForStateTransition(#P_ACTIVE, 1000) THEN + ROS_Error("Timeout stopping program") + RETURN FALSE + ENDIF + ENDIF + + ; Reset the program + CWRITE($CMD,statement_state,command_mode, "RESET 1") + IF NOT WaitForState(#P_RESET, 1000) THEN + ROS_Error("Timeout resetting program") + RETURN FALSE + ENDIF + + RETURN TRUE +ENDFCT + +DEFFCT BOOL WaitForState(ProState: IN, Timeout: IN) + DECL PRO_STATE ProState + INT Timeout + INT StartTime, TriggerTime + + StartTime = $ROB_TIMER + + ; Set the trigger time based on the start time. Account for integer rollover + IF (INTEGER_MAX - Timeout) < StartTime THEN + TriggerTime = Timeout - (INTEGER_MAX-StartTime) + ELSE + TriggerTime = StartTime + Timeout + ENDIF + + IF TriggerTime >= StartTime THEN + ; No worries about timer rollover + REPEAT + WAIT SEC 0.12 + UNTIL ($PRO_STATE1 == ProState) OR ($ROB_TIMER > TriggerTime) OR ($ROB_TIMER < StartTime) + + ELSE + ; Timer rollover must be accounted for + REPEAT + WAIT SEC 0.12 + UNTIL ($PRO_STATE1 == ProState) OR (($ROB_TIMER > TriggerTime) AND ($ROB_TIMER < StartTime)) + ENDIF + + RETURN $PRO_STATE1 == ProState +ENDFCT + +DEFFCT BOOL WaitForStateTransition(ProState: IN, Timeout: IN) + DECL PRO_STATE ProState + INT Timeout + INT StartTime, TriggerTime + + StartTime = $ROB_TIMER + + ; Set the trigger time based on the start time. Account for integer rollover + IF ((INTEGER_MAX-Timeout) < StartTime) THEN + TriggerTime = Timeout - (INTEGER_MAX-StartTime) + ELSE + TriggerTime = StartTime + Timeout + ENDIF + + IF (TriggerTime >= StartTime) THEN + ; No worries about timer rollover + REPEAT + WAIT SEC 0.12 + UNTIL (($PRO_STATE1 <> ProState) OR ($ROB_TIMER > TriggerTime) OR ($ROB_TIMER < StartTime)) + ELSE + ; Timer rollover must be accounted for + REPEAT + WAIT SEC 0.12 + UNTIL (($PRO_STATE1 <> ProState) OR (($ROB_TIMER > TriggerTime) AND ($ROB_TIMER < StartTime))) + ENDIF + + IF ($PRO_STATE1 <> ProState) THEN + RETURN TRUE + ENDIF + + RETURN FALSE +ENDFCT + +GLOBAL DEF ROS_SelectControlMode(mode: IN) + DECL ControlMode mode + + ROS_Info1String("Control mode set to %1", ROS_ControlModeToString(mode)) + selected_control_mode = mode +END + +; Returns the selected control mode as a string +GLOBAL DEFFCT CHAR [32] ROS_GetSelCtrlModeStr() + RETURN ROS_ControlModeToString(selected_control_mode) +ENDFCT + +GLOBAL DEFFCT ControlMode ROS_GetSelControlMode() + RETURN selected_control_mode +ENDFCT + +; Selects the cycle time associated with the specified integer +; Returns TRUE, if a valid integer was provided, otherwise FALSE +GLOBAL DEFFCT BOOL ROS_SelectCycleTime(cycle_time_int: IN) + INT cycle_time_int + + selected_cycle_time = ROS_IntToCycleTime(cycle_time_int) + IF selected_cycle_time == #NONE_CYCLE_TIME THEN + RETURN FALSE + ENDIF + + ROS_Debug1String("Cycle time set to %1", ROS_CycleTimeToString(selected_cycle_time)) + RETURN TRUE +ENDFCT + +GLOBAL DEFFCT ROS_CycleTime ROS_GetSelCycTime() + RETURN selected_cycle_time +ENDFCT + +GLOBAL DEF ROS_ResetProgramHandler() + selected_control_mode = #INACTIVE + selected_cycle_time = #RSI_12MS +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/status.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/status.dat new file mode 100644 index 0000000..f6dd2eb --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/status.dat @@ -0,0 +1,14 @@ +&ACCESS R +DEFDAT status + DECL ControlMode control_mode = #INACTIVE + DECL ROS_CycleTime cycle_time = #NONE_CYCLE_TIME + BOOL drives_powered = FALSE + BOOL emergency_stop = FALSE + BOOL guard_stop = FALSE + BOOL in_motion = FALSE + BOOL motion_possible = FALSE + INT operation_mode = 0 + BOOL robot_stopped = FALSE + + BOOL first_update = TRUE +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/status.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/status.src new file mode 100644 index 0000000..304e3ba --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/EKIserver/status.src @@ -0,0 +1,92 @@ +&ACCESS R +DEF status() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEF ROS_UpdateStatus() + BOOL should_update + should_update = FALSE + + IF ROS_GetSelControlMode() <> control_mode THEN + control_mode = ROS_GetSelControlMode() + should_update = TRUE + ENDIF + + IF ROS_GetSelCycTime() <> cycle_time THEN + cycle_time = ROS_GetSelCycTime() + should_update = TRUE + ENDIF + + IF $PERI_RDY <> drives_powered THEN + drives_powered = $PERI_RDY + should_update = TRUE + ENDIF + + IF (NOT $ALARM_STOP OR NOT $ALARM_STOP_INTERN) <> emergency_stop THEN + emergency_stop = NOT $ALARM_STOP OR NOT $ALARM_STOP_INTERN + should_update = TRUE + ENDIF + + IF (NOT $USER_SAF) <> guard_stop THEN + guard_stop = NOT $USER_SAF + should_update = TRUE + ENDIF + + IF $PRO_MOVE <> in_motion THEN + in_motion = $PRO_MOVE + should_update = TRUE + ENDIF + + IF ($COULD_START_MOTION AND $PRO_ACT) <> motion_possible THEN + motion_possible = $COULD_START_MOTION AND $PRO_ACT + should_update = TRUE + ENDIF + + IF ROS_OperationModeToInt() <> operation_mode THEN + operation_mode = ROS_OperationModeToInt() + should_update = TRUE + ENDIF + + IF $STOPMESS <> robot_stopped THEN + robot_stopped = $STOPMESS + should_update = TRUE + ENDIF + + IF should_update OR first_update THEN + ROS_WriteStatus() + ROS_Debug("Status updated") + ROS_SetLastEvent(#STATUS_UPDATED) + first_update = FALSE + ENDIF +END + +GLOBAL DEF ROS_ResetStatus() + first_update = TRUE +END + +GLOBAL DEF ROS_WriteStatus() + DECL EKI_STATUS ret + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@ControlMode", ROS_ControlModeToInt(control_mode)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@CycleTime", ROS_CycleTimeToInt(cycle_time)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@DrivesPowered", ROS_BoolToInt(drives_powered)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@EmergencyStop", ROS_BoolToInt(emergency_stop)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@GuardStop", ROS_BoolToInt(guard_stop)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@InMotion", ROS_BoolToInt(in_motion)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@MotionPossible", ROS_BoolToInt(motion_possible)) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@OperationMode", operation_mode) + ret = EKI_SetInt(INTERFACE_NAME[], "Robot/Status/@RobotStopped", ROS_BoolToInt(robot_stopped)) +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_helper.dat b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_helper.dat new file mode 100644 index 0000000..bf5fc9c --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_helper.dat @@ -0,0 +1,8 @@ +&ACCESS R +DEFDAT rsi_helper PUBLIC + CONST CHAR AXISCORR_NAME[64] + AXISCORR_NAME[] = "AxisCorr_1" + + CONST CHAR AXISCORRMON_NAME[64] + AXISCORRMON_NAME[] = "AxisCorrMon_1" +ENDDAT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_helper.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_helper.src new file mode 100644 index 0000000..8f6700d --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_helper.src @@ -0,0 +1,102 @@ +&ACCESS R +DEF rsi_helper() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= +END + +GLOBAL DEF ROS_SetAxisCorrLimits(cont_id: IN) + INT cont_id + INT ret + REAL val1 + REAL val2 + + ;======================================== + ; Set AxisCorr parameters + ;======================================== + + ; A1 + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA1", $SOFTN_END[1] - $AXIS_ACT_MEAS.A1) + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA1", $SOFTP_END[1] - $AXIS_ACT_MEAS.A1) + + ; A2 + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA2", $SOFTN_END[2] - $AXIS_ACT_MEAS.A2) + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA2", $SOFTP_END[2] - $AXIS_ACT_MEAS.A2) + + ; A3 + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA3", $SOFTN_END[3] - $AXIS_ACT_MEAS.A3) + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA3", $SOFTP_END[3] - $AXIS_ACT_MEAS.A3) + + ; A4 + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA4", $SOFTN_END[4] - $AXIS_ACT_MEAS.A4) + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA4", $SOFTP_END[4] - $AXIS_ACT_MEAS.A4) + + ; A5 + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA5", $SOFTN_END[5] - $AXIS_ACT_MEAS.A5) + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA5", $SOFTP_END[5] - $AXIS_ACT_MEAS.A5) + + ; A6 + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA6", $SOFTN_END[6] - $AXIS_ACT_MEAS.A6) + ret = RSI_SetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA6", $SOFTP_END[6] - $AXIS_ACT_MEAS.A6) + + ;======================================== + ; Set AxisCorrMon parameters + ;======================================== + + ; A1 + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA1", val1) + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA1", val2) + ret = RSI_SetPublicPar(cont_id, AXISCORRMON_NAME[], "MaxA1", Max(ABS(val1), ABS(val2))) + + ; A2 + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA2", val1) + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA2", val2) + ret = RSI_SetPublicPar(cont_id, AXISCORRMON_NAME[], "MaxA2", Max(ABS(val1), ABS(val2))) + + ; A3 + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA3", val1) + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA3", val2) + ret = RSI_SetPublicPar(cont_id, AXISCORRMON_NAME[], "MaxA3", Max(ABS(val1), ABS(val2))) + + ; A4 + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA4", val1) + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA4", val2) + ret = RSI_SetPublicPar(cont_id, AXISCORRMON_NAME[], "MaxA4", Max(ABS(val1), ABS(val2))) + + ; A5 + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA5", val1) + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA5", val2) + ret = RSI_SetPublicPar(cont_id, AXISCORRMON_NAME[], "MaxA5", Max(ABS(val1), ABS(val2))) + + ; A6 + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "LowerLimA6", val1) + ret = RSI_GetPublicPar(cont_id, AXISCORR_NAME[], "UpperLimA6", val2) + ret = RSI_SetPublicPar(cont_id, AXISCORRMON_NAME[], "MaxA6", Max(ABS(val1), ABS(val2))) +END + +DEFFCT REAL Max(r1: IN, r2: IN) + REAL r1 + REAL r2 + + IF (VarState("r1") <> #INITIALIZED) OR (VarState("r2") <> #INITIALIZED) THEN + RETURN 0.0 + ENDIF + + IF r1 > r2 THEN + RETURN r1 + ENDIF + + RETURN r2 +ENDFCT diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_joint_pos_12ms.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_joint_pos_12ms.src new file mode 100644 index 0000000..283945e --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_joint_pos_12ms.src @@ -0,0 +1,69 @@ +&ACCESS RVP +&PARAM DISKPATH = KRC:\R1\Program\b_ctrldbox +&PARAM EDITMASK = * +&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe +DEF rsi_joint_pos_12ms() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= + + ; Declaration of KRL variables + DECL INT ret + DECL INT cont_id + + ; FOLD INI + ; FOLD BASISTECH INI + GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM () + INTERRUPT ON 3 + BAS (#INITMOV, 0) + ; ENDFOLD (BASISTECH INI) + ; ENDFOLD (INI) + + WAIT SEC 1.0 + + ; Move to start position + PTP $AXIS_ACT_MEAS + + ; Create RSI Context + ret = RSI_CREATE("b_ctrldbox_rsi", cont_id, TRUE) + IF (ret <> RSIOK) THEN + HALT + ENDIF + + ; Set AxisCorr parameters to allow full range of motion + ROS_SetAxisCorrLimits(cont_id) + + ; Start RSI execution + ret = RSI_ON(#ABSOLUTE, #IPO) + IF (ret <> RSIOK) THEN + HALT + ENDIF + + ; Sensor guided movement + RSI_MOVECORR() + + ; Turn off RSI + ret = RSI_OFF() + IF (ret <> RSIOK) THEN + HALT + ENDIF + + PTP $AXIS_ACT_MEAS + + ret = RSI_DELETE(cont_id) + IF (ret <> RSIOK) THEN + HALT + ENDIF +END diff --git a/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_joint_pos_4ms.src b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_joint_pos_4ms.src new file mode 100644 index 0000000..44f56e0 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/KRC/R1/Program/RSI/rsi_joint_pos_4ms.src @@ -0,0 +1,73 @@ +&ACCESS RVP +&PARAM DISKPATH = KRC:\R1\Program\b_ctrldbox +&PARAM EDITMASK = * +&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe +DEF rsi_joint_pos_4ms() + ; ============================================= + ; Copyright 2025 KUKA Hungaria Kft. + ; + ; Licensed under the Apache License, Version 2.0 (the "License"); + ; you may not use this file except in compliance with the License. + ; You may obtain a copy of the License at + ; + ; http://www.apache.org/licenses/LICENSE-2.0 + ; + ; Unless required by applicable law or agreed to in writing, software + ; distributed under the License is distributed on an "AS IS" BASIS, + ; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + ; See the License for the specific language governing permissions and + ; limitations under the License. + ; ============================================= + + ; Declaration of KRL variables + DECL INT ret ; Return value for RSI commands + DECL INT cont_id ; ContainerID + + ;FOLD INI + ;FOLD BASISTECH INI + GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM () + INTERRUPT ON 3 + BAS (#INITMOV, 0) + ;ENDFOLD (BASISTECH INI) + ;FOLD USER INI + ;Make your modifications here + + ;ENDFOLD (USER INI) + ;ENDFOLD (INI) + + WAIT SEC 1.0 + + ; Move to start position + PTP $AXIS_ACT_MEAS + + ; Create RSI Context + ret = RSI_CREATE("b_ctrldbox_rsi", cont_id, TRUE) + IF (ret <> RSIOK) THEN + HALT + ENDIF + + ; Set AxisCorr parameters to allow full range of motion + ROS_SetAxisCorrLimits(cont_id) + + ; Start RSI execution + ret = RSI_ON(#ABSOLUTE, #IPO_FAST) + IF (ret <> RSIOK) THEN + HALT + ENDIF + + ; Sensor guided movement + RSI_MOVECORR() + + ; Turn off RSI + ret = RSI_OFF() + IF (ret <> RSIOK) THEN + HALT + ENDIF + + PTP $AXIS_ACT_MEAS + + ret = RSI_DELETE(cont_id) + IF (ret <> RSIOK) THEN + HALT + ENDIF +END diff --git a/workspaces/kuka/kss_under_8.5/backup.bat b/workspaces/kuka/kss_under_8.5/backup.bat new file mode 100644 index 0000000..b81fc15 --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/backup.bat @@ -0,0 +1,73 @@ +@echo off +setlocal enabledelayedexpansion + +REM ============================================ +REM KUKA KSS < v8.6 b_ctrldbox Backup Script +REM Copies the CURRENT on-controller files that +REM deploy.bat would overwrite into a timestamped +REM backup folder, for manual comparison later. +REM ============================================ + +echo. +echo ============================================ +echo KUKA KSS below v8.6 b_ctrldbox Backup +echo ============================================ +echo. + +REM ============================================ +REM PATH CONFIGURATION - must match deploy.bat +REM ============================================ +set "DST_RSI_CONFIG=C:\KRC\ROBOTER\Config\User\Common\SensorInterface" +set "DST_EKI_CONFIG=C:\KRC\ROBOTER\Config\User\Common\EthernetKRL" +set "DST_PROGRAM=C:\KRC\ROBOTER\KRC\R1\Program\b_ctrldbox" + +REM ============================================ +REM TIMESTAMP +REM ============================================ +for /f "skip=1" %%x in ('wmic os get localdatetime') do if not defined MyDate set "MyDate=%%x" +set "TIMESTAMP=%MyDate:~0,8%_%MyDate:~8,6%" + +set "BACKUP_ROOT=%~dp0Backup\%TIMESTAMP%" + +echo Backup folder: +echo %BACKUP_ROOT% +echo. + +call :BackupFolder "RSI config files" "%DST_RSI_CONFIG%" "%BACKUP_ROOT%\Config\User\Common\SensorInterface" +call :BackupFolder "EKI config files" "%DST_EKI_CONFIG%" "%BACKUP_ROOT%\Config\User\Common\EthernetKRL" +call :BackupFolder "Program files (RSI+EKI)" "%DST_PROGRAM%" "%BACKUP_ROOT%\KRC\R1\Program\b_ctrldbox" + +echo. +echo ============================================ +echo Backup finished. +echo Backed up files are in: +echo %BACKUP_ROOT% +echo ============================================ +pause +goto :eof + +REM ============================================ +REM FUNCTION: BackupFolder +REM Arguments: %1=Description, %2=Source (current on-controller), %3=Destination (backup) +REM ============================================ +:BackupFolder +set "desc=%~1" +set "src=%~2" +set "dst=%~3" + +echo. +echo ============================================ +echo BACKUP: %desc% +echo FROM: %src% +echo TO: %dst% +echo ============================================ + +if not exist "%src%\*" ( + echo Source folder does not exist or is empty - nothing to back up. + goto :eof +) + +if not exist "%dst%\" mkdir "%dst%" +xcopy /Y /E /I "%src%\*" "%dst%\" +echo Backup completed. +goto :eof diff --git a/workspaces/kuka/kss_under_8.5/deploy.bat b/workspaces/kuka/kss_under_8.5/deploy.bat new file mode 100644 index 0000000..07900ad --- /dev/null +++ b/workspaces/kuka/kss_under_8.5/deploy.bat @@ -0,0 +1,122 @@ +@echo off +setlocal enabledelayedexpansion + +REM ============================================ +REM KUKA KSS < v8.6 Deployment Script +REM (RSI Context deployed as separate .rsi / +REM .rsi.diagram / .rsi.xml files instead of +REM a single .rsix package) +REM ============================================ + +REM ============================================ +REM RSI CONFIGURATION SELECTION +REM ============================================ +echo. +echo ============================================ +echo KUKA KSS below v8.6 b_ctrldbox Deployment +echo ============================================ +echo. +echo Select RSI configuration: +echo 1. Standard (6 robot axes only) +echo 2. External Axis (6 robot axes + external axes support) +echo 3. GPIO (6 robot axes + GPIO support) +echo. +set /p rsi_config="Enter selection (1/2/3): " + +if "!rsi_config!"=="2" ( + set "RSI_VARIANT=ext_axis" + echo. + echo Selected: External Axis configuration +) else if "!rsi_config!"=="3" ( + set "RSI_VARIANT=gpios" + echo. + echo Selected: GPIO configuration +) else ( + set "RSI_VARIANT=" + echo. + echo Selected: Standard configuration +) + +REM ============================================ +REM PATH CONFIGURATION - Update these as needed +REM ============================================ +if "!RSI_VARIANT!"=="" ( + set "SRC_RSI_CONFIG=Config\User\Common\SensorInterface" +) else ( + set "SRC_RSI_CONFIG=Config\User\Common\SensorInterface\!RSI_VARIANT!" +) +set "DST_RSI_CONFIG=C:\KRC\ROBOTER\Config\User\Common\SensorInterface" + +set "SRC_RSI_PROGRAM=KRC\R1\Program\RSI" + +set "SRC_EKI_CONFIG=Config\User\Common\EthernetKRL" +set "DST_EKI_CONFIG=C:\KRC\ROBOTER\Config\User\Common\EthernetKRL" + +set "SRC_EKI_PROGRAM=KRC\R1\Program\EKIserver" + +set "DST_PROGRAM=C:\KRC\ROBOTER\KRC\R1\Program\b_ctrldbox" + +REM ============================================ +REM CHECK FOR EXISTING PROGRAM FILES +REM ============================================ +set "hasFiles=0" +if exist "%DST_PROGRAM%\*" ( + for %%F in ("%DST_PROGRAM%\*") do set "hasFiles=1" +) +if "!hasFiles!"=="1" ( + echo. + echo ============================================ + echo WARNING: Program folder contains existing files: + echo %DST_PROGRAM% + echo. + for %%F in ("%DST_PROGRAM%\*") do echo - %%~nxF + echo ============================================ + set /p delconfirm="Delete existing program files before deployment? (Y/N): " + if /i "!delconfirm!"=="Y" ( + del /Q "%DST_PROGRAM%\*" + echo Existing program files deleted. + ) +) + +REM ============================================ +REM DEPLOYMENT +REM ============================================ +call :CopyFiles "RSI config files" "%SRC_RSI_CONFIG%" "%DST_RSI_CONFIG%" +call :CopyFiles "RSI program files" "%SRC_RSI_PROGRAM%" "%DST_PROGRAM%" +call :CopyFiles "EKI config files" "%SRC_EKI_CONFIG%" "%DST_EKI_CONFIG%" +call :CopyFiles "EKI program files" "%SRC_EKI_PROGRAM%" "%DST_PROGRAM%" + +echo. +echo ============================================ +echo Deployment script finished. +echo ============================================ +pause +goto :eof + +REM ============================================ +REM FUNCTION: CopyFiles +REM Arguments: %1=Description, %2=Source, %3=Destination +REM ============================================ +:CopyFiles +set "desc=%~1" +set "src=%~2" +set "dst=%~3" + +echo. +echo ============================================ +echo COPY: %desc% +echo FROM: %src% +echo TO: %dst% +echo. +echo Files to copy: +for %%F in ("%src%\*") do echo - %%~nxF +echo ============================================ +set /p confirm="Proceed with this copy? (Y/N): " +if /i "!confirm!"=="Y" ( + if not exist "%dst%\" mkdir "%dst%" + xcopy /Y "%src%\*" "%dst%\" + echo Copy completed. +) else ( + echo Skipped. +) +goto :eof