Implemented the action interface.

Added the necessary conversions to the IMU data.

src/bioloid_registers.h

@@ -120,7 +120,7 @@ typedef enum {
   BIOLOID_IMU_TEMP                = 0x80,
   BIOLOID_ACTION_PAGE             = 0x81,
   BIOLOID_ACTION_CONTROL          = 0x82,
-  BIOLOID_ACTION_SATUTUS          = 0x83
+  BIOLOID_ACTION_STATUS           = 0x83
 } bioloid_registers;
 
 #endif

src/bioloid_robot.cpp

 #include "bioloid_robot.h"
 #include "bioloid_robot_exceptions.h"
+#include <iostream>
 
 CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id)
 {
@@ -213,7 +214,7 @@ void CBioloid_Robot::mm_set_period(double period_ms)
 
 unsigned char CBioloid_Robot::mm_get_num_servos(void)
 {
-  this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&this->num_servos);
+  this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->num_servos);
 
   return this->num_servos;
 }
@@ -386,27 +387,38 @@ double CBioloid_Robot::mm_get_servo_angle(unsigned char servo_id)
 // motion action interface
 void CBioloid_Robot::action_load_page(unsigned char page_id)
 {
-
+  this->robot_device->write_byte_register(BIOLOID_ACTION_PAGE,page_id);
 }
 
 unsigned char CBioloid_Robot::action_get_current_page(void)
 {
+  unsigned char page_id;
+
+  this->robot_device->read_byte_register(BIOLOID_ACTION_PAGE,&page_id);
 
+  return page_id;
 }
 
 void CBioloid_Robot::action_start(void)
 {
-
+  this->robot_device->write_byte_register(BIOLOID_ACTION_CONTROL,0x01);
 }
 
 void CBioloid_Robot::action_stop(void)
 {
-
+  this->robot_device->write_byte_register(BIOLOID_ACTION_CONTROL,0x02);
 }
 
 bool CBioloid_Robot::action_is_page_running(void)
 {
+  unsigned char status;
 
+  this->robot_device->read_byte_register(BIOLOID_ACTION_STATUS,&status);
+  std::cout << std::hex << (int)status << std::endl;
+  if(status&0x01)
+    return true;
+  else
+    return false;
 }
 
 // IMU interface
@@ -465,7 +477,7 @@ std::vector<double> CBioloid_Robot::imu_get_accel(void)
   this->robot_device->read_registers(BIOLOID_IMU_ACCEL_X_L,(unsigned char *)values,6);
 
   for(i=0;i<3;i++)
-    accel[i]=((double)values[i]);
+    accel[i]=((double)values[i]/1000.0);
 
   return accel;
 }
@@ -479,7 +491,7 @@ std::vector<double> CBioloid_Robot::imu_get_gyro(void)
   this->robot_device->read_registers(BIOLOID_IMU_GYRO_X_L,(unsigned char *)values,6);
 
   for(i=0;i<3;i++)
-    gyro[i]=((double)values[i]);
+    gyro[i]=((double)values[i]/4.0);
 
   return gyro;
 }
@@ -493,7 +505,7 @@ std::vector<double> CBioloid_Robot::imu_get_compass(void)
   this->robot_device->read_registers(BIOLOID_IMU_COMPASS_X_L,(unsigned char *)values,6);
 
   for(i=0;i<3;i++)
-    compass[i]=((double)values[i]);
+    compass[i]=((double)values[i]/8192.0);
 
   return compass;
 

src/examples/bioloid_robot_test.cpp

@@ -7,16 +7,55 @@ std::string robot_device="A401293W";
 
 int main(int argc, char *argv[])
 {
+  int i=0,num_servos,count=0;
+  std::vector<double> angles,accel,gyro,compass;
+
   try{
     CBioloid_Robot tina("Tina",robot_device,115200,192);
 
-    while(1)
+    num_servos=tina.mm_get_num_servos();
+    std::cout << "Found " << num_servos << " servos" << std::endl;
+    // enable all servos and assign them to the action module
+    for(i=0;i<num_servos;i++)
+    {
+      tina.mm_enable_servo(i);
+      tina.mm_assign_module(i,BIOLOID_MM_ACTION);
+    }
+    tina.mm_start();
+    tina.imu_enable();
+    angles=tina.mm_get_servo_angles();
+    for(i=0;i<num_servos;i++)
+      std::cout << "Servo " << i << " angle: " << angles[i] << std::endl;
+
+    // execute an action
+    tina.action_load_page(2);
+    tina.action_start();
+
+    while(tina.action_is_page_running())
+    {
+      usleep(100000);
+      angles=tina.mm_get_servo_angles();
+      for(i=0;i<num_servos;i++)
+        std::cout << "Servo " << i << " angle: " << angles[i] << std::endl;
+    }
+    for(i=0;i<10;i++)
     {
-      tina.set_manager_LED();
-      sleep(1);
-      tina.clear_manager_LED();
-      sleep(1);
+      accel=tina.imu_get_accel();
+      gyro=tina.imu_get_gyro();
+      compass=tina.imu_get_compass();
+      std::cout << "accel x: " << accel[0] << std::endl;
+      std::cout << "accel y: " << accel[1] << std::endl;
+      std::cout << "accel z: " << accel[2] << std::endl;
+      std::cout << "gyro x: " << gyro[0] << std::endl;
+      std::cout << "gyro y: " << gyro[1] << std::endl;
+      std::cout << "gyro z: " << gyro[2] << std::endl;
+      std::cout << "compass x: " << compass[0] << std::endl;
+      std::cout << "compass y: " << compass[1] << std::endl;
+      std::cout << "compass z: " << compass[2] << std::endl;
+      usleep(100000);
     }
+    tina.imu_disable();
+    tina.mm_stop();
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }