diff --git a/include/darwin_imu.h b/include/darwin_imu.h
index 2bcde08815ff7e3cf36b9c4aa17913cb92f87ae6..7774de35b0606c11ac2a9c199914eb17f742034a 100644
--- a/include/darwin_imu.h
+++ b/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();
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 04fc7ec5e9c4f140c9a182e4356e13c5680f08b6..32b34195439f2d3d51bb4ca40d3544fa1c0520ab 100644
--- a/include/darwin_registers.h
+++ b/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
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 36145dd1d25eac593ae559b1bdbc76162aad7458..40c95ed4ecd6bf84d9b7c8ae5996c11732cb0356 100644
--- a/src/CMakeLists.txt
+++ b/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)
diff --git a/src/darwin_imu.cpp b/src/darwin_imu.cpp
index 703666cead6fa5fe1f44bb755e6dd304d1b8606a..45089ac1bc4e8aa003fb68b69a76dbc5cc0d7d90 100644
--- a/src/darwin_imu.cpp
+++ b/src/darwin_imu.cpp
@@ -1,5 +1,6 @@
#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;
}
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index d911ad868e5500d1a0a708ba3a22db33bd594e7e..f87360add9ab527c8640530ce74e2fd9d1c5ada3 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/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;
diff --git a/src/examples/darwin_walking_test.cpp b/src/examples/darwin_walking_test.cpp
index 095108d41c097731635252a7aba942996374ae42..d43e42d61ef6241f28ca25933a794dda24665188 100644
--- a/src/examples/darwin_walking_test.cpp
+++ b/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;