Changed the interface of the IMU module.

include/darwin_imu.h

@@ -10,12 +10,7 @@ class CDarwinIMU : public CDarwinRobotBase
     CDarwinIMU(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
     void start(void);
     void stop(void);
-    void start_gyro_cal(void);
-    void set_cal_samples(unsigned short int num_samples);
-    unsigned short int get_cal_samples(void);
-    bool is_gyro_cal_done(void);
-    bool is_accel_detected(void);
-    bool is_gyro_detected(void);
+    bool is_accel_calibrated(void);
     void get_accel_data(double *x,double *y,double *z);
     void get_gyro_data(double *x,double *y,double *z);
     virtual ~CDarwinIMU();

include/darwin_registers.h

@@ -119,22 +119,35 @@
 #define WALK_L_ANKLE_ROLL_SERVO_ID     99
 #define WALK_L_SHOULDER_PITCH_SERVO_ID 100
 
-#define IMU_CONTROL_OFFSET             392//   bit 7   |   bit 6  |    bit 5    |  bit 4  | bit 3 |    bit 2   | bit 1 | bit 0
-                                          // accel_det | gyro_det | calibrating | running |       | start_cal  | stop  | start
+#define IMU_CONTROL_OFFSET             392// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+                                          //       |       |       |       |       |       | stop  | start
   #define IMU_START                    0x01
   #define IMU_STOP                     0x02
-  #define IMU_START_CAL                0x04 
-  #define IMU_RUNNING                  0x10
-  #define IMU_CALIBRATING              0x20
-  #define IMU_ACCEL_DET                0x40
-  #define IMU_GYRO_DET                 0x80
-#define IMU_CAL_SAMPLES_OFFSET         393 
-#define IMU_GYRO_X_OFFSET              395
-#define IMU_GYRO_Y_OFFSET              397
-#define IMU_GYRO_Z_OFFSET              399
-#define IMU_ACCEL_X_OFFSET             401
-#define IMU_ACCEL_Y_OFFSET             403
-#define IMU_ACCEL_Z_OFFSET             405
+#define IMU_DUMMY_OFFSET               393 
+#define IMU_ACCEL_X_OFFSET             395
+#define IMU_ACCEL_Y_OFFSET             397
+#define IMU_ACCEL_Z_OFFSET             399
+#define IMU_COMPASS_X_OFFSET           401
+#define IMU_COMPASS_Y_OFFSET           403
+#define IMU_COMPASS_Z_OFFSET           405
+#define IMU_GYRO_X_OFFSET              407
+#define IMU_GYRO_Y_OFFSET              409
+#define IMU_GYRO_Z_OFFSET              411
+#define IMU_HEADING_OFFSET             413
+#define IMU_ROLL_OFFSET                415
+#define IMU_PITCH_OFFSET               417  
+#define IMU_QUATERNION_W_OFFSET        419 
+#define IMU_QUATERNION_X_OFFSET        421
+#define IMU_QUATERNION_Y_OFFSET        423
+#define IMU_QUATERNION_Z_OFFSET        425
+#define IMU_LIN_ACCEL_X_OFFSET         427
+#define IMU_LIN_ACCEL_Y_OFFSET         429
+#define IMU_LIN_ACCEL_Z_OFFSET         431
+#define IMU_GRAVITY_X_OFFSET           433
+#define IMU_GRAVITY_Y_OFFSET           435
+#define IMU_GRAVITY_Z_OFFSET           437
+#define IMU_TEMPERATURE_OFFSET         439
+#define IMU_CALIBRATION_STATUS_OFFSET  440
 
 #define BALANCE_KNEE_GAIN_OFFSET        7 
 #define BALANCE_ANKLE_ROLL_GAIN_OFFSET  9
@@ -142,7 +155,7 @@
 #define BALANCE_HIP_ROLL_GAIN_OFFSET    13
 
 #define ADC_PERIOD_OFFSET               15
-#define ADC_CONTROL_OFFSET              403// bit 7 | bit 6 | bit 5 |  bit 4  | bit 3 | bit 2 | bit 1 | bit 0
+#define ADC_CONTROL_OFFSET              441// bit 7 | bit 6 | bit 5 |  bit 4  | bit 3 | bit 2 | bit 1 | bit 0
                                            //       |       |       | running |       |       | stop  | start 
 
 #define ADC_START                       0x01

src/CMakeLists.txt

@@ -35,7 +35,7 @@ IF(EIGEN3_FOUND)
   SET(kin_leg_headers ../include/darwin_leg_kinematics.h ../include/darwin_robot_exceptions.h)
 ENDIF(EIGEN3_FOUND)
 
-ADD_DEFINITIONS(-D_SIM)
+#ADD_DEFINITIONS(-D_SIM)
 
 # add the necessary include directories
 INCLUDE_DIRECTORIES(../include)

src/darwin_imu.cpp

 #include "darwin_imu.h"
 #include "darwin_robot_exceptions.h"
+#include <iostream>
 
 CDarwinIMU::CDarwinIMU(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id) : CDarwinRobotBase(name,bus_id,bus_speed,id)
 {
@@ -11,66 +12,22 @@ void CDarwinIMU::start(void)
   this->robot_device->write_byte_register(IMU_CONTROL_OFFSET,IMU_START);
 }
 
-void CDarwinIMU::set_cal_samples(unsigned short int num_samples)
-{
-  this->is_valid();
-  this->robot_device->write_word_register(IMU_CAL_SAMPLES_OFFSET,num_samples);
-}
-
-unsigned short int CDarwinIMU::get_cal_samples(void)
-{
-  unsigned short int samples;
-
-  this->is_valid();
-  this->robot_device->read_word_register(IMU_CAL_SAMPLES_OFFSET,&samples);
-  return samples;
-}
-
 void CDarwinIMU::stop(void)
 {
   this->is_valid();
   this->robot_device->write_byte_register(IMU_CONTROL_OFFSET,IMU_STOP);
 }
 
-void CDarwinIMU::start_gyro_cal(void)
-{
-  this->is_valid();
-  this->robot_device->write_byte_register(IMU_CONTROL_OFFSET,IMU_START_CAL);
-}
-
-bool CDarwinIMU::is_gyro_cal_done(void)
+bool CDarwinIMU::is_accel_calibrated(void)
 {
   unsigned char status;
 
   this->is_valid();
-  this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
-  if(status&IMU_CALIBRATING)
-    return false;
-  else
+  this->robot_device->read_byte_register(IMU_CALIBRATION_STATUS_OFFSET,&status);
+  std::cout << "status: " << std::hex << (int)status << std::endl;
+  if((status&0xC0)==0xC0)
     return true;
-}
-
-bool CDarwinIMU::is_accel_detected(void)
-{
-  unsigned char status;
-
-  this->is_valid();
-  this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
-  if(status&IMU_ACCEL_DET)
-    return true;
-  else 
-    return false;
-}
-
-bool CDarwinIMU::is_gyro_detected(void)
-{
-  unsigned char status;
-
-  this->is_valid();
-  this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
-  if(status&IMU_GYRO_DET)
-    return true;
-  else 
+  else
     return false;
 }
 

src/examples/darwin_imu_test.cpp

@@ -18,18 +18,10 @@ int main(int argc, char *argv[])
   try{
     CDarwinIMU darwin_imu("Darwin",robot_device,1000000,0x01);
     std::cout << "found darwin controller" << std::endl;
-    std::cout << "Number of calibration samples: " << darwin_imu.get_cal_samples() << std::endl;
     darwin_imu.start();
-    std::cout << "Gyroscope detected: " << darwin_imu.is_gyro_detected() << std::endl;
-    std::cout << "Accelerometer detected: " << darwin_imu.is_accel_detected() << std::endl;
-    darwin_imu.start_gyro_cal();
-    while(!darwin_imu.is_gyro_cal_done())
-    {
-      std::cout << "calibrating gyro ..." << std::endl;
-      usleep(100000);
-    }
-    for(i=0;i<50;i++)
+    for(i=0;i<500;i++)
     {
+      darwin_imu.is_accel_calibrated(); 
       darwin_imu.get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
       darwin_imu.get_accel_data(&accel_x,&accel_y,&accel_z);
       std::cout << "Gyro: x:" << gyro_x << ", y: " << gyro_y << ", z: " << gyro_z << " Accel: x: " << accel_x << ", y: " << accel_y << " , z: " << accel_z << std::endl;

src/examples/darwin_walking_test.cpp

@@ -61,15 +61,6 @@ int main(int argc, char *argv[])
       usleep(100000);
     }
     imu.start();
-    std::cout << "Gyroscope detected: " << imu.is_gyro_detected() << std::endl;
-    std::cout << "Accelerometer detected: " << imu.is_accel_detected() << std::endl;
-    imu.start_gyro_cal();
-    while(!imu.is_gyro_cal_done())
-    {
-      std::cout << "calibrating gyro ..." << std::endl;
-      usleep(100000);
-    }
-    std::cout << "calibration done" << std::endl;
     balance.enable();
     i=0;
     remaining_servos=num_servos;