From baf222a3cdc2ec9bb9c37480f10564348f1db9d5 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Tue, 28 Jan 2020 00:38:27 +0100 Subject: [PATCH] Updated the mmeory map. Implemented the functions of the motion manager and action interfaces. --- src/darwin_registers.h | 16 +++--- src/darwin_robot.cpp | 114 +++++++++++++++++------------------------ src/darwin_robot.h | 7 ++- 3 files changed, 60 insertions(+), 77 deletions(-) diff --git a/src/darwin_registers.h b/src/darwin_registers.h index 471329c..a86f6ff 100644 --- a/src/darwin_registers.h +++ b/src/darwin_registers.h @@ -11,19 +11,23 @@ #define MANAGER_RUNNING 0x40 #define MANAGER_SCANNING 0x80 #define MANAGER_NUM_DEVICES 131 +#define MANAGER_PRESENT_DEVICES 132 #define MMANAGER_PERIOD 17 #define MMANAGER_OFFSETS 18 -#define MMANAGER_NUM_MODELS 132 -#define MMANAGER_NUM_DEVICES 133 -#define MMANAGER_ENABLE 134 +#define MMANAGER_NUM_MODELS 136 +#define MMANAGER_NUM_DEVICES 137 +#define MMANAGER_PRESENT_SERVOS 138 +#define MMANAGER_ENABLE 142 #define MMANAGER_EVEN_SER_EN 0x80 #define MMANAGER_EVEN_SER_MOD 0x70 #define MMANAGER_ODD_SER_EN 0x08 #define MMANAGER_ODD_SER_MOD 0x07 -#define ACTION_NUM_MODULES 150 -#define ACTION_NUM_DEVICES 151 -#define ACTION_CONTROL 152 +#define ACTION_CONTROL 158 + #define ACTION_START 0x01 + #define ACTION_STOP 0x02 + #define ACTION_STATUS 0x80 +#define ACTION_PAGE_ID 159 #endif diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp index 1304851..10cfe2c 100644 --- a/src/darwin_robot.cpp +++ b/src/darwin_robot.cpp @@ -471,7 +471,7 @@ unsigned int CDarwinRobot::managet_get_num_masters(void) if(this->robot_device==NULL) throw CDarwinRobotException(_HERE_,"Invalid robot device"); - this->robot_device->read_byte_register(MANAGER_PERIOD,&num); + this->robot_device->read_byte_register(MANAGER_NUM_MASTERS,&num); return num; } @@ -526,7 +526,33 @@ unsigned char CDarwinRobot::manager_get_num_devices(void) return num; } +unsigned int CDarwinRobot::manager_get_present_devices(void) +{ + unsigned int present_devices; + + if(this->robot_device!=NULL) + { + this->robot_device->read_registers(MANAGER_PRESENT_DEVICES,(unsigned char *)&present_devices,4); + return present_devices; + } + else + throw CDarwinRobotException(_HERE_,"Invalid robot device"); +} + /* motion manager interface */ +unsigned int CDarwinRobot::mm_get_present_servos(void) +{ + unsigned int present_servos; + + if(this->robot_device!=NULL) + { + this->robot_device->read_registers(MMANAGER_PRESENT_SERVOS,(unsigned char *)&present_servos,4); + return present_servos; + } + else + throw CDarwinRobotException(_HERE_,"Invalid robot device"); +} + void CDarwinRobot::mm_set_period(double period_s) { unsigned char period; @@ -697,19 +723,6 @@ mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id) } /* -unsigned int CDarwinRobot::mm_get_present_servos(void) -{ - unsigned int present_servos; - - if(this->robot_device!=NULL) - { - this->robot_device->read_registers(DARWIN_MM_PRESENT_SERVOS1,(unsigned char *)&present_servos,4); - return present_servos; - } - else - throw CDarwinRobotException(_HERE_,"Invalid robot device"); -} - void CDarwinRobot::mm_enable_balance(void) { if(this->robot_device!=NULL) @@ -981,49 +994,24 @@ void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double } */ // motion action interface -unsigned int CDarwinRobot::action_get_num_models(void) -{ - unsigned char num; - - if(this->robot_device==NULL) - throw CDarwinRobotException(_HERE_,"Invalid robot device"); - this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num); - return num; -} - -unsigned int CDarwinRobot::action_get_num_devices(void) -{ - unsigned char num; - - if(this->robot_device==NULL) - throw CDarwinRobotException(_HERE_,"Invalid robot device"); - this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num); - return num; -} - -/* void CDarwinRobot::action_load_page(unsigned char page_id) { - if(this->robot_device!=NULL) - this->robot_device->write_byte_register(DARWIN_ACTION_PAGE,page_id); - else + if(this->robot_device==NULL) throw CDarwinRobotException(_HERE_,"Invalid robot device"); + this->robot_device->write_byte_register(ACTION_PAGE_ID,page_id); } unsigned char CDarwinRobot::action_get_current_page(void) { unsigned char page_id; - if(this->robot_device!=NULL) - { - this->robot_device->read_byte_register(DARWIN_ACTION_PAGE,&page_id); - - return page_id; - } - else + if(this->robot_device==NULL) throw CDarwinRobotException(_HERE_,"Invalid robot device"); + this->robot_device->read_byte_register(ACTION_PAGE_ID,&page_id); + return page_id; } +/* unsigned char CDarwinRobot::action_get_current_step(void) { unsigned char step_id; @@ -1037,45 +1025,37 @@ unsigned char CDarwinRobot::action_get_current_step(void) else throw CDarwinRobotException(_HERE_,"Invalid robot device"); } +*/ void CDarwinRobot::action_start(void) { - if(this->robot_device!=NULL) - { - this->action_status|=ACTION_START; - this->robot_device->write_byte_register(DARWIN_ACTION_CNTRL,this->action_status); - } - else + if(this->robot_device==NULL) throw CDarwinRobotException(_HERE_,"Invalid robot device"); + this->action_status|=ACTION_START; + this->robot_device->write_byte_register(ACTION_CONTROL,this->action_status); } void CDarwinRobot::action_stop(void) { - if(this->robot_device!=NULL) - { - this->action_status|=ACTION_STOP; - this->robot_device->write_byte_register(DARWIN_ACTION_CNTRL,this->action_status); - } - else + if(this->robot_device==NULL) throw CDarwinRobotException(_HERE_,"Invalid robot device"); + this->action_status|=ACTION_STOP; + this->robot_device->write_byte_register(ACTION_CONTROL,this->action_status); } bool CDarwinRobot::action_is_page_running(void) { unsigned char status; - if(this->robot_device!=NULL) - { - this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&status); - if(status&ACTION_STATUS) - return true; - else - return false; - } - else + if(this->robot_device==NULL) throw CDarwinRobotException(_HERE_,"Invalid robot device"); + this->robot_device->read_byte_register(ACTION_CONTROL,&status); + if(status&ACTION_STATUS) + return true; + else + return false; } -*/ + /* walking interface */ /* void CDarwinRobot::walk_set_x_offset(double offset_m) diff --git a/src/darwin_robot.h b/src/darwin_robot.h index 1dcd71f..f2a4173 100644 --- a/src/darwin_robot.h +++ b/src/darwin_robot.h @@ -10,7 +10,7 @@ extern const std::string servo_names[MAX_NUM_SERVOS]; /* available motion modules */ -typedef enum {DARWIN_MM_NONE=0x00,DARWIN_MM_ACTION=0x01,DARWIN_MM_WALKING=0x02,DARWIN_MM_JOINTS=0x03,DARWIN_MM_HEAD=0x04,DARWIN_MM_GRIPPER=0x05} mm_mode_t; +typedef enum {DARWIN_MM_NONE=0x07,DARWIN_MM_ACTION=0x00,DARWIN_MM_WALKING=0x01,DARWIN_MM_JOINTS=0x02,DARWIN_MM_HEAD=0x03,DARWIN_MM_GRIPPER=0x04} mm_mode_t; /* available grippers */ typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t; /* available leds */ @@ -111,7 +111,9 @@ class CDarwinRobot void manager_start_scan(void); bool manager_is_scanning(void); unsigned char manager_get_num_devices(void); + unsigned int manager_get_present_devices(void); // motion manager interface + unsigned int mm_get_present_servos(void); void mm_set_period(double period_s); double mm_get_period(void); void mm_enable_servo(unsigned char servo_id); @@ -120,7 +122,6 @@ class CDarwinRobot 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); -// unsigned int mm_get_present_servos(void); // void mm_enable_balance(void); // void mm_disable_balance(void); // bool mm_is_balance_enabled(void); @@ -141,8 +142,6 @@ class CDarwinRobot // 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 - unsigned int action_get_num_models(void); - unsigned int action_get_num_devices(void); void action_load_page(unsigned char page_id); unsigned char action_get_current_page(void); unsigned char action_get_current_step(void); -- GitLab