diff --git a/.github/workflows/ci_msbuild.yaml b/.github/workflows/ci_msbuild.yaml
index aeb7e437..2c785217 100644
--- a/.github/workflows/ci_msbuild.yaml
+++ b/.github/workflows/ci_msbuild.yaml
@@ -41,7 +41,7 @@ jobs:
# controllers and for which ROS 2 versions builds fail / succeed)
fail-fast: false
matrix:
- controller: [yrc1000, yrc1000u]
+ controller: [dx200, yrc1000, yrc1000u]
ros2_codename: [humble, galactic, foxy]
steps:
diff --git a/.gitignore b/.gitignore
index fb57938f..50a54284 100644
--- a/.gitignore
+++ b/.gitignore
@@ -351,10 +351,13 @@ healthchecksdb
# M+ libmicroros distribution directories
libmicroros_yrc1000_humble/
libmicroros_yrc1000u_humble/
+libmicroros_dx200_humble/
libmicroros_yrc1000_galactic/
libmicroros_yrc1000u_galactic/
+libmicroros_dx200_galactic/
libmicroros_yrc1000_foxy/
libmicroros_yrc1000u_foxy/
+libmicroros_dx200_foxy/
# M+ build output
*.out
diff --git a/MotoROS2.sln b/MotoROS2.sln
index 4df1e436..85b4d384 100644
--- a/MotoROS2.sln
+++ b/MotoROS2.sln
@@ -7,6 +7,9 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MotoROS2", "src\MotoROS2_Al
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
+ DX200_foxy|x86 = DX200_foxy|x86
+ DX200_galactic|x86 = DX200_galactic|x86
+ DX200_humble|x86 = DX200_humble|x86
YRC1000_foxy|x86 = YRC1000_foxy|x86
YRC1000_galactic|x86 = YRC1000_galactic|x86
YRC1000_humble|x86 = YRC1000_humble|x86
@@ -15,6 +18,12 @@ Global
YRC1000u_humble|x86 = YRC1000u_humble|x86
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
+ {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_foxy|x86.ActiveCfg = DX200_foxy|Win32
+ {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_foxy|x86.Build.0 = DX200_foxy|Win32
+ {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_galactic|x86.ActiveCfg = DX200_galactic|Win32
+ {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_galactic|x86.Build.0 = DX200_galactic|Win32
+ {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_humble|x86.ActiveCfg = DX200_humble|Win32
+ {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_humble|x86.Build.0 = DX200_humble|Win32
{64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_foxy|x86.ActiveCfg = YRC1000_foxy|Win32
{64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_foxy|x86.Build.0 = YRC1000_foxy|Win32
{64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_galactic|x86.ActiveCfg = YRC1000_galactic|Win32
diff --git a/README.md b/README.md
index 0ccf24c9..624557e0 100644
--- a/README.md
+++ b/README.md
@@ -9,7 +9,7 @@ SPDX-License-Identifier: CC-BY-SA-4.0
-
+
@@ -31,8 +31,13 @@ The following sections document how to download, install, configure, use and tro
- download the [latest release](#download) and extract onto USB stick
- edit the [configuration file](#configuration), at least Agent IP and port
- insert USB stick into pendant
-- *Maintenance* mode: load the appropriate `mr2_yrc1_*.out` or `mr2_yrc1m_*.out` MotoPlus application [onto controller](#installation)
-- *Online* mode: load `motoros2_config.yaml` via `USER DEFINED FILES`
+- *Maintenance* mode: load the MotoPlus application [onto the controller](#installation):
+ - DX200: `mr2_dx2_*.out`
+ - YRC1000: `mr2_yrc1_*.out`
+ - YRC1000micro: `mr2_yrc1m_*.out`
+- *Online* mode:
+ - DX200: insert USB stick with `motoros2_config.yaml` into `CN106` in the controller cabinet
+ - YC1000[micro]: load `motoros2_config.yaml` via `USER DEFINED FILES`
- start an instance of the [micro-ROS Agent](#the-micro-ros-agent) on a PC
- reboot controller
- verify [MotoROS2 is running](#verifying-successful-installation)
@@ -53,7 +58,7 @@ The following sections document how to download, install, configure, use and tro
- [Verifying YAML correctness](#verifying-yaml-correctness)
- [Example INFORM jobs](#example-inform-jobs)
- [Installation](#installation)
- - [YRC1000 and YRC1000micro](#yrc1000-and-yrc1000micro)
+ - [DX200, YRC1000 and YRC1000micro](#dx200-yrc1000-and-yrc1000micro)
- [Building from source](#building-from-source)
- [Updating the configuration](#updating-the-configuration)
- [The micro-ROS Agent](#the-micro-ros-agent)
@@ -86,11 +91,15 @@ The following sections document how to download, install, configure, use and tro
The following general requirements must be met in order to be able to use MotoROS2 with a Yaskawa Motoman robot controller:
-- controller series: YRC1000 or YRC1000micro
+- controller series: DX200, YRC1000, or YRC1000micro
- minimum versions of system software:
+ - `DN2.44.00-00` for DX200
- `YAS2.80.00-00` for YRC1000
- `YBS2.31.00-00` for YRC1000micro
-- the controller must have a correctly configured network connection (either `LAN2` or `LAN3`)
+- the controller must have a correctly configured network connection:
+ - DX200: `LAN`
+ - YRC1000: either `LAN2` or `LAN3`
+ - YRC1000micro: either `LAN2` or `LAN3`
- MotoPlus and Motoman-Driver must be enabled on the controller
- ROS 2 version: Foxy, Galactic, Humble or Rolling.
ROS 2 Iron Irwini is not yet supported.
@@ -151,6 +160,9 @@ The values must match *exactly*.
|**Controller** |**ROS 2 Version** | **File** |**Version** | **MD5 hash** |
|---------------|------------------|-------------------|------------|------------------------------------|
+| DX200 | Foxy | `mr2_dx2_f.out` | `TODO` | `TODO` |
+| DX200 | Galactic | `mr2_dx2_g.out` | `TODO` | `TODO` |
+| DX200 | Humble | `mr2_dx2_h.out` | `TODO` | `TODO` |
| YRC1000 | Foxy | `mr2_yrc1_f.out` | `v0.1.0` | `eb3c028d0989b6cce2eb4d50a9f45001` |
| YRC1000 | Galactic | `mr2_yrc1_g.out` | `v0.1.0` | `78abcead2e2504109a49287648a9bc04` |
| YRC1000 | Humble | `mr2_yrc1_h.out` | `v0.1.0` | `c5d0f2cce281ed1cb8194badaaffc511` |
@@ -213,13 +225,13 @@ All MotoROS2 release `.zip`s contain a copy of the default INFORM jobs (`.jbi` a
The following variants are shipped with MotoROS2 in the `robot_jobs` sub directory:
-| **Directory** | **Description** | **Supported controller(s)** |
-|:----------------------------|:---------------------|:----------------------------|
-| `single_arm` | Single robot group | YRC1000, YRC1000micro |
-| `single_arm_with_ext_axis` | Robot + station axis | YRC1000, YRC1000micro |
-| `single_arm_with_base_axis` | Robot + base axis | YRC1000, YRC1000micro |
-| `two_arms` | Two robot groups | YRC1000, YRC1000micro |
-| `sda_dual_arm` | SDA robots *only* | - |
+| **Directory** | **Description** | **Supported controller(s)** |
+|:----------------------------|:---------------------|:-----------------------------|
+| `single_arm` | Single robot group | DX200, YRC1000, YRC1000micro |
+| `single_arm_with_ext_axis` | Robot + station axis | DX200, YRC1000, YRC1000micro |
+| `single_arm_with_base_axis` | Robot + base axis | DX200, YRC1000, YRC1000micro |
+| `two_arms` | Two robot groups | DX200, YRC1000, YRC1000micro |
+| `sda_dual_arm` | SDA robots *only* | - |
**These jobs are not required.**
The INFORM job will be automatically generated at startup.
@@ -229,10 +241,10 @@ If needed, open a new issue on the [Issue tracker](https://github.com/yaskawa-gl
## Installation
-Place the `.out` (main binary), `.yaml` (configuration), and `.dat` (I/O names) files on an external storage device: both Secure Digital (SD) and USB sticks can be used.
+Place the `.out` (main binary), `.yaml` (configuration), and `.dat` (I/O names) files on an external storage device: Compact Flash (CF), Secure Digital (SD), and USB sticks can be used depending on the controller model.
Insert the storage device into the robot's programming pendant and refer to the following section.
-### YRC1000 and YRC1000micro
+### DX200, YRC1000, and YRC1000micro
Turn on the robot controller while holding the `{Main Menu}` key on the keypad to enter *Maintenance* mode.
You may release the key when you see the Yaskawa logo appear on the screen.
@@ -240,11 +252,11 @@ You may release the key when you see the Yaskawa logo appear on the screen.
In *Maintenance* mode:
1. upgrade to *MANAGEMENT* security level by touching `[System Info]`→`[Security]` (default password is all `9`'s)
- 1. touch `[MotoPlus APL]`→`[Device]` to select either SD or USB memory type
- 1. touch `[MotoPlus APL]`→`[Load (User App)]` to select and load the `mr2_yrc1_*.out` (or `mr2_yrc1m_*.out`) file
+ 1. touch `[MotoPlus APL]`→`[Device]` to select CF, SD, or USB memory type
+ 1. touch `[MotoPlus APL]`→`[Load (User App)]` to select and load the `mr2_*_*.out` file
1. touch `[MotoPlus APL]`→`[File List]` and verify that MotoROS2 was properly installed and no other MotoPlus applications are currently loaded on the controller
- 1. touch `[File]`→`[Initialize]` and select `USER DEFINED FILES`
- 1. select `SRAM RAM DRIVE` and initialize it
+ 1. (YRC1000[micro] only): touch `[File]`→`[Initialize]` and select `USER DEFINED FILES`
+ 1. (YRC1000[micro] only): select `SRAM RAM DRIVE` and initialize it
1. rotate the pendant key-switch (upper left of pendant) fully counter-clockwise into `TEACH` mode
1. reboot the robot controller into regular mode
@@ -254,13 +266,13 @@ In *Normal Operation* mode:
touch `[RESET]` to clear the alarm
1. upgrade to *MANAGEMENT* security level by touching `[System Info]`→`[Security]` (default password is all `9`'s)
1. touch `[PARAMETER]`→`[S2C]` and set the following parameters:
+ 1. `S2C541 = 0`
+ 1. `S2C542 = 0`
1. `S2C1102 = 2`
1. `S2C1104 = 2`
+ 1. `S2C1117 = 1` (DX200 only)
1. `S2C1250 = 1`
1. `S2C1402 = 3`
- 1. touch `[EX MEMORY]`→`[Load]`
- 1. cursor to `USER DEFINED FILES` and press `[SELECT]`
- 1. cursor to `motoros2_config.yaml` and press `[SELECT]` then `[ENTER]`
If a custom INFORM job will be used:
@@ -268,7 +280,27 @@ In *Normal Operation* mode:
1. cursor to `JOB` and press `[SELECT]`
1. cursor to your job file and press `[SELECT]` then `[ENTER]`
-Within 30 seconds of loading the configuration file, you should get alarm `8001[10] Speed FB enabled, reboot now`. Reboot again and there should be no alarms.
+#### YRC1000 and YRC1000 micro
+
+ 1. touch `[EX MEMORY]`→`[Load]`
+ 1. cursor to `USER DEFINED FILES` and press `[SELECT]`
+ 1. cursor to `motoros2_config.yaml` and press `[SELECT]` then `[ENTER]`
+
+#### DX200
+
+ 1. power down the DX200 controller
+ 1. copy the `motoros2_config.yaml` file to a USB storage drive
+ 1. insert the USB drive into the `CN106` USB port inside the controller cabinet
+ 1. leave the drive in place and close the controller cabinet
+ 1. power up the DX200 controller
+
+Note: on DX200, the USB stick used to store `motoros2_config.yaml` can't be removed, it *must* remain inserted into the USB port labelled `CN106`, or at least as long as MotoROS2 is installed on the controller.
+Without the USB stick in `CN106`, MotoROS2 would not be able to load its configuration and alarms will be raised on each controller (re)boot.
+
+### All supported controllers
+
+Within 30 seconds of loading the configuration file, you should get alarm `8001[10] Speed FB enabled, reboot now`.
+Reboot again and there should be no alarms.
If you receive any errors or alarms after rebooting, please refer to the [Troubleshooting](#troubleshooting) section for information on how to remedy the issue.
@@ -280,7 +312,9 @@ Please refer to [doc/Building from source](doc/build_from_source.md).
It may be necessary to update MotoROS2 configuration during or after initial deployment.
-### Controller software YAS4.70 or YBS3.02 or later
+### YRC1000 and YRC1000micro
+
+#### Controller software YAS4.70 or YBS3.02 or later
In *Normal Operation* mode:
@@ -293,7 +327,7 @@ In *Normal Operation* mode:
1. touch `[YES]` to overwrite
1. reboot the robot controller
-### Older controller software
+#### Older controller software
Due to the way the controller treats files, `motoros2_config.yaml` cannot be directly overwritten using the `[EX MEMORY]` menu.
Instead, MotoROS2 has a built-in mechanism which updates the controller's copy of the `.yaml` file with a new version placed on a USB stick.
@@ -329,6 +363,15 @@ To extract a copy of your current configuration from the teach pendant:
1. cursor to `USER DEFINED FILES` and press `[SELECT]`
1. cursor to `motoros2_config.yaml` and press `[SELECT]` then `[ENTER]`
+### DX200 only
+
+ 1. power down the robot controller and open the cabinet
+ 1. locate the USB port labelled `CN106` on the robot's CPU board and remove the USB stick
+ 1. overwrite the `motoros2_config.yaml` file in the root directory of a USB stick with an updated version
+ 1. replace the USB stick into `CN106` and close the controller cabinet
+ 1. power on the controller and wait for it to fully boot (ie: you see the regular UI on the teach pendant)
+ 1. verify MotoROS2 has started
+
## The micro-ROS Agent
The micro-ROS Agent acts as the transparent bridge between MotoROS2 and ROS 2.
@@ -739,7 +782,7 @@ As such, no statements are made about priorities or development schedule for any
The following items are on the MotoROS2 roadmap, and are listed here in no particular order:
-- support DX200/FS100 controllers
+- support FS100 controllers
- read/write of controller variables
- CRUD of INFORM job files (ie: create, retrieve, update, delete)
- starting/stopping INFORM jobs (other than `INIT_ROS`)
diff --git a/doc/faq.md b/doc/faq.md
index 2f76018a..08e4cfb3 100644
--- a/doc/faq.md
+++ b/doc/faq.md
@@ -15,44 +15,44 @@ MotoROS2 *development* (ie: building MotoROS2 from source) does require a MotoPl
Development of a ROS 2 application that interfaces with MotoROS2 does **not** require a MotoPlus SDK license.
-## Can MotoROS2 be installed on DX100, FS100 or DX200 controllers?
+## Can MotoROS2 be installed on DX100 or FS100 controllers?
-MotoROS2 only supports YRC1000 and YRC1000micro controllers.
+MotoROS2 only supports DX200, YRC1000 and YRC1000micro controllers.
## Does MotoROS2 support all Yaskawa Motoman robots?
-MotoROS2 is manipulator agnostic, and as such is expected to be compatible with all Yaskawa Motoman robots which can be used with the controller series supported by MotoROS2 (ie: YRC1000 and YRC1000micro).
+MotoROS2 is manipulator agnostic, and as such is expected to be compatible with all Yaskawa Motoman robots which can be used with the controller series supported by MotoROS2 (ie: DX200, YRC1000 and YRC1000micro).
## Can MotoROS2 be used with SDA robots?
-The current version of MotoROS2 is only compatible with YRC1000 and YRC1000micro controllers.
-This controller is not used with SDA robots (ie: dual-arm robots), and as such, MotoROS2 does not support SDA robots.
+The current version of MotoROS2 is only compatible with DX200, YRC1000 and YRC1000micro controllers.
+These controllers are not used with SDA robots (ie: dual-arm robots), and as such, MotoROS2 does not support SDA robots.
Note: the limitation is not MotoROS2, but the controller.
-Users of SDA robots paired with a YRC1000 (or newer) wishing to use MotoROS2 are encouraged to contact the developers.
+Users of SDA robots paired with a DX200 or YRC1000 (or newer) wishing to use MotoROS2 are encouraged to contact the developers.
## Can MotoROS2 be used with Scara robots?
-The current version of MotoROS2 is compatible with YRC1000 and YRC1000micro controllers.
-Provided the robot is used with a YRC1000 generation controller, it should be supported by MotoROS2.
+The current version of MotoROS2 is compatible with DX200, YRC1000 and YRC1000micro controllers.
+Provided the robot is used with a DX200 or YRC1000 generation controller, it should be supported by MotoROS2.
## Can MotoROS2 be used with Delta robots?
-The current version of MotoROS2 is only compatible with YRC1000 and YRC1000micro controllers.
-This controller is not used with Delta robots, and as such, MotoROS2 does not support Delta robots.
+The current version of MotoROS2 is only compatible with DX200, YRC1000 and YRC1000micro controllers.
+These controllers are not used with Delta robots, and as such, MotoROS2 does not support Delta robots.
Note: the limitation is not MotoROS2, but the controller.
-Users of Delta robots paired with a YRC1000 (or newer) wishing to use MotoROS2 are encouraged to contact the developers.
+Users of Delta robots paired with a DX200 or YRC1000 (or newer) wishing to use MotoROS2 are encouraged to contact the developers.
## Can MotoROS2 be used with palletizing robots?
-The current version of MotoROS2 is compatible with YRC1000 and YRC1000micro controllers.
-Provided the palletizing robot is used with a YRC1000 generation controller, it should be supported by MotoROS2.
+The current version of MotoROS2 is compatible with DX200, YRC1000 and YRC1000micro controllers.
+Provided the palletizing robot is used with a DX200 or YRC1000 generation controller, it should be supported by MotoROS2.
## Can MotoROS2 be used with welding robots?
-The current version of MotoROS2 is compatible with YRC1000 and YRC1000micro controllers.
-Provided the robot is used with a YRC1000 generation controller, the *motion* of the arm can be controlled by MotoROS2.
+The current version of MotoROS2 is compatible with DX200, YRC1000 and YRC1000micro controllers.
+Provided the robot is used with a DX200 or YRC1000 generation controller, the *motion* of the arm can be controlled by MotoROS2.
Note: MotoROS2 does not have any process-controls.
It cannot be used to control the welding power source.
diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md
index 2dd0612c..467a2fcb 100644
--- a/doc/troubleshooting.md
+++ b/doc/troubleshooting.md
@@ -194,6 +194,8 @@ ALARM 8003
*Solution:*
Your robot controller requires internal configuration changes to support the MotoROS2 driver.
+For DX200: ensure the controller is updated to at least `DN2.44.00-00`.
+
For YRC1000 and YRC1000micro: ensure the controller is updated to at least `YAS2.80.00-00` (for YRC1000) and `YBS2.31.00-00` (for YRC1000micro).
If the system software version is below this, please contact Yaskawa Motoman for assistance with upgrading the controller.
@@ -612,7 +614,8 @@ ALARM 8013
*Solution:*
Follow the [setup instructions](../README.md#installation) to load the `motoros2_config.yaml` configuration file.
-Be sure to follow the steps for initializing SRAM and setting the `S2C` parameters.
+Double check the setting of the `S2C` parameters.
+Additionally, on YRC-generation controllers, be sure to follow the steps for initializing SRAM.
### Alarm: 8013[1]
@@ -910,3 +913,36 @@ ALARM 8014
*Solution:*
There was a failure when generating the default INFORM job.
Please obtain the standard job from the Github repository and load it using the teach pendant.
+
+### Alarm: 8015[0]
+
+*Example:*
+
+```text
+ALARM 8015
+ Failed to parse RBCALIB.DAT
+[0]
+```
+
+*Solution:*
+Check the following robot parameters on the teach pendant and make sure they are set to the values shown here:
+
+- S2C1103 = 2
+- S2C1117 = 1
+
+If that does not resolve the issue, please contact Yaskawa technical support for assistance.
+Include a copy of the `ALL.PRM` and `CMOS.BIN` from your robot controller.
+
+### Alarm: 8015[1] - [4]
+
+*Example:*
+
+```text
+ALARM 8015
+ Failed to parse RBCALIB.DAT
+[1]
+```
+
+*Solution:*
+Open a new ticket on the MotoROS2 [Issue tracker](https://github.com/yaskawa-global/motoros2/issues).
+Please include a copy of the `RBCALIB.DAT` from your robot controller along with the output from the [Debug log client](#debug-log-client).
diff --git a/src/ConfigFile.c b/src/ConfigFile.c
index 4fa2d655..25aa66c7 100644
--- a/src/ConfigFile.c
+++ b/src/ConfigFile.c
@@ -321,7 +321,7 @@ void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event)
break;
case Value_UserLanPort:
-#if defined (DX200) || defined (FS100)
+#if defined (FS100) || defined (DX200)
// single port, override whatever was configured
*(Ros_UserLan_Port_Setting*)activeItem->valueToSet = CFG_ROS_USER_LAN1;
Ros_Debug_BroadcastMsg("DX200 or FS100: override to 'USER_LAN1'");
@@ -704,7 +704,11 @@ void Ros_ConfigFile_Parse()
Ros_ConfigFile_SetAllDefaultValues();
+#if defined (YRC1000) || defined (YRC1000u)
+ //config file always resides on USB for DX200/FS100, so only check
+ //on YRC1000 and micro
Ros_ConfigFile_CheckUsbForNewConfigFile();
+#endif
do
{
@@ -717,15 +721,34 @@ void Ros_ConfigFile_Parse()
//Parse file
yaml_parser_t parser;
yaml_event_t event;
- char sramFilePath[128];
+ const int CHAR_BUFFER_SIZE = 128;
+ char configFilePath[CHAR_BUFFER_SIZE];
+ char storageDrive[CHAR_BUFFER_SIZE];
int fd;
struct stat fileStat;
- sprintf(sramFilePath, "%s\\%s", MP_SRAM_DEV_DOS, CONFIG_FILE_NAME);
+#if defined (FS100) || defined (DX200)
+ snprintf(storageDrive, CHAR_BUFFER_SIZE, "%s", MP_USB0_DEV_DOS);
+#elif defined (YRC1000) || defined (YRC1000u)
+ snprintf(storageDrive, CHAR_BUFFER_SIZE, "%s", MP_SRAM_DEV_DOS);
+#else
+#error Ros_ConfigFile_Parse: unsupported platform
+#endif
+
+ snprintf(configFilePath, CHAR_BUFFER_SIZE, "%s\\%s", storageDrive, CONFIG_FILE_NAME);
Ros_Debug_BroadcastMsg("Checking configuration file: %s", CONFIG_FILE_NAME);
- fd = mpOpen(sramFilePath, O_RDONLY, 0);
+ fd = mpOpen(configFilePath, O_RDONLY, 0);
+
+#if defined (DX200)
+ if (fd < 0)
+ {
+ //try again using second USB port
+ snprintf(configFilePath, CHAR_BUFFER_SIZE, "%s\\%s", MP_USB1_DEV_DOS, CONFIG_FILE_NAME);
+ fd = mpOpen(configFilePath, O_RDONLY, 0);
+ }
+#endif
if (fd >= 0)
{
diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c
index 0c550f29..3d905a79 100644
--- a/src/ControllerStatusIO.c
+++ b/src/ControllerStatusIO.c
@@ -117,7 +117,7 @@ BOOL Ros_Controller_Initialize()
for (i = 0; i < MAX_ROBOT_CALIBRATION_FILES; i += 1)
{
MP_RB_CALIB_DATA calibData;
- if (mpGetRobotCalibrationData(i, &calibData) == OK)
+ if (Ros_mpGetRobotCalibrationData(i, &calibData) == OK)
{
if (calibData.s_rb.grp_no <= MP_R8_GID && //the slave is a robot
calibData.m_rb.grp_no <= MP_R8_GID) //the master is another robot's RF
diff --git a/src/CtrlGroup.c b/src/CtrlGroup.c
index 061091f6..78ace713 100644
--- a/src/CtrlGroup.c
+++ b/src/CtrlGroup.c
@@ -83,7 +83,7 @@ CtrlGroup* Ros_CtrlGroup_Create(int groupIndex, BOOL bIsLastGrpToInit, float int
// Populate values
ctrlGroup->groupNo = groupIndex;
ctrlGroup->numAxes = numAxes;
- ctrlGroup->groupId = mpCtrlGrpNo2GrpId(groupIndex);
+ ctrlGroup->groupId = Ros_mpCtrlGrpNo2GrpId(groupIndex);
if (Ros_CtrlGroup_IsRobot(ctrlGroup))
{
@@ -241,6 +241,34 @@ void Ros_CtrlGrp_Cleanup(CtrlGroup* ctrlGroup)
mpSemDelete(ctrlGroup->inc_q.q_lock);
}
+
+//-------------------------------------------------------------------
+// Search through the control group to find the GroupId that matches
+// the group number
+//-------------------------------------------------------------------
+MP_GRP_ID_TYPE Ros_mpCtrlGrpNo2GrpId(int groupNo)
+{
+#if defined (YRC1000) || defined (YRC1000u)
+ return mpCtrlGrpNo2GrpId(groupNo);
+
+#elif defined (FS100) || defined (DX200)
+ MP_GRP_ID_TYPE grp_id;
+
+ for(grp_id = MP_R1_GID; grp_id < MP_S24_GID; ++grp_id)
+ {
+ if(groupNo == mpCtrlGrpId2GrpNo(grp_id))
+ return grp_id;
+ }
+
+ return -1;
+
+#else
+#error "Ros_mpCtrlGrpNo2GrpId: unsupported platform"
+
+#endif
+}
+
+
//-------------------------------------------------------------------
// Get the commanded pulse position in pulse (in motoman joint order)
// Used for MOTION SERVER connection for positional planning calculations.
diff --git a/src/CtrlGroup.h b/src/CtrlGroup.h
index ef5827b4..556edee3 100644
--- a/src/CtrlGroup.h
+++ b/src/CtrlGroup.h
@@ -111,6 +111,8 @@ typedef struct
extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod);
extern void Ros_CtrlGrp_Cleanup(CtrlGroup* ctrlGroup);
+extern MP_GRP_ID_TYPE Ros_mpCtrlGrpNo2GrpId(int groupNo);
+
extern BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
extern BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
extern BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES]);
diff --git a/src/DX200_foxyCompilerArguments.mps b/src/DX200_foxyCompilerArguments.mps
new file mode 100644
index 00000000..a1d36a01
--- /dev/null
+++ b/src/DX200_foxyCompilerArguments.mps
@@ -0,0 +1 @@
+-DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~"
diff --git a/src/DX200_foxyLinkerArguments.mps b/src/DX200_foxyLinkerArguments.mps
new file mode 100644
index 00000000..f9469ff0
--- /dev/null
+++ b/src/DX200_foxyLinkerArguments.mps
@@ -0,0 +1 @@
+-std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~"
diff --git a/src/DX200_galacticCompilerArguments.mps b/src/DX200_galacticCompilerArguments.mps
new file mode 100644
index 00000000..a1d36a01
--- /dev/null
+++ b/src/DX200_galacticCompilerArguments.mps
@@ -0,0 +1 @@
+-DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~"
diff --git a/src/DX200_galacticLinkerArguments.mps b/src/DX200_galacticLinkerArguments.mps
new file mode 100644
index 00000000..f9469ff0
--- /dev/null
+++ b/src/DX200_galacticLinkerArguments.mps
@@ -0,0 +1 @@
+-std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~"
diff --git a/src/DX200_humbleCompilerArguments.mps b/src/DX200_humbleCompilerArguments.mps
new file mode 100644
index 00000000..a1d36a01
--- /dev/null
+++ b/src/DX200_humbleCompilerArguments.mps
@@ -0,0 +1 @@
+-DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~"
diff --git a/src/DX200_humbleLinkerArguments.mps b/src/DX200_humbleLinkerArguments.mps
new file mode 100644
index 00000000..f9469ff0
--- /dev/null
+++ b/src/DX200_humbleLinkerArguments.mps
@@ -0,0 +1 @@
+-std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~"
diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h
index 364738a7..f88731b6 100644
--- a/src/ErrorHandling.h
+++ b/src/ErrorHandling.h
@@ -78,6 +78,7 @@ typedef enum
ALARM_ALLOCATION_FAIL,
ALARM_CONFIGURATION_FAIL,
ALARM_INFORM_JOB_FAIL,
+ ALARM_DAT_FILE_PARSE_FAIL,
} ALARM_MAIN_CODE;
@@ -202,6 +203,15 @@ typedef enum
SUBCODE_INFORM_FAIL_TO_LOAD_JOB,
} INFORM_SUBCODE;
+typedef enum
+{
+ SUBCODE_DAT_FAIL_OPEN,
+ SUBCODE_DAT_FAIL_PARSE_RBCALIB,
+ SUBCODE_DAT_FAIL_PARSE_MGROUP,
+ SUBCODE_DAT_FAIL_PARSE_SGROUP,
+ SUBCODE_DAT_FAIL_PARSE_SRANG
+} DAT_SUBCODE;
+
extern void motoRosAssert(BOOL mustBeTrue, ASSERTION_SUBCODE subCodeIfFalse);
extern void motoRosAssert_withMsg(BOOL mustBeTrue, ASSERTION_SUBCODE subCodeIfFalse, char* msgFmtIfFalse, ...);
diff --git a/src/MotoPlusExterns.h b/src/MotoPlusExterns.h
index 0cd5f50d..ac20bba8 100644
--- a/src/MotoPlusExterns.h
+++ b/src/MotoPlusExterns.h
@@ -13,7 +13,9 @@
extern size_t mpNumBytesFree(void);
+#if defined (YRC1000) || defined (YRC1000u)
extern MP_GRP_ID_TYPE mpCtrlGrpNo2GrpId(int grp_no);
+#endif
//M+ only defines MAX_ALARM_COUNT, but we'd like to avoid magic nrs
//as much as possible, so we define our own here for errors.
diff --git a/src/MotoROS.h b/src/MotoROS.h
index 3b6cea06..b804153e 100644
--- a/src/MotoROS.h
+++ b/src/MotoROS.h
@@ -102,6 +102,7 @@
#include "InformCheckerAndGenerator.h"
#include "MathConstants.h"
#include "MotoROS_PlatformLib.h"
+#include "Ros_mpGetRobotCalibrationData.h"
extern void Ros_Sleep(float milliseconds);
diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj
index 417ff72e..6a57bcdf 100644
--- a/src/MotoROS2_AllControllers.vcxproj
+++ b/src/MotoROS2_AllControllers.vcxproj
@@ -1,6 +1,18 @@
+
+ DX200_foxy
+ Win32
+
+
+ DX200_galactic
+ Win32
+
+
+ DX200_humble
+ Win32
+
YRC1000u_foxy
Win32
@@ -34,6 +46,21 @@
MotoROS2
+
+ Makefile
+ false
+ v141
+
+
+ Makefile
+ false
+ v141
+
+
+ Makefile
+ false
+ v141
+
Makefile
false
@@ -67,6 +94,15 @@
+
+
+
+
+
+
+
+
+
@@ -86,6 +122,42 @@
+
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_h" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"
+ $(MP_VS_Install)DevTools\OnlineDownload.exe
+ $(NMakePreprocessorDefinitions);DX200;MOTOROS2_MEM_TRACE_ENABLE
+ $(MP_VS_Install)DX200\gnu\4.3.3-vxworks-6.9\lib\gcc\i586-wrs-vxworks\4.3.3\include;$(MP_VS_Install)DX200\inc;$(SolutionDir)libmicroros_dx200_humble\include;
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_h" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "$(ProjectName)" -b "$(OutDir)\" -o clean
+
+ $(SolutionDir)bin\$(Configuration)\
+
+ $(ProjectDir)..\libmicroros_dx200_humble\include;
+
+
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_g" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"
+ $(MP_VS_Install)DevTools\OnlineDownload.exe
+ $(NMakePreprocessorDefinitions);DX200;MOTOROS2_MEM_TRACE_ENABLE
+ $(MP_VS_Install)DX200\gnu\4.3.3-vxworks-6.9\lib\gcc\i586-wrs-vxworks\4.3.3\include;$(MP_VS_Install)DX200\inc;$(SolutionDir)libmicroros_dx200_galactic\include;
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_g" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "$(ProjectName)" -b "$(OutDir)\" -o clean
+
+ $(SolutionDir)bin\$(Configuration)\
+
+ $(ProjectDir)..\libmicroros_dx200_galactic\include;
+
+
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_f" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"
+ $(MP_VS_Install)DevTools\OnlineDownload.exe
+ $(NMakePreprocessorDefinitions);DX200;MOTOROS2_MEM_TRACE_ENABLE
+ $(MP_VS_Install)DX200\gnu\4.3.3-vxworks-6.9\lib\gcc\i586-wrs-vxworks\4.3.3\include;$(MP_VS_Install)DX200\inc;$(SolutionDir)libmicroros_dx200_foxy\include;
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_f" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"
+ "$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "$(ProjectName)" -b "$(OutDir)\" -o clean
+
+ $(SolutionDir)bin\$(Configuration)\
+
+ $(ProjectDir)..\libmicroros_dx200_foxy\include;
+
"$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_yrc1_h" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)YRC1000\inc"
$(MP_VS_Install)DevTools\OnlineDownload.exe
@@ -158,6 +230,21 @@
$(ProjectDir)..\libmicroros_yrc1000u_foxy\include;
+
+
+ _buildLog\MotoROS2$(Configuration).log
+
+
+
+
+ _buildLog\MotoROS2$(Configuration).log
+
+
+
+
+ _buildLog\MotoROS2$(Configuration).log
+
+
_buildLog\MotoROS2$(Configuration).log
@@ -193,6 +280,9 @@
+
+
+
@@ -200,8 +290,14 @@
+
+
+
+
+
+
@@ -216,6 +312,8 @@
+
+
@@ -253,6 +351,7 @@
+
@@ -286,8 +385,9 @@
+
-
\ No newline at end of file
+
diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters
index 70e9e43a..bb612e56 100644
--- a/src/MotoROS2_AllControllers.vcxproj.filters
+++ b/src/MotoROS2_AllControllers.vcxproj.filters
@@ -97,18 +97,30 @@
+
+ Compile Settings
+
+
+ Compile Settings
+
Compile Settings
Compile Settings
+
+ MotoPlus Libraries
+
MotoPlus Libraries
MotoPlus Libraries
+
+ MotoPlus Libraries
+
MotoPlus Libraries
@@ -178,6 +190,9 @@
Docs\doc
+
+ MotoPlus Libraries\micro-ROS
+
MotoPlus Libraries\micro-ROS
@@ -214,6 +229,24 @@
Compile Settings
+
+ MotoPlus Libraries\micro-ROS
+
+
+ Compile Settings
+
+
+ Compile Settings
+
+
+ MotoPlus Libraries\micro-ROS
+
+
+ Compile Settings
+
+
+ Compile Settings
+
@@ -285,6 +318,9 @@
Source Files\Services
+
+ Source Files\Robot Controller
+
@@ -380,5 +416,8 @@
Header Files\Services
+
+ Header Files\Robot Controller
+
-
\ No newline at end of file
+
diff --git a/src/MotoROS_PlatformLib.dnLib b/src/MotoROS_PlatformLib.dnLib
new file mode 100644
index 00000000..aa2d0188
Binary files /dev/null and b/src/MotoROS_PlatformLib.dnLib differ
diff --git a/src/ParameterExtraction.dnLib b/src/ParameterExtraction.dnLib
new file mode 100644
index 00000000..b5e1a97f
Binary files /dev/null and b/src/ParameterExtraction.dnLib differ
diff --git a/src/Ros_mpGetRobotCalibrationData.c b/src/Ros_mpGetRobotCalibrationData.c
new file mode 100644
index 00000000..64dace84
--- /dev/null
+++ b/src/Ros_mpGetRobotCalibrationData.c
@@ -0,0 +1,217 @@
+// Ros_mpGetRobotCalibrationData.c
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#include "MotoROS.h"
+
+#define FILENAME_RBCALIB_DAT "RBCALIB.DAT"
+#define PATH_TO_RBCALIB_DAT "MPRAM1:0\\RBCALIB.DAT"
+
+#if defined (FS100) || defined (DX200)
+//The download and parsing of the DAT file is expensive. I only want to
+//perform it once. So let's cache the data in an array.
+//
+//This is an array of POINTERS. We'll only allocate a slot if that calibration
+//file is configured.
+MP_RB_CALIB_DATA* Ros_CalibrationFiles[MAX_ROBOT_CALIBRATION_FILES];
+#endif
+
+void Ros_mpGetRobotCalibrationData_Initialize()
+{
+#if defined (FS100) || defined (DX200)
+ int ret;
+ int fd;
+ BOOL bRet;
+
+ const int SIZEOFBUFFER = 256;
+ char buffer[SIZEOFBUFFER];
+
+ rcutils_string_array_t splitSpace = rcutils_get_zero_initialized_string_array();
+ rcutils_string_array_t splitComma = rcutils_get_zero_initialized_string_array();
+
+ bzero(Ros_CalibrationFiles, sizeof(Ros_CalibrationFiles));
+
+ //make sure to remove any old copy that may be out there
+ mpRemove(PATH_TO_RBCALIB_DAT); //dont care if this fails
+
+ //save from CMOS to DRAM
+ ret = mpSaveFile(MP_DRV_ID_DRAM, "", FILENAME_RBCALIB_DAT); //`path` argument is blank so file is on the root of the drive
+ if (ret != OK)
+ {
+ Ros_Debug_BroadcastMsg("Couldn't save " FILENAME_RBCALIB_DAT " to DRAM drive. Error (%d). (This is normal if the calibration doesn't exist)", ret);
+ Ros_Debug_BroadcastMsg("/tf will be inaccurate for multi-group systems");
+ return;
+ }
+
+ fd = mpOpen(PATH_TO_RBCALIB_DAT, O_RDONLY, 0);
+ if (fd < 0)
+ {
+ Ros_Debug_BroadcastMsg("Couldn't open " FILENAME_RBCALIB_DAT " from DRAM drive. fd (%d)", fd);
+ Ros_Debug_BroadcastMsg("/tf will be inaccurate for multi-group systems");
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_OPEN);
+ return;
+ }
+
+ while (TRUE) //unknown file size. this will `break` when done with file
+ {
+ //---------------------------------------------------------------------
+ bRet = FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //RBCALIB x
+
+ if (bRet && strstr(buffer, "//RBCALIB"))
+ {
+ ret = rcutils_split(buffer, ' ', g_motoros2_Allocator, &splitSpace);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (RBCALIB) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_RBCALIB);
+ break;
+ }
+
+ int fileNo = strtol(splitSpace.data[1], NULL, 10) - 1; //fileNo is zero-based, RBCALIB is one-based
+ Ros_CalibrationFiles[fileNo] = (MP_RB_CALIB_DATA*)mpMalloc(sizeof(MP_RB_CALIB_DATA));
+
+ ret = rcutils_string_array_fini(&splitSpace); RCUTILS_UNUSED(ret);
+
+ //---------------------------------------------------------------------
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //MTOOL x
+
+ //---------------------------------------------------------------------
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //MGROUP x,x,x,....
+ ret = rcutils_split(buffer, ' ', g_motoros2_Allocator, &splitSpace);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (MGROUP) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_MGROUP);
+ break;
+ }
+ ret = rcutils_split(splitSpace.data[1], ',', g_motoros2_Allocator, &splitComma);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (MGROUP) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_MGROUP);
+ break;
+ }
+
+ for (int groupIndex = 0; groupIndex < 32; groupIndex += 1)
+ {
+ if (strtol(splitComma.data[groupIndex], NULL, 10) == 1)
+ {
+ Ros_CalibrationFiles[fileNo]->m_rb.grp_no = groupIndex;
+ break;
+ }
+ }
+ ret = rcutils_string_array_fini(&splitSpace); RCUTILS_UNUSED(ret);
+ ret = rcutils_string_array_fini(&splitComma); RCUTILS_UNUSED(ret);
+
+ //---------------------------------------------------------------------
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //MPULSE
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //MRBC1
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //MRBC2
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //MRBC3
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //STOOL x
+
+ //---------------------------------------------------------------------
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //SGROUP x,x,x,....
+ ret = rcutils_split(buffer, ' ', g_motoros2_Allocator, &splitSpace);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (SGROUP) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_SGROUP);
+ break;
+ }
+ ret = rcutils_split(splitSpace.data[1], ',', g_motoros2_Allocator, &splitComma);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (SGROUP) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_SGROUP);
+ break;
+ }
+
+ for (int groupIndex = 0; groupIndex < 32; groupIndex += 1)
+ {
+ if (strtol(splitComma.data[groupIndex], NULL, 10) == 1)
+ {
+ Ros_CalibrationFiles[fileNo]->s_rb.grp_no = groupIndex;
+ break;
+ }
+ }
+ ret = rcutils_string_array_fini(&splitSpace); RCUTILS_UNUSED(ret);
+ ret = rcutils_string_array_fini(&splitComma); RCUTILS_UNUSED(ret);
+
+ //---------------------------------------------------------------------
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //SPULSE
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //SSTC1
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //SSTC2
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //SSTC3
+
+ //---------------------------------------------------------------------
+ FileUtilityFunctions_ReadLine(fd, buffer, SIZEOFBUFFER); // //SRANG x,y,z,rx,ry,rz
+ ret = rcutils_split(buffer, ' ', g_motoros2_Allocator, &splitSpace);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (SRANG) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_SRANG);
+ break;
+ }
+ ret = rcutils_split(splitSpace.data[1], ',', g_motoros2_Allocator, &splitComma);
+ if (ret != RCUTILS_RET_OK)
+ {
+ Ros_Debug_BroadcastMsg("Failed to parse robot calibration data (SRANG) (%d)", ret);
+ mpSetAlarm(ALARM_DAT_FILE_PARSE_FAIL, "Failed to parse " FILENAME_RBCALIB_DAT, SUBCODE_DAT_FAIL_PARSE_SRANG);
+ break;
+ }
+
+ Ros_CalibrationFiles[fileNo]->pos_uow[0] = (int)(strtod(splitComma.data[0], NULL) * 1000);
+ Ros_CalibrationFiles[fileNo]->pos_uow[1] = (int)(strtod(splitComma.data[1], NULL) * 1000);
+ Ros_CalibrationFiles[fileNo]->pos_uow[2] = (int)(strtod(splitComma.data[2], NULL) * 1000);
+
+ Ros_CalibrationFiles[fileNo]->ang_uow[0] = (int)(strtod(splitComma.data[3], NULL) * 10000);
+ Ros_CalibrationFiles[fileNo]->ang_uow[1] = (int)(strtod(splitComma.data[4], NULL) * 10000);
+ Ros_CalibrationFiles[fileNo]->ang_uow[2] = (int)(strtod(splitComma.data[5], NULL) * 10000);
+
+ ret = rcutils_string_array_fini(&splitSpace); RCUTILS_UNUSED(ret);
+ ret = rcutils_string_array_fini(&splitComma); RCUTILS_UNUSED(ret);
+ }
+ else
+ break;
+ }
+
+ mpClose(fd);
+ mpRemove(PATH_TO_RBCALIB_DAT); //dont care if this fails
+#endif
+}
+
+void Ros_mpGetRobotCalibrationData_Cleanup()
+{
+#if defined (FS100) || defined (DX200)
+ for (int i = 0; i < MAX_ROBOT_CALIBRATION_FILES; i += 1)
+ {
+ if (Ros_CalibrationFiles[i])
+ mpFree(Ros_CalibrationFiles[i]);
+ }
+#endif
+}
+
+LONG Ros_mpGetRobotCalibrationData(ULONG file_no, MP_RB_CALIB_DATA *rData)
+{
+#if defined (YRC1000) || defined (YRC1000u)
+ // on these controllers we can forward to mpGetRobotCalibrationData(..)
+ return mpGetRobotCalibrationData(file_no, rData);
+
+#elif defined (FS100) || defined (DX200)
+ if (Ros_CalibrationFiles[file_no] != NULL) //if this calibration file exists
+ {
+ memcpy(rData, Ros_CalibrationFiles[file_no], sizeof(MP_RB_CALIB_DATA));
+ return OK;
+ }
+ else
+ return NG;
+
+#else
+ #error Ros_mpGetRobotCalibrationData: unsupported platform
+
+#endif
+}
diff --git a/src/Ros_mpGetRobotCalibrationData.h b/src/Ros_mpGetRobotCalibrationData.h
new file mode 100644
index 00000000..4380d4cb
--- /dev/null
+++ b/src/Ros_mpGetRobotCalibrationData.h
@@ -0,0 +1,60 @@
+// Ros_mpGetRobotCalibrationData.h
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef MOTOROS2_ROS_MP_GET_ROBOT_CALIBRATION_DATA_H
+#define MOTOROS2_ROS_MP_GET_ROBOT_CALIBRATION_DATA_H
+
+
+#include "motoPlus.h"
+
+
+#if defined (FS100) || defined (DX200)
+
+// from mpSysCtrl.h
+
+#ifdef MP_RC_XYZ_NUM
+#error "DX200/FS100 M+ SDK conflicts with MP_RC_XYZ_NUM definition"
+#endif
+
+#ifdef MP_RC_CALIB_P_NUM
+#error "DX200/FS100 M+ SDK conflicts with MP_RC_CALIB_P_NUM definition"
+#endif
+
+#ifdef MP_RC_RB_P_NUM
+#error "DX200/FS100 M+ SDK conflicts with MP_RC_RB_P_NUM definition"
+#endif
+
+#define MP_RC_XYZ_NUM (3)
+#define MP_RC_CALIB_P_NUM (5)
+#define MP_RC_RB_P_NUM (2)
+
+typedef struct {
+ CHAR tool_no;
+ UCHAR reserved[3];
+ LONG axis_data[MP_GRP_AXES_NUM];
+} MP_RB_CALIB_POS;
+
+typedef struct {
+ ULONG grp_no;
+ MP_RB_CALIB_POS calib_pos[MP_RC_CALIB_P_NUM][MP_RC_RB_P_NUM];
+} MP_RB_CALIB_INFO;
+
+typedef struct {
+ MP_RB_CALIB_INFO m_rb;
+ MP_RB_CALIB_INFO s_rb;
+ LONG pos_uow[MP_RC_XYZ_NUM];
+ LONG ang_uow[MP_RC_XYZ_NUM];
+} MP_RB_CALIB_DATA;
+
+#endif
+
+extern void Ros_mpGetRobotCalibrationData_Initialize();
+extern void Ros_mpGetRobotCalibrationData_Cleanup();
+extern LONG Ros_mpGetRobotCalibrationData(ULONG file_no, MP_RB_CALIB_DATA *rData);
+
+
+#endif // MOTOROS2_ROS_MP_GET_ROBOT_CALIBRATION_DATA_H
diff --git a/src/controllers.xml b/src/controllers.xml
index caae217a..f5af3298 100644
--- a/src/controllers.xml
+++ b/src/controllers.xml
@@ -11,6 +11,9 @@ SPDX-License-Identifier: Apache-2.0
yrcmLib_foxy;yrcmLib
yrcLib_galactic;yrcLib
yrcmLib_galactic;yrcmLib
+ dnLib_humble;dnLib
+ dnLib_foxy;dnLib
+ dnLib_galactic;dnLib
@@ -20,6 +23,9 @@ SPDX-License-Identifier: Apache-2.0
MOTOPLUS-YRC1000micro
MOTOPLUS-YRC1000
MOTOPLUS-YRC1000micro
+ MOTOPLUS-DX200
+ MOTOPLUS-DX200
+ MOTOPLUS-DX200
@@ -29,5 +35,8 @@ SPDX-License-Identifier: Apache-2.0
YRC1000u\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe
YRC1000\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe
YRC1000u\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe
+ DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe
+ DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe
+ DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe
diff --git a/src/main.c b/src/main.c
index 1819a4ea..789bb2f1 100644
--- a/src/main.c
+++ b/src/main.c
@@ -7,8 +7,8 @@
#include "MotoROS.h"
-#if !(YRC1000 || YRC1000u)
-#error This must be compiled for a YRC generation controller!
+#if !(defined (DX200) || defined (YRC1000) || defined (YRC1000u))
+#error MotoROS2 is only supported on DX2 and YRC1 generation controllers
#endif
void RosInitTask();
@@ -99,6 +99,8 @@ void RosInitTask()
Ros_Allocation_Initialize();
+ Ros_mpGetRobotCalibrationData_Initialize(); //must occur before Ros_Controller_Initialize
+
Ros_Communication_ConnectToAgent();
Ros_Controller_SetIOState(IO_FEEDBACK_AGENTCONNECTED, TRUE);
@@ -203,7 +205,8 @@ void RosInitTask()
Ros_ActionServer_FJT_Cleanup();
Ros_PositionMonitor_Cleanup();
Ros_Controller_Cleanup();
- Ros_Communication_Cleanup();
+ Ros_Communication_Cleanup();
+ Ros_mpGetRobotCalibrationData_Cleanup();
//--------------------------------
Ros_Controller_SetIOState(IO_FEEDBACK_INITIALIZATION_DONE, FALSE);