diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 167c0d70a0878966357380ebfde0f02e05a922d0..1f2f2b1167a9392aa350d6187eb9d35d8bc933e4 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -4,15 +4,19 @@ SET(sources bioloid_robot.cpp bioloid_robot_exceptions.cpp)
SET(headers bioloid_robot.h bioloid_robot_exceptions.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
# add the necessary include directories
INCLUDE_DIRECTORIES(.)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(~/bioloid_robot/stm32_code/include/)
# create the shared library
ADD_LIBRARY(bioloid_robot SHARED ${sources})
# link necessary libraries
TARGET_LINK_LIBRARIES(bioloid_robot ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(bioloid_robot ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(bioloid_robot ${dynamixel_LIBRARY})
INSTALL(TARGETS bioloid_robot
RUNTIME DESTINATION bin
diff --git a/src/bioloid_registers.h b/src/bioloid_registers.h
deleted file mode 100644
index 0db62862f8aaf57aa1a77e5edcb572fa382c5924..0000000000000000000000000000000000000000
--- a/src/bioloid_registers.h
+++ /dev/null
@@ -1,126 +0,0 @@
-#ifndef _BIOLOID_REGISTERS_
-#define _BIOLOID_REGISTERS_
-
-#define MAX_NUM_SERVOS 31
-
-typedef enum {
- BIOLOID_MODEL_NUMBER_L = 0x00,
- BIOLOID_MODEL_NUMBER_H = 0x01,
- BIOLOID_VERSION = 0x02,
- BIOLOID_ID = 0x03,
- BIOLOID_BAUD_RATE = 0x04,
- BIOLOID_RETURN_DELAY_TIME = 0x05,
- BIOLOID_MM_PERIOD_LOW = 0x06,
- BIOLOID_MM_PERIOD_HIGH = 0x07,
- BIOLOID_RETURN_LEVEL = 0x10,
- BIOLOID_DXL_POWER = 0x18,
- BIOLOID_LED = 0x19,
- BIOLOID_PUSHBUTTON = 0x1A,
- BIOLOID_MM_NUM_SERVOS = 0x1B,
- BIOLOID_MM_CONTROL = 0x1C,
- BIOLOID_MM_STATUS = 0x1D,
- BIOLOID_MM_MODULE_EN0 = 0x1E,
- BIOLOID_MM_MODULE_EN1 = 0x1F,
- BIOLOID_MM_MODULE_EN2 = 0x20,
- BIOLOID_MM_MODULE_EN3 = 0x21,
- BIOLOID_MM_MODULE_EN4 = 0x22,
- BIOLOID_MM_MODULE_EN5 = 0x23,
- BIOLOID_MM_MODULE_EN6 = 0x24,
- BIOLOID_MM_MODULE_EN7 = 0x25,
- BIOLOID_MM_MODULE_EN8 = 0x26,
- BIOLOID_MM_MODULE_EN9 = 0x27,
- BIOLOID_MM_MODULE_EN10 = 0x28,
- BIOLOID_MM_MODULE_EN11 = 0x29,
- BIOLOID_MM_MODULE_EN12 = 0x2A,
- BIOLOID_MM_MODULE_EN13 = 0x2B,
- BIOLOID_MM_MODULE_EN14 = 0x2C,
- BIOLOID_MM_MODULE_EN15 = 0x2D,
- BIOLOID_MM_SERVO0_CUR_POS_L = 0x2E,
- BIOLOID_MM_SERVO0_CUR_POS_H = 0x2F,
- BIOLOID_MM_SERVO1_CUR_POS_L = 0x30,
- BIOLOID_MM_SERVO1_CUR_POS_H = 0x31,
- BIOLOID_MM_SERVO2_CUR_POS_L = 0x32,
- BIOLOID_MM_SERVO2_CUR_POS_H = 0x33,
- BIOLOID_MM_SERVO3_CUR_POS_L = 0x34,
- BIOLOID_MM_SERVO3_CUR_POS_H = 0x35,
- BIOLOID_MM_SERVO4_CUR_POS_L = 0x36,
- BIOLOID_MM_SERVO4_CUR_POS_H = 0x37,
- BIOLOID_MM_SERVO5_CUR_POS_L = 0x38,
- BIOLOID_MM_SERVO5_CUR_POS_H = 0x39,
- BIOLOID_MM_SERVO6_CUR_POS_L = 0x3A,
- BIOLOID_MM_SERVO6_CUR_POS_H = 0x3B,
- BIOLOID_MM_SERVO7_CUR_POS_L = 0x3C,
- BIOLOID_MM_SERVO7_CUR_POS_H = 0x3D,
- BIOLOID_MM_SERVO8_CUR_POS_L = 0x3E,
- BIOLOID_MM_SERVO8_CUR_POS_H = 0x3F,
- BIOLOID_MM_SERVO9_CUR_POS_L = 0x40,
- BIOLOID_MM_SERVO9_CUR_POS_H = 0x41,
- BIOLOID_MM_SERVO10_CUR_POS_L = 0x42,
- BIOLOID_MM_SERVO10_CUR_POS_H = 0x43,
- BIOLOID_MM_SERVO11_CUR_POS_L = 0x44,
- BIOLOID_MM_SERVO11_CUR_POS_H = 0x45,
- BIOLOID_MM_SERVO12_CUR_POS_L = 0x46,
- BIOLOID_MM_SERVO12_CUR_POS_H = 0x47,
- BIOLOID_MM_SERVO13_CUR_POS_L = 0x48,
- BIOLOID_MM_SERVO13_CUR_POS_H = 0x49,
- BIOLOID_MM_SERVO14_CUR_POS_L = 0x4A,
- BIOLOID_MM_SERVO14_CUR_POS_H = 0x4B,
- BIOLOID_MM_SERVO15_CUR_POS_L = 0x4C,
- BIOLOID_MM_SERVO15_CUR_POS_H = 0x4D,
- BIOLOID_MM_SERVO16_CUR_POS_L = 0x4E,
- BIOLOID_MM_SERVO16_CUR_POS_H = 0x4F,
- BIOLOID_MM_SERVO17_CUR_POS_L = 0x50,
- BIOLOID_MM_SERVO17_CUR_POS_H = 0x51,
- BIOLOID_MM_SERVO18_CUR_POS_L = 0x52,
- BIOLOID_MM_SERVO18_CUR_POS_H = 0x53,
- BIOLOID_MM_SERVO19_CUR_POS_L = 0x54,
- BIOLOID_MM_SERVO19_CUR_POS_H = 0x55,
- BIOLOID_MM_SERVO20_CUR_POS_L = 0x56,
- BIOLOID_MM_SERVO20_CUR_POS_H = 0x57,
- BIOLOID_MM_SERVO21_CUR_POS_L = 0x58,
- BIOLOID_MM_SERVO21_CUR_POS_H = 0x59,
- BIOLOID_MM_SERVO22_CUR_POS_L = 0x5A,
- BIOLOID_MM_SERVO22_CUR_POS_H = 0x5B,
- BIOLOID_MM_SERVO23_CUR_POS_L = 0x5C,
- BIOLOID_MM_SERVO23_CUR_POS_H = 0x5D,
- BIOLOID_MM_SERVO24_CUR_POS_L = 0x5E,
- BIOLOID_MM_SERVO24_CUR_POS_H = 0x5F,
- BIOLOID_MM_SERVO25_CUR_POS_L = 0x60,
- BIOLOID_MM_SERVO25_CUR_POS_H = 0x61,
- BIOLOID_MM_SERVO26_CUR_POS_L = 0x62,
- BIOLOID_MM_SERVO26_CUR_POS_H = 0x63,
- BIOLOID_MM_SERVO27_CUR_POS_L = 0x64,
- BIOLOID_MM_SERVO27_CUR_POS_H = 0x65,
- BIOLOID_MM_SERVO28_CUR_POS_L = 0x66,
- BIOLOID_MM_SERVO28_CUR_POS_H = 0x67,
- BIOLOID_MM_SERVO29_CUR_POS_L = 0x68,
- BIOLOID_MM_SERVO29_CUR_POS_H = 0x69,
- BIOLOID_MM_SERVO30_CUR_POS_L = 0x6A,
- BIOLOID_MM_SERVO30_CUR_POS_H = 0x6B,
- BIOLOID_IMU_STATUS = 0x6C,
- BIOLOID_IMU_CONTROL = 0x6D,
- BIOLOID_IMU_ACCEL_X_L = 0x6E,
- BIOLOID_IMU_ACCEL_X_H = 0x6F,
- BIOLOID_IMU_ACCEL_Y_L = 0x70,
- BIOLOID_IMU_ACCEL_Y_H = 0x71,
- BIOLOID_IMU_ACCEL_Z_L = 0x72,
- BIOLOID_IMU_ACCEL_Z_H = 0x73,
- BIOLOID_IMU_GYRO_X_L = 0x74,
- BIOLOID_IMU_GYRO_X_H = 0x75,
- BIOLOID_IMU_GYRO_Y_L = 0x76,
- BIOLOID_IMU_GYRO_Y_H = 0x77,
- BIOLOID_IMU_GYRO_Z_L = 0x78,
- BIOLOID_IMU_GYRO_Z_H = 0x79,
- BIOLOID_IMU_COMPASS_X_L = 0x7A,
- BIOLOID_IMU_COMPASS_X_H = 0x7B,
- BIOLOID_IMU_COMPASS_Y_L = 0x7C,
- BIOLOID_IMU_COMPASS_Y_H = 0x7D,
- BIOLOID_IMU_COMPASS_Z_L = 0x7E,
- BIOLOID_IMU_COMPASS_Z_H = 0x7F,
- BIOLOID_IMU_TEMP = 0x80,
- BIOLOID_ACTION_PAGE = 0x81,
- BIOLOID_ACTION_CONTROL = 0x82,
- BIOLOID_ACTION_STATUS = 0x83
-} bioloid_registers;
-
-#endif
diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp
index 6ac13f9690df3c74f56e5a0b029a5dbfddc97d40..e78961dc99affcca2b5061d58a71dcd0ae6ab52e 100644
--- a/src/bioloid_robot.cpp
+++ b/src/bioloid_robot.cpp
@@ -2,29 +2,36 @@
#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)
+CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id)
{
- int num_buses;
-
this->robot_device=NULL;
- this->current_led_value=0x00;
- this->current_pushbutton_value=0x00;
- this->num_servos=0;
+ this->serial_device=NULL;
if(name!="")
{
this->robot_name=name;
this->dyn_server=CDynamixelServer::instance();
- num_buses=this->dyn_server->get_num_buses();
- if(num_buses>0)
- {
- this->dyn_server->config_bus(bus_id,bus_speed);
+ try{
+ /* open and configure the serial port */
+ this->serial_device=new CRS232(this->robot_name+"_serial_port");
+ this->serial_device->open((void *)&serial_dev);
+ this->serial_config.baud=baudrate;
+ this->serial_config.num_bits=8;
+ this->serial_config.parity=none;
+ this->serial_config.stop_bits=1;
+ this->serial_device->config(&this->serial_config);
+ this->dyn_server->config_comm(this->serial_device);
this->robot_device=dyn_server->get_device(id);
this->robot_id=id;
- }
- else
- {
+ }catch(CException &e){
+ if(this->robot_device!=NULL)
+ this->dyn_server->free_device(id);
+ if(this->serial_device!=NULL)
+ {
+ delete this->serial_device;
+ this->serial_device=NULL;
+ }
/* handle exceptions */
- throw CBioloidRobotException(_HERE_,"No FTDI USB devices found");
+ throw;
}
}
else
@@ -35,176 +42,81 @@ CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &bus_id,int b
}
// GPIO interface
-void CBioloid_Robot::set_manager_LED(void)
-{
- this->current_led_value|=0x01;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::clear_manager_LED(void)
-{
- this->current_led_value&=0xFE;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::set_program_LED(void)
-{
- this->current_led_value|=0x02;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::clear_program_LED(void)
-{
- this->current_led_value&=0xFD;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::set_play_LED(void)
-{
- this->current_led_value|=0x04;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::clear_play_LED(void)
-{
- this->current_led_value&=0xFB;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::set_TxD_LED(void)
-{
- this->current_led_value|=0x08;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::clear_TxD_LED(void)
-{
- this->current_led_value&=0xF7;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::set_RxD_LED(void)
-{
- this->current_led_value|=0x10;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::clear_RxD_LED(void)
-{
- this->current_led_value&=0xEF;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::set_aux_LED(void)
-{
- this->current_led_value|=0x20;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-void CBioloid_Robot::clear_aux_LED(void)
+void CBioloid_Robot::set_led(led_t led)
{
- this->current_led_value&=0xDF;
- this->robot_device->write_byte_register(BIOLOID_LED,this->current_led_value);
-}
-
-bool CBioloid_Robot::is_mode_button_pressed(void)
-{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x01)
+ switch(led)
{
- this->current_pushbutton_value&=0xFE;
- return true;
+ case USER1_LED: this->robot_device->write_byte_register(BIOLOID_USER1_LED_CNTRL,0x02);
+ break;
+ case USER2_LED: this->robot_device->write_byte_register(BIOLOID_USER2_LED_CNTRL,0x02);
+ break;
+ case RXD_LED: this->robot_device->write_byte_register(BIOLOID_RXD_LED_CNTRL,0x02);
+ break;
+ case TXD_LED: this->robot_device->write_byte_register(BIOLOID_TXD_LED_CNTRL,0x02);
+ break;
}
- else
- return false;
}
-bool CBioloid_Robot::is_start_button_pressed(void)
+void CBioloid_Robot::clear_led(led_t led)
{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x02)
+ switch(led)
{
- this->current_pushbutton_value&=0xFD;
- return true;
+ case USER1_LED: this->robot_device->write_byte_register(BIOLOID_USER1_LED_CNTRL,0x00);
+ break;
+ case USER2_LED: this->robot_device->write_byte_register(BIOLOID_USER2_LED_CNTRL,0x00);
+ break;
+ case RXD_LED: this->robot_device->write_byte_register(BIOLOID_RXD_LED_CNTRL,0x00);
+ break;
+ case TXD_LED: this->robot_device->write_byte_register(BIOLOID_TXD_LED_CNTRL,0x00);
+ break;
}
- else
- return false;
}
-bool CBioloid_Robot::is_up_button_pressed(void)
+void CBioloid_Robot::toggle_led(led_t led)
{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x04)
+ switch(led)
{
- this->current_pushbutton_value&=0xFB;
- return true;
+ case USER1_LED: this->robot_device->write_byte_register(BIOLOID_USER1_LED_CNTRL,0x04);
+ break;
+ case USER2_LED: this->robot_device->write_byte_register(BIOLOID_USER2_LED_CNTRL,0x04);
+ break;
+ case RXD_LED: this->robot_device->write_byte_register(BIOLOID_RXD_LED_CNTRL,0x04);
+ break;
+ case TXD_LED: this->robot_device->write_byte_register(BIOLOID_TXD_LED_CNTRL,0x04);
+ break;
}
- else
- return false;
}
-bool CBioloid_Robot::is_down_button_pressed(void)
+void CBioloid_Robot::blink_led(led_t led,int period_ms)
{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&value);
+ unsigned char data[3];
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x08)
+ data[0]=0x08;// blink led
+ data[1]=period_ms%256;
+ data[2]=period_ms/256;
+ switch(led)
{
- this->current_pushbutton_value&=0xF7;
- return true;
+ case USER1_LED: this->robot_device->write_registers(BIOLOID_USER1_LED_CNTRL,data,3);
+ break;
+ case USER2_LED: this->robot_device->write_registers(BIOLOID_USER2_LED_CNTRL,data,3);
+ break;
+ case RXD_LED: this->robot_device->write_registers(BIOLOID_RXD_LED_CNTRL,data,3);
+ break;
+ case TXD_LED: this->robot_device->write_registers(BIOLOID_TXD_LED_CNTRL,data,3);
+ break;
}
- else
- return false;
}
-bool CBioloid_Robot::is_left_button_pressed(void)
+bool CBioloid_Robot::is_button_pressed(pushbutton_t pushbutton)
{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x10)
- {
- this->current_pushbutton_value&=0xEF;
- return true;
- }
- else
- return false;
}
-bool CBioloid_Robot::is_right_button_pressed(void)
+void assign_button_callback(pushbutton_t pushbutton, void (*pb_callback)(void))
{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x20)
- {
- this->current_pushbutton_value&=0xDF;
- return true;
- }
- else
- return false;
}
// motion manager interface
-void CBioloid_Robot::mm_set_period(double period_ms)
+/*void CBioloid_Robot::mm_set_period(double period_ms)
{
unsigned short period;
// internal time in 0|16 fixed point float format in seconds
@@ -256,7 +168,6 @@ void CBioloid_Robot::mm_enable_servo(unsigned char servo_id)
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -274,7 +185,6 @@ void CBioloid_Robot::mm_disable_servo(unsigned char servo_id)
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -292,7 +202,6 @@ bool CBioloid_Robot::mm_is_servo_enabled(unsigned char servo_id)
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -319,7 +228,6 @@ void CBioloid_Robot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -343,7 +251,6 @@ mm_mode_t CBioloid_Robot::mm_get_module(unsigned char servo_id)
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -375,17 +282,16 @@ double CBioloid_Robot::mm_get_servo_angle(unsigned char servo_id)
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
this->robot_device->read_word_register(BIOLOID_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
angle=((double)value)/128.0;
return angle;
-}
+}*/
// motion action interface
-void CBioloid_Robot::action_load_page(unsigned char page_id)
+/*void CBioloid_Robot::action_load_page(unsigned char page_id)
{
this->robot_device->write_byte_register(BIOLOID_ACTION_PAGE,page_id);
}
@@ -419,114 +325,16 @@ bool CBioloid_Robot::action_is_page_running(void)
return true;
else
return false;
-}
-
-// IMU interface
-void CBioloid_Robot::imu_enable(void)
-{
- this->robot_device->write_byte_register(BIOLOID_IMU_CONTROL,0x01);
-}
-
-void CBioloid_Robot::imu_disable(void)
-{
- this->robot_device->write_byte_register(BIOLOID_IMU_CONTROL,0x00);
-}
-
-bool CBioloid_Robot::imu_is_accel_present(void)
-{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_IMU_STATUS,&value);
-
- if(value&0x02)
- return true;
- else
- return false;
-}
-
-bool CBioloid_Robot::imu_is_gyro_present(void)
-{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_IMU_STATUS,&value);
-
- if(value&0x04)
- return true;
- else
- return false;
-}
-
-bool CBioloid_Robot::imu_is_compass_present(void)
-{
- unsigned char value;
-
- this->robot_device->read_byte_register(BIOLOID_IMU_STATUS,&value);
-
- if(value&0x08)
- return true;
- else
- return false;
-}
-
-std::vector<double> CBioloid_Robot::imu_get_accel(void)
-{
- int i=0;
- short int values[3];
- std::vector<double> accel(3);
-
- 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]/1000.0);
-
- return accel;
-}
-
-std::vector<double> CBioloid_Robot::imu_get_gyro(void)
-{
- int i=0;
- short int values[3];
- std::vector<double> gyro(3);
-
- 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]/4.0);
-
- return gyro;
-}
-
-std::vector<double> CBioloid_Robot::imu_get_compass(void)
-{
- int i=0;
- short int values[3];
- std::vector<double> compass(3);
-
- 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]/8192.0);
-
- return compass;
-
-}
+}*/
-double CBioloid_Robot::imu_get_temperature(void)
-{
- unsigned short int value;
- double temp;
-
- this->robot_device->read_word_register(BIOLOID_IMU_TEMP,&value);
- temp=((double)value);
-
- return temp;
-}
-
CBioloid_Robot::~CBioloid_Robot()
{
- if(this->robot_device!=NULL)
+ if(this->serial_device!=NULL)
{
- this->dyn_server->free_device(this->robot_id);
+ delete this->serial_device;
+ this->serial_device=NULL;
}
+ if(this->robot_device!=NULL)
+ this->dyn_server->free_device(this->robot_id);
}
diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h
index 48338b6b46df2ca9c2e2e995fb180879a2617ef5..bfb182dd22be426cc96faa3709eef6ac130561c3 100644
--- a/src/bioloid_robot.h
+++ b/src/bioloid_robot.h
@@ -1,46 +1,41 @@
#ifndef _BIOLOID_ROBOT_H
#define _BIOLOID_ROBOT_H
+#include "dynamixelserver.h"
#include "dynamixel.h"
+#include "rs232.h"
#include "bioloid_registers.h"
+/* available motion modules */
typedef enum {BIOLOD_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t;
+/* available leds */
+typedef enum {USER1_LED,USER2_LED,RXD_LED,TXD_LED} led_t;
+/* available pushbuttons */
+typedef enum {USER_PB,RESET_PB,START_PB,MODE_PB} pushbutton_t;
class CBioloid_Robot
{
private:
+ /* dynamixel interface */
CDynamixelServer *dyn_server;
CDynamixel *robot_device;
- std::string robot_name;
unsigned char robot_id;
- // GPIO variables
- unsigned char current_led_value;
- unsigned char current_pushbutton_value;
- // motion manager variables
- unsigned char num_servos;
+ /* communication interface */
+ CRS232 *serial_device;
+ TRS232_config serial_config;
+ /* robot's name */
+ std::string robot_name;
public:
- CBioloid_Robot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
+ CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id);
// GPIO interface
- void set_manager_LED(void);
- void clear_manager_LED(void);
- void set_program_LED(void);
- void clear_program_LED(void);
- void set_play_LED(void);
- void clear_play_LED(void);
- void set_TxD_LED(void);
- void clear_TxD_LED(void);
- void set_RxD_LED(void);
- void clear_RxD_LED(void);
- void set_aux_LED(void);
- void clear_aux_LED(void);
- bool is_mode_button_pressed(void);
- bool is_start_button_pressed(void);
- bool is_up_button_pressed(void);
- bool is_down_button_pressed(void);
- bool is_left_button_pressed(void);
- bool is_right_button_pressed(void);
+ void set_led(led_t led);
+ void clear_led(led_t led);
+ void toggle_led(led_t led);
+ void blink_led(led_t led, int period_ms);
+ bool is_button_pressed(pushbutton_t pushbutton);
+ void assign_button_callback(pushbutton_t pushbutton, void (*pb_callback)(void));
// motion manager interface
- void mm_set_period(double period_ms);
+/* void mm_set_period(double period_ms);
unsigned char mm_get_num_servos(void);
void mm_start(void);
void mm_stop(void);
@@ -53,23 +48,13 @@ class CBioloid_Robot
void mm_assign_module(unsigned char servo_id, mm_mode_t mode);
mm_mode_t mm_get_module(unsigned char servo_id);
std::vector<double> mm_get_servo_angles(void);
- double mm_get_servo_angle(unsigned char servo_id);
+ double mm_get_servo_angle(unsigned char servo_id);*/
// motion action interface
- void action_load_page(unsigned char page_id);
+/* void action_load_page(unsigned char page_id);
unsigned char action_get_current_page(void);
void action_start(void);
void action_stop(void);
- bool action_is_page_running(void);
- // IMU interface
- void imu_enable(void);
- void imu_disable(void);
- bool imu_is_accel_present(void);
- bool imu_is_gyro_present(void);
- bool imu_is_compass_present(void);
- std::vector<double> imu_get_accel(void);
- std::vector<double> imu_get_gyro(void);
- std::vector<double> imu_get_compass(void);
- double imu_get_temperature(void);
+ bool action_is_page_running(void);*/
~CBioloid_Robot();
};
diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp
index 9395e023fd5334ecf3b768ff673f4367df0c65ea..db7f35aba248e0995c3febceff9b04abf97617d0 100644
--- a/src/examples/bioloid_robot_test.cpp
+++ b/src/examples/bioloid_robot_test.cpp
@@ -3,67 +3,23 @@
#include <iostream>
-std::string robot_device="A401293W";
+std::string robot_device="/dev/ttyO1";
int main(int argc, char *argv[])
{
- int i=0,num_servos,count=0;
- std::vector<double> angles,accel,gyro,compass;
+ unsigned int i=0;
try{
- CBioloid_Robot tina("Tina",robot_device,115200,192);
-
- 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=1;i<=18;i++)
- {
- tina.mm_enable_servo(i);
- tina.mm_assign_module(i,BIOLOID_MM_ACTION);
- }
- tina.mm_enable_power();
- 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(32);
- 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;
- count++;
- if(count>50)
- {
- tina.action_stop();
- count=0;
- }
- }
+ CBioloid_Robot tina("Tina",robot_device,921600,0x02);
+ tina.blink_led(USER1_LED,1000);
+ sleep(10);
+ tina.clear_led(USER1_LED);
/* for(i=0;i<10;i++)
{
- 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);
+ std::cout << "toggle led" << std::endl;
+ tina.toggle_led(USER1_LED);
+ sleep(1);
}*/
-// tina.imu_disable();
- tina.mm_stop();
- tina.mm_disable_power();
}catch(CException &e){
std::cout << e.what() << std::endl;
}