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

Added initial support for the motion manager module.

parent 8e471b93
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/bioloid_robot.cpp
+ 231
− 83
View file @ 24b074f5
......@@ -43,6 +43,10 @@ CBioloidRobot::CBioloidRobot(const std::string &name,std::string &serial_dev,int
this->clear_pending_interrupts();
/* get the current zigbee status */
this->robot_device->read_byte_register(BIOLOID_ZIGBEE_CNTRL,&this->zigbee_status);
/* get the current manager status */
this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&this->manager_status);
/* get the number of servos detected */
this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->manager_num_servos);
}catch(CException &e){
if(this->robot_device!=NULL)
this->dyn_server->free_device(id);
......@@ -388,149 +392,283 @@ bool CBioloidRobot::is_zigbee_enabled(void)
}
// motion manager interface
/*void CBioloidRobot::mm_set_period(double period_ms)
void CBioloidRobot::mm_set_period(double period_ms)
{
unsigned short period;
// internal time in 0|16 fixed point float format in seconds
period=(period_ms/1000.0)*65536;
this->robot_device->write_word_register(BIOLOID_MM_PERIOD_LOW,period);
if(this->robot_device!=NULL)
{
// internal time in 0|16 fixed point float format in seconds
period=(period_ms/1000.0)*65536;
this->robot_device->write_word_register(BIOLOID_MM_PERIOD_L,period);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
double CBioloidRobot::mm_get_period(void)
{
unsigned short period;
if(this->robot_device!=NULL)
{
this->robot_device->read_word_register(BIOLOID_MM_PERIOD_L,&period);
return (((double)period)*1000.0)/65536;
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
unsigned char CBioloidRobot::mm_get_num_servos(void)
{
this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->num_servos);
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->manager_num_servos);
return this->num_servos;
return this->manager_num_servos;
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_start(void)
{
this->robot_device->write_byte_register(BIOLOID_MM_CONTROL,0x01);
if(this->robot_device!=NULL)
{
this->manager_status|=0x01;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_stop(void)
{
this->robot_device->write_byte_register(BIOLOID_MM_CONTROL,0x00);
if(this->robot_device!=NULL)
{
this->manager_status&=0xFE;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
bool CBioloidRobot::mm_is_running(void)
{
unsigned char value;
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&value);
if(value&0x01)
return true;
else
return false;
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_enable_balance(void)
{
if(this->robot_device!=NULL)
{
this->manager_status|=0x02;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_disable_balance(void)
{
if(this->robot_device!=NULL)
{
this->manager_status&=0xFE;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
bool CBioloidRobot::mm_is_balance_enabled(void)
{
unsigned char value;
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&value);
if(value&0x02)
return true;
else
return false;
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_enable_power(void)
{
this->robot_device->write_byte_register(BIOLOID_DXL_POWER,0x01);
if(this->robot_device!=NULL)
{
this->manager_status|=0x04;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_disable_power(void)
{
this->robot_device->write_byte_register(BIOLOID_DXL_POWER,0x00);
if(this->robot_device!=NULL)
{
this->manager_status|=0xFB;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
bool CBioloidRobot::mm_is_power_enabled(void)
{
unsigned char value;
this->robot_device->read_byte_register(BIOLOID_DXL_POWER,&value);
if(value==0x01)
return true;
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&value);
if(value&0x04)
return true;
else
return false;
}
else
return false;
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_enable_servo(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
value|=0x08;
else// even servo
value|=0x80;
this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
value|=0x08;
else// even servo
value|=0x80;
this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_disable_servo(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
value&=0xF7;
else// even servo
value&=0x7F;
this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
value&=0xF7;
else// even servo
value&=0x7F;
this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
bool CBioloidRobot::mm_is_servo_enabled(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
{
if(value&0x01)
return true;
else
return false;
}
else// even servo
if(this->robot_device!=NULL)
{
if(value&0x10)
return true;
else
return false;
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
{
if(value&0x08)
return true;
else
return false;
}
else// even servo
{
if(value&0x80)
return true;
else
return false;
}
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
void CBioloidRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
{
value&=0xF8;
value|=(unsigned char)mode;
}
else// even servo
if(this->robot_device!=NULL)
{
value&=0x8F;
value|=(((unsigned char)mode)<<4);
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
{
value&=0xF8;
value|=(unsigned char)mode;
}
else// even servo
{
value&=0x8F;
value|=(((unsigned char)mode)<<4);
}
this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
}
this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
mm_mode_t CBioloidRobot::mm_get_module(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
if(servo_id>MAX_NUM_SERVOS)
{
throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
return (mm_mode_t)(value&0x07);
else
return (mm_mode_t)((value&0x70)>>4);
}
// get the current value
this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
return (mm_mode_t)(value&0x07);
else
return (mm_mode_t)((value&0x70)>>4);
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
std::vector<double> CBioloidRobot::mm_get_servo_angles(void)
......@@ -539,12 +677,17 @@ std::vector<double> CBioloidRobot::mm_get_servo_angles(void)
short int values[MAX_NUM_SERVOS];
std::vector<double> angles(MAX_NUM_SERVOS);
this->robot_device->read_registers(BIOLOID_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
if(this->robot_device!=NULL)
{
this->robot_device->read_registers(BIOLOID_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
for(i=0;i<MAX_NUM_SERVOS;i++)
angles[i]=((double)values[i])/128.0;
for(i=0;i<MAX_NUM_SERVOS;i++)
angles[i]=((double)values[i])/128.0;
return angles;
return angles;
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
......@@ -552,15 +695,20 @@ double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
unsigned short int value;
double angle;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
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;
if(servo_id>MAX_NUM_SERVOS)
{
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;
}*/
return angle;
}
else
throw CBioloidRobotException(_HERE_,"Invalid robot device");
}
// motion action interface
/*void CBioloidRobot::action_load_page(unsigned char page_id)
......
src/bioloid_robot.h
+ 12
− 2
View file @ 24b074f5
......@@ -10,6 +10,8 @@
#include "threadserver.h"
#include "eventserver.h"
#define MAX_NUM_SERVOS 31
/* 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 */
......@@ -47,6 +49,9 @@ class CBioloidRobot
CFunctor *mode_pb;
/* zigbee status */
unsigned char zigbee_status;
/* motion manager variables */
unsigned char manager_status;
unsigned char manager_num_servos;
protected:
static void *ext_int_thread(void *param);
void clear_pending_interrupts(void);
......@@ -133,10 +138,15 @@ class CBioloidRobot
void zigbee_disable(void);
bool is_zigbee_enabled(void);
// motion manager interface
/* void mm_set_period(double period_ms);
void mm_set_period(double period_ms);
double mm_get_period(void);
unsigned char mm_get_num_servos(void);
void mm_start(void);
void mm_stop(void);
bool mm_is_running(void);
void mm_enable_balance(void);
void mm_disable_balance(void);
bool mm_is_balance_enabled(void);
void mm_enable_power(void);
void mm_disable_power(void);
bool mm_is_power_enabled(void);
......@@ -146,7 +156,7 @@ class CBioloidRobot
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);
unsigned char action_get_current_page(void);
......
src/examples/bioloid_robot_test.cpp
+ 4
− 3
View file @ 24b074f5
......@@ -75,9 +75,10 @@ int main(int argc, char *argv[])
std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl;
usleep(100000);
}*/
tina.zigbee_enable_power();
sleep(10);
tina.zigbee_disable_power();
/* tina.zigbee_enable_power();
sleep(3);
tina.zigbee_disable_power();*/
std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
}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