Skip to content
Snippets Groups Projects
Commit 62e1414a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Integrated the GPIO (only LED's) into the dynamixel interface.

parent 5f21bc6c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 4
− 0
View file @ 62e1414a
......@@ -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
......
src/bioloid_registers.h deleted 100644 → 0
+ 0
− 126
View file @ 5f21bc6c
#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
src/bioloid_robot.cpp
+ 76
− 268
View file @ 62e1414a
......@@ -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);
}
src/bioloid_robot.h
+ 24
− 39
View file @ 62e1414a
#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();
};
......
src/examples/bioloid_robot_test.cpp
+ 9
− 53
View file @ 62e1414a
......@@ -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;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment