Used the Serial version of the Dynamixel Server.

Changed the buttons mappings to use the cable with the PS3 controller.
Added the move joits functions to move between predefined positions.

include/arm_control.h

 #ifndef _ARM_CONTROL_H
 #define _ARM_CONTROL_H
 
-#include "dynamixelserver_ftdi.h"
+#include "dynamixelserver_serial.h"
 #include "dynamixel_motor.h"
 #include "threadserver.h"
 #include "eventserver.h"
@@ -28,14 +28,24 @@ class CArmControl
     CEventServer *event_server;
     std::string finish_thread_event_id;
     std::vector<CDynamixelMotor *> motors;
-    CDynamixelServerFTDI *dyn_server;
+    unsigned char pan_id;
+    unsigned char tilt_id;
+    unsigned char roll_id;
+    CDynamixelServerSerial *dyn_server;
     CJoystick joy;
     std::vector<bool> current_button_state;
     double max_speed;
+    double max_accel;
     TJoints max_angles;
     TJoints min_angles;
     TJoints max_torques;
     TJoints target_torques;
+    TJoints target_angles;
+    TJoints target_stop_angles;
+    TJoints target_speeds;
+    TJoints target_accels;
+    TJoints target_dir;
+    bool target_run;
     std::vector<TJoints> stored_poses;
     std::string poses_file;
     CBNO055IMUDriver imu;
@@ -43,6 +53,7 @@ class CArmControl
     TJoints imu_zero;
   protected:
     static void *position_monitor_thread(void *param);
+    void move_joints(void);
   public:
     CArmControl(const std::string &joy_device,const std::string &imu_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id);
     bool get_button_state(unsigned int button_id);

src/examples/arm_control_test.cpp

@@ -2,14 +2,14 @@
 #include "exceptions.h"
 #include <iostream>
 
-std::string dyn_serial="FT2FWOYO";
+std::string dyn_device="/dev/ttyUSB0";
 std::string joy_device="/dev/input/js0";
 std::string imu_device="/dev/ttyAMA0";
 
 int main(int argc, char *argv[])
 {
   try{
-    CArmControl arm(joy_device,imu_device,dyn_serial,1000000,6,5,4);
+    CArmControl arm(joy_device,imu_device,dyn_device,1000000,6,5,4);
 
     arm.load_poses("/home/pi/code/arm_control/src/xml/poses.xml");
     arm.set_max_angles(PAN,90.0,-90.0);

src/xml/poses.xml

@@ -2,26 +2,26 @@
 <arm_motions xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="arm_motions_cfg.xsd">
   <arm_motion>
     <button_name>triangle</button_name>
-    <pan>2.49267</pan>
-    <tilt>1.6129</tilt>
-    <roll>-64.6628</roll>
+    <pan>90.176</pan>
+    <tilt>2.78592</tilt>
+    <roll>-3.37243</roll>
   </arm_motion>
   <arm_motion>
     <button_name>circle</button_name>
-    <pan>-6.59824</pan>
-    <tilt>-3.07918</tilt>
+    <pan>-98.9736</pan>
+    <tilt>-8.06452</tilt>
     <roll>-2.49267</roll>
   </arm_motion>
   <arm_motion>
     <button_name>cross</button_name>
-    <pan>-82.5513</pan>
-    <tilt>-7.77126</tilt>
-    <roll>28.2991</roll>
+    <pan>-3.37243</pan>
+    <tilt>-4.2522</tilt>
+    <roll>-92.8152</roll>
   </arm_motion>
   <arm_motion>
     <button_name>square</button_name>
-    <pan>-42.9619</pan>
-    <tilt>38.563</tilt>
-    <roll>-58.2111</roll>
+    <pan>-4.54545</pan>
+    <tilt>41.4956</tilt>
+    <roll>-90.7625</roll>
   </arm_motion>
 </arm_motions>