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

Added the API for the motion manager interface.

parent f1c046a3
No related branches found
No related tags found
No related merge requests found
......@@ -34,7 +34,11 @@ const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
std::string("Servo23"),
std::string("Servo24"),
std::string("Servo25"),
std::string("Servo26")};
std::string("Servo26"),
std::string("Servo27"),
std::string("Servo28"),
std::string("Servo29"),
std::string("Servo30")};
const led_info_t leds[GPIO_NUM_LEDS]={{DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
{DARWIN_RX_LED_CNTRL,DARWIN_RX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
......@@ -64,6 +68,8 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
this->dyn_server->config_bus(bus_id,bus_speed);
this->robot_device=dyn_server->get_device(id,dyn_version2);
this->robot_id=id;
/* get the current manager status */
this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
}
else
{
......@@ -246,7 +252,6 @@ double CDarwinRobot::adc_get_temperature(void)
if(this->robot_device!=NULL)
{
this->robot_device->read_word_register(DARWIN_ADC_TEMP_L,&value);
std::cout << std::hex << value << std::endl;
return ((double)value)/((double)(1<<10));
}
......@@ -403,194 +408,258 @@ void CDarwinRobot::imu_get_gyro_data(double *x,double *y,double *z)
}
// motion manager interface
/*void CDarwinRobot::mm_set_period(double period_ms)
void CDarwinRobot::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(DARWIN_MM_PERIOD_LOW,period);
if(this->robot_device!=NULL)
{
period=(period_ms/1000.0);
this->robot_device->write_word_register(DARWIN_MM_PERIOD_L,period);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
double CDarwinRobot::mm_get_period(void)
{
unsigned short period;
this->robot_device->read_word_register(DARWIN_MM_PERIOD_LOW,&period);
return (((double)period)*1000.0)/65536;
if(this->robot_device!=NULL)
{
this->robot_device->read_word_register(DARWIN_MM_PERIOD_L,&period);
return ((double)period)*1000.0;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
double CDarwinRobot::mm_get_current_period(void)
unsigned char CDarwinRobot::mm_get_num_servos(void)
{
unsigned short period;
this->robot_device->read_word_register(DARWIN_MM_CUR_PERIOD,&period);
return ((double)period)/1000.0;
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->num_servos);
return this->num_servos;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_get_num_servos(int *v1_servos,int *v2_servos)
void CDarwinRobot::mm_start(void)
{
unsigned char data;
this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&data);
*v1_servos=data&0x1F;
*v2_servos=(data>>5)&0x07;
if(this->robot_device!=NULL)
{
this->manager_status|=MANAGER_ENABLE;
this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_start(void)
void CDarwinRobot::mm_stop(void)
{
unsigned char status;
this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
status|=0x01;
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
if(this->robot_device!=NULL)
{
this->manager_status&=(~MANAGER_ENABLE);
this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_stop(void)
bool CDarwinRobot::mm_is_running(void)
{
unsigned char status;
unsigned char value;
this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
status&=0xFE;
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
if(value&MANAGER_ENABLE)
return true;
else
return false;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_enable_power(void)
void CDarwinRobot::mm_enable_balance(void)
{
this->robot_device->write_byte_register(DARWIN_DXL_POWER,0x01);
if(this->robot_device!=NULL)
{
this->manager_status|=MANAGER_EN_BAL;
this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_disable_power(void)
void CDarwinRobot::mm_disable_balance(void)
{
this->robot_device->write_byte_register(DARWIN_DXL_POWER,0x00);
sleep(1);
if(this->robot_device!=NULL)
{
this->manager_status&=(~MANAGER_EN_BAL);
this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
bool CDarwinRobot::mm_is_power_enabled(void)
bool CDarwinRobot::mm_is_balance_enabled(void)
{
unsigned char value;
this->robot_device->read_byte_register(DARWIN_DXL_POWER,&value);
if(value==0x01)
return true;
if(this->robot_device!=NULL)
{
this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
if(value&MANAGER_EN_BAL)
return true;
else
return false;
}
else
return false;
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
void CDarwinRobot::mm_enable_power(void)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
// handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
this->manager_status|=MANAGER_EN_PWR;
this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
}
// get the current value
this->robot_device->read_byte_register(DARWIN_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(DARWIN_MM_MODULE_EN0+servo_id/2,value);
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_disable_servo(unsigned char servo_id)
void CDarwinRobot::mm_disable_power(void)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
// handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
this->manager_status&=(~MANAGER_EN_PWR);
this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
}
// get the current value
this->robot_device->read_byte_register(DARWIN_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(DARWIN_MM_MODULE_EN0+servo_id/2,value);
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
bool CDarwinRobot::mm_is_servo_enabled(unsigned char servo_id)
bool CDarwinRobot::mm_is_power_enabled(void)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
// handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_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)
this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
if(value&MANAGER_EN_PWR)
return true;
else
return false;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_enable_balance(void)
void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
{
unsigned char status;
unsigned char value;
this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
status|=0x02;
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
if(this->robot_device!=NULL)
{
if(servo_id>MAX_NUM_SERVOS)
{
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
value|=MANAGER_ODD_SER_EN;
else// even servo
value|=MANAGER_EVEN_SER_EN;
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_disable_balance(void)
void CDarwinRobot::mm_disable_servo(unsigned char servo_id)
{
unsigned char status;
unsigned char value;
this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
status&=0xFD;
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
if(this->robot_device!=NULL)
{
if(servo_id>MAX_NUM_SERVOS)
{
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
value&=(~MANAGER_ODD_SER_EN);
else// even servo
value&=(~MANAGER_EVEN_SER_EN);
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
bool CDarwinRobot::mm_is_balance_enabled(void)
bool CDarwinRobot::mm_is_servo_enabled(unsigned char servo_id)
{
unsigned char value;
this->robot_device->read_byte_register(DARWIN_MM_STATUS,&value);
if(value==0x01)
return true;
if(this->robot_device!=NULL)
{
if(servo_id>MAX_NUM_SERVOS)
{
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
{
if(value&MANAGER_ODD_SER_EN)
return true;
else
return false;
}
else// even servo
{
if(value&MANAGER_EVEN_SER_EN)
return true;
else
return false;
}
}
else
return false;
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
// handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_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 CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
{
value&=(~MANAGER_ODD_SER_MOD);
value|=(unsigned char)mode;
}
else// even servo
{
value&=(~MANAGER_EVEN_SER_MOD);
value|=(((unsigned char)mode)<<4);
}
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
}
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_assign_module(std::string &servo,std::string &module)
......@@ -622,17 +691,21 @@ mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
if(this->robot_device!=NULL)
{
// handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
if(servo_id>MAX_NUM_SERVOS)
{
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
if(servo_id%2)// odd servo
return (mm_mode_t)(value&MANAGER_ODD_SER_MOD);
else
return (mm_mode_t)((value&MANAGER_EVEN_SER_MOD)>>4);
}
// get the current value
this->robot_device->read_byte_register(DARWIN_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 CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_load_config(std::string &filename)
......@@ -660,34 +733,94 @@ void CDarwinRobot::mm_load_config(std::string &filename)
std::vector<double> CDarwinRobot::mm_get_servo_angles(void)
{
int i=0;
short int values[MAX_NUM_SERVOS];
short int values[MAX_NUM_SERVOS];
std::vector<double> angles(MAX_NUM_SERVOS);
this->robot_device->read_registers(DARWIN_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
if(this->robot_device!=NULL)
{
this->robot_device->read_registers(DARWIN_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 CDarwinRobotException(_HERE_,"Invalid robot device");
}
double CDarwinRobot::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)
{
if(servo_id>MAX_NUM_SERVOS)
{
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
angle=((double)value)/128.0;
return angle;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_set_servo_offset(unsigned char servo_id,double offset)
{
if(offset<-9.0 || offset>9.0)
throw CDarwinRobotException(_HERE_,"Desired offset out of range");
else
{
/ handle exceptions
if(servo_id>MAX_NUM_SERVOS)
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
else
{
this->robot_device->write_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,(unsigned char)((int)(offset*16.0)));
usleep(10000);
}
}
}
double CDarwinRobot::mm_get_servo_offset(unsigned char servo_id)
{
unsigned char value;
double offset;
if(servo_id>MAX_NUM_SERVOS)
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
else
{
this->robot_device->read_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,&value);
offset=((double)((signed char)value))/16.0;
return offset;
}
this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
angle=((double)value)/128.0;
}
return angle;
std::vector<double> CDarwinRobot::mm_get_servo_offsets(void)
{
int i=0;
signed char values[MAX_NUM_SERVOS];
std::vector<double> offsets(MAX_NUM_SERVOS);
if(this->robot_device!=NULL)
{
this->robot_device->read_registers(DARWIN_MM_SERVO0_OFFSET,(unsigned char *)values,MAX_NUM_SERVOS);
for(i=0;i<MAX_NUM_SERVOS;i++)
offsets[i]=((double)values[i])/16.0;
return offsets;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
/*void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
{
this->robot_device->write_byte_register(DARWIN_MM_KNEE_GAIN,(unsigned char)(knee*64.0));
this->robot_device->write_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,(unsigned char)(ankle_pitch*64.0));
......
......@@ -45,6 +45,7 @@ class CDarwinRobot
unsigned char robot_id;
// motion manager variables
unsigned char num_servos;
unsigned char manager_status;
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
......@@ -77,25 +78,28 @@ class CDarwinRobot
// motion manager interface
void mm_set_period(double period_ms);
double mm_get_period(void);
double mm_get_current_period(void);
void mm_get_num_servos(int *v1_servos,int *v2_servos);
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);
void mm_enable_servo(unsigned char servo_id);
void mm_disable_servo(unsigned char servo_id);
bool mm_is_servo_enabled(unsigned char servo_id);
void mm_enable_balance(void);
void mm_disable_balance(void);
bool mm_is_balance_enabled(void);
void mm_assign_module(unsigned char servo_id, mm_mode_t mode);
void mm_assign_module(std::string &servo,std::string &module);
mm_mode_t mm_get_module(unsigned char servo_id);
void mm_load_config(std::string &filename);
std::vector<double> mm_get_servo_angles(void);
double mm_get_servo_angle(unsigned char servo_id);
void mm_set_servo_offset(unsigned char servo_id,double offset);
double mm_get_servo_offset(unsigned char servo_id);
std::vector<double> mm_get_servo_offsets(void);
void mm_load_config(std::string &filename);
void mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
void mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
// motion action interface
......
......@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
std::cout << "found darwin controller" << std::endl;
darwin.adc_start();
for(i=0;i<1000;i++)
for(i=0;i<50;i++)
{
std::cout << "Temperature: " << darwin.adc_get_temperature() << std::endl;
std::cout << "Vref: " << darwin.adc_get_vrefint() << std::endl;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment