Added the API for the IMU dynamixel interface.

src/darwin_robot.cpp

@@ -194,7 +194,7 @@ bool CDarwinRobot::is_button_pressed(pushbutton_t pushbutton)
 }
 
 // ADC interface
-void CDarwinRobot::adc_enable(void)
+void CDarwinRobot::adc_start(void)
 {
   if(this->robot_device!=NULL)
     this->robot_device->write_byte_register(DARWIN_ADC_CNTRL,ADC_START);
@@ -202,6 +202,22 @@ void CDarwinRobot::adc_enable(void)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
+bool CDarwinRobot::adc_is_running(void)
+{
+  unsigned char status;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_ADC_CNTRL,&status);
+    if(status&ADC_RUNNING)
+      return true;
+    else
+      return false;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
 void CDarwinRobot::adc_set_period(unsigned char period_ms)
 {
   if(this->robot_device!=NULL)
@@ -230,6 +246,7 @@ double CDarwinRobot::adc_get_temperature(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_word_register(DARWIN_ADC_TEMP_L,&value);
+    std::cout << std::hex << value << std::endl;
 
     return ((double)value)/((double)(1<<10));
   }
@@ -251,7 +268,7 @@ double CDarwinRobot::adc_get_vrefint(void)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::adc_disable(void)
+void CDarwinRobot::adc_stop(void)
 {
   if(this->robot_device!=NULL)
     this->robot_device->write_byte_register(DARWIN_ADC_CNTRL,ADC_STOP);
@@ -259,6 +276,132 @@ void CDarwinRobot::adc_disable(void)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
+// imu interface
+void CDarwinRobot::imu_start(void)
+{
+  if(this->robot_device!=NULL)
+    this->robot_device->write_byte_register(DARWIN_IMU_CNTRL,IMU_START);
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::imu_set_cal_samples(unsigned short int num_samples)
+{
+  if(this->robot_device!=NULL)
+    this->robot_device->write_word_register(DARWIN_IMU_CAL_SAMPLES_L,num_samples);
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+unsigned short int CDarwinRobot::imu_get_cal_samples(void)
+{
+  unsigned short int samples;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_IMU_CAL_SAMPLES_L,&samples);
+    return samples;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::imu_stop(void)
+{
+  if(this->robot_device!=NULL)
+    this->robot_device->write_byte_register(DARWIN_IMU_CNTRL,IMU_STOP);
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::imu_start_gyro_cal(void)
+{
+  if(this->robot_device!=NULL)
+    this->robot_device->write_byte_register(DARWIN_IMU_CNTRL,IMU_START_CAL);
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::imu_is_gyro_cal_done(void)
+{
+  unsigned char status;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_IMU_CNTRL,&status);
+    if(status&IMU_CALIBRATING)
+      return true;
+    else
+      return false;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::imu_is_accel_detected(void)
+{
+  unsigned char status;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_IMU_CNTRL,&status);
+    if(status&IMU_ACCEL_DET)
+      return true;
+    else 
+      return false;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::imu_is_gyro_detected(void)
+{
+  unsigned char status;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_IMU_CNTRL,&status);
+    if(status&IMU_GYRO_DET)
+      return true;
+    else 
+      return false;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::imu_get_accel_data(double *x,double *y,double *z)
+{
+  unsigned char data[6];
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(DARWIN_IMU_ACCEL_X_L,data,6);
+
+    *x=((double)((signed short int)(data[0]+(data[1]<<8))))/1000.0;
+    *y=((double)((signed short int)(data[2]+(data[3]<<8))))/1000.0;
+    *z=((double)((signed short int)(data[4]+(data[5]<<8))))/1000.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::imu_get_gyro_data(double *x,double *y,double *z)
+{
+  unsigned char data[6];
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(DARWIN_IMU_GYRO_X_L,data,6);
+
+    *x=((double)(data[0]+(data[1]<<8)))/1000.0;
+    *y=((double)(data[2]+(data[3]<<8)))/1000.0;
+    *z=((double)(data[4]+(data[5]<<8)))/1000.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
 // motion manager interface
 /*void CDarwinRobot::mm_set_period(double period_ms)
 {
@@ -602,101 +745,6 @@ bool CDarwinRobot::action_is_page_running(void)
     return false;
 }
 
-// imu interface
-void CDarwinRobot::imu_start(void)
-{
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_IMU_CONTROL,&status);
-  status|=0x01;
-  this->robot_device->write_byte_register(DARWIN_IMU_CONTROL,status);
-}
-
-void CDarwinRobot::imu_stop(void)
-{
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_IMU_CONTROL,&status);
-  status&=0xFE;
-  this->robot_device->write_byte_register(DARWIN_IMU_CONTROL,status);
-}
-
-void CDarwinRobot::imu_start_gyro_cal(void)
-{
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_IMU_CONTROL,&status);
-  status|=0x02;
-  this->robot_device->write_byte_register(DARWIN_IMU_CONTROL,status);
-}
-
-bool CDarwinRobot::imu_is_gyro_cal_done(void)
-{
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_IMU_STATUS,&status);
-  if(status&0x08)
-    return true;
-  else
-    return false;
-}
-
-bool CDarwinRobot::imu_is_accel_detected(void)
-{
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_IMU_STATUS,&status);
-  if(status&0x01)
-    return true;
-  else 
-    return false;
-}
-
-bool CDarwinRobot::imu_is_gyro_detected(void)
-{
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_IMU_STATUS,&status);
-  if(status&0x02)
-    return true;
-  else 
-    return false;
-}
-
-void CDarwinRobot::imu_get_accel_data(short int *x,short int *y,short int *z)
-{
-  unsigned char data[6];
-
-  this->robot_device->read_registers(DARWIN_IMU_ACCEL_X_L,data,6);
-
-  *x=data[0]+(data[1]<<8);
-  *y=data[2]+(data[3]<<8);
-  *z=data[4]+(data[5]<<8);
-}
-
-void CDarwinRobot::imu_get_gyro_data(short int *x,short int *y,short int *z)
-{
-  unsigned char data[6];
-
-  this->robot_device->read_registers(DARWIN_IMU_GYRO_X_L,data,6);
-
-  *x=data[0]+(data[1]<<8);
-  *y=data[2]+(data[3]<<8);
-  *z=data[4]+(data[5]<<8);
-}
-
-// adc interface
-void CDarwinRobot::adc_get_data(void)
-{
-  unsigned char data[27];
-  int i=0;
-
-  this->robot_device->read_registers(DARWIN_SERVO_0_SPEED,data,27);
-
-  for(i=0;i<27;i++)
-    std::cout << (int)data[i] << std::endl;
-}
-
 // walking interface
 void CDarwinRobot::walking_set_speeds(double fw_step,double side_step,double turn)
 {

src/darwin_robot.h

@@ -56,12 +56,24 @@ class CDarwinRobot
     void gpio_aux2_color(double red, double green, double blue);
     bool is_button_pressed(pushbutton_t pushbutton);
     // ADC interface
-    void adc_enable(void);
+    void adc_start(void);
+    bool adc_is_running(void);
     void adc_set_period(unsigned char period_ms);
     double adc_get_value(adc_t adc);
     double adc_get_temperature(void);
     double adc_get_vrefint(void);
-    void adc_disable(void);
+    void adc_stop(void);
+    // imu interface
+    void imu_start(void);
+    void imu_stop(void);
+    void imu_start_gyro_cal(void);
+    void imu_set_cal_samples(unsigned short int num_samples);
+    unsigned short int imu_get_cal_samples(void);
+    bool imu_is_gyro_cal_done(void);
+    bool imu_is_accel_detected(void);
+    bool imu_is_gyro_detected(void);
+    void imu_get_accel_data(double *x,double *y,double *z);
+    void imu_get_gyro_data(double *x,double *y,double *z);
     // motion manager interface
     void mm_set_period(double period_ms);
     double mm_get_period(void);
@@ -92,15 +104,6 @@ class CDarwinRobot
     void action_start(void);
     void action_stop(void);
     bool action_is_page_running(void);
-    // imu interface
-    void imu_start(void);
-    void imu_stop(void);
-    void imu_start_gyro_cal(void);
-    bool imu_is_gyro_cal_done(void);
-    bool imu_is_accel_detected(void);
-    bool imu_is_gyro_detected(void);
-    void imu_get_accel_data(short int *x,short int *y,short int *z);
-    void imu_get_gyro_data(short int *x,short int *y,short int *z);
     // walking interface
     void walking_set_speeds(double fw_step,double side_step,double turn);
     void walking_start(void);

src/examples/CMakeLists.txt

@@ -13,6 +13,11 @@ ADD_EXECUTABLE(darwin_adc_test darwin_adc_test.cpp)
 # link necessary libraries
 TARGET_LINK_LIBRARIES(darwin_adc_test darwin_robot)
 
+# create an example application
+ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
+
 # create an example application
 #ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
 # link necessary libraries

src/examples/darwin_adc_test.cpp

+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+
+#include <iostream>
+
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
+
+int main(int argc, char *argv[])
+{
+  int i=0;
+
+  try{
+    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    std::cout << "found darwin controller" << std::endl;
+    darwin.adc_start();
+    for(i=0;i<1000;i++)
+    {
+      std::cout << "Temperature: " << darwin.adc_get_temperature() << std::endl;
+      std::cout << "Vref: " << darwin.adc_get_vrefint() << std::endl;
+      std::cout << "Channel1: " << darwin.adc_get_value(ADC_CH1) << std::endl;
+      std::cout << "Channel2: " << darwin.adc_get_value(ADC_CH2) << std::endl;
+      std::cout << "Channel3: " << darwin.adc_get_value(ADC_CH3) << std::endl;
+      std::cout << "Channel4: " << darwin.adc_get_value(ADC_CH4) << std::endl;
+      std::cout << "Channel5: " << darwin.adc_get_value(ADC_CH5) << std::endl;
+      std::cout << "Channel6: " << darwin.adc_get_value(ADC_CH6) << std::endl;
+      std::cout << "Channel7: " << darwin.adc_get_value(ADC_CH7) << std::endl;
+      std::cout << "Channel8: " << darwin.adc_get_value(ADC_CH8) << std::endl;
+      std::cout << "Channel9: " << darwin.adc_get_value(ADC_CH9) << std::endl;
+      std::cout << "Channel10: " << darwin.adc_get_value(ADC_CH10) << std::endl;
+      std::cout << "Channel11: " << darwin.adc_get_value(ADC_CH11) << std::endl;
+      std::cout << "Channel12: " << darwin.adc_get_value(ADC_CH12) << std::endl;
+      std::cout << "Channel13: " << darwin.adc_get_value(ADC_CH13) << std::endl;
+      std::cout << "Channel14: " << darwin.adc_get_value(ADC_CH14) << std::endl;
+      std::cout << "Channel15: " << darwin.adc_get_value(ADC_CH16) << std::endl;
+      std::cout << "Channel16: " << darwin.adc_get_value(ADC_CH18) << std::endl;
+      usleep(100000);
+    }
+    darwin.adc_stop();
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}

src/examples/darwin_imu_test.cpp

@@ -3,52 +3,26 @@
 
 #include <iostream>
 
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
 
 int main(int argc, char *argv[])
 {
-  int i=0,num_v1_servos,num_v2_servos;
-  std::vector<double> angles;
-  short int accel_x,accel_y,accel_z;
-  short int gyro_x,gyro_y,gyro_z;
+  int i=0;
+  double accel_x,accel_y,accel_z;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
-    darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
-    std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
-    darwin.mm_enable_power();
-
-    // calibrate the imu
-    darwin.imu_start_gyro_cal();
-    while(!darwin.imu_is_gyro_cal_done())
-    {
-      std::cout << "waiting for the gyro to finish calibration" << std::endl;
-      usleep(100000);
-    }
+    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    std::cout << "found darwin controller" << std::endl;
+    std::cout << "Number of calibration samples: " << darwin.imu_get_cal_samples() << std::endl;
     darwin.imu_start();
-    for(i=1;i<=num_v1_servos+num_v2_servos;i++)
-    {
-      darwin.mm_enable_servo(i);
-      darwin.mm_assign_module(i,DARWIN_MM_JOINTS);
-    }
-    darwin.mm_start();
-    for(i=0;i<100000;i++)
+    for(i=0;i<50;i++)
     {
       darwin.imu_get_accel_data(&accel_x,&accel_y,&accel_z);
-      std::cout << accel_x << "," << accel_y << "," << accel_z << "," << i << std::endl;
-      darwin.imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
-      std::cout << gyro_x << "," << gyro_y << "," << gyro_z << std::endl;
-      darwin.adc_get_data();
-      angles=darwin.mm_get_servo_angles();
-      for(i=0;i<angles.size();i++)
-        std::cout << "Servo " << i << " angle: " << angles[i] << std::endl;
-      std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
+      std::cout << "Accel: x:" << accel_x << ", y: " << accel_y << ", z: " << accel_z << std::endl;
       usleep(100000);
     }
-    darwin.mm_stop();
-    std::cout << "ending" << std::endl;
     darwin.imu_stop();
-    darwin.mm_disable_power();
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }