Removed the imu and joy objects from the arm_control object.

Added functions to get and set the necessary information from and to the outside.

include/arm_control.h

@@ -6,8 +6,6 @@
 #include "threadserver.h"
 #include "eventserver.h"
 #include "mutex.h"
-#include "joystick.h"
-#include "bno055_imu_driver.h"
 
 typedef enum {PAN=0,TILT=1,ROLL=2} motor_id_t;
 
@@ -23,6 +21,7 @@ class CArmControl
   private:
     CMutex access;
     bool exit_flag;
+    bool enabled;
     CThreadServer *thread_server;
     std::string position_monitor_thread_id;
     CEventServer *event_server;
@@ -32,7 +31,6 @@ class CArmControl
     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;
@@ -45,25 +43,32 @@ class CArmControl
     TJoints target_speeds;
     TJoints target_accels;
     TJoints target_dir;
+    TJoints current_angles;
     bool target_run;
     std::vector<TJoints> stored_poses;
     std::string poses_file;
-    CBNO055IMUDriver imu;
     bool imu_enabled;    
     TJoints imu_zero;
+    TJoints imu_angles;
+    bool torque_enabled;
   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);
-    bool exit(void);
-    // servo functionis
+    CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id);
+    void exit(void);
+    bool is_exit(void);
+    void enable(void);
+    void disable(void);
+    bool is_enabled(void);
+    // servo functions
     void set_max_angles(unsigned int axis_id,double max,double min);
     void set_max_torque(unsigned int axis_id,double max);
     void enable_torque(bool enable);
+    bool is_torque_enabled(void);
     void move(unsigned int axis_id,double torque);
     void stop(void);
+    void get_current_angles(double &pan, double &tilt,double &roll);
     // stored poses
     void increase_max_speed(void);
     void decrease_max_speed(void);
@@ -71,7 +76,11 @@ class CArmControl
     void save_poses(const std::string &filename);
     void update_pose(unsigned int pose_id);
     void execute_pose(unsigned int pose_id);
-    void toggle_imu(void);    
+    // imu functions
+    void enable_imu(double pan,double tilt, double roll);    
+    void disable_imu(void);
+    bool is_imu_enabled(void);
+    void update_imu(double pan,double tilt, double roll);
     ~CArmControl();
 };
 

src/examples/arm_control_test.cpp

@@ -17,7 +17,7 @@ std::string motion_file_path="/home/sergi/Downloads/arm_control/src/xml/poses.xm
 int main(int argc, char *argv[])
 {
   try{
-    CArmControl arm(joy_device,imu_device,dyn_device,1000000,6,5,4);
+    CArmControl arm(dyn_device,1000000,6,5,4);
 
     arm.load_poses(motion_file_path);
     arm.set_max_angles(PAN,130.0,-130.0);
@@ -28,7 +28,7 @@ int main(int argc, char *argv[])
     arm.set_max_torque(ROLL,30);
  
   
-    while(!arm.exit());
+    while(!arm.is_exit());
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }