diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp index 27c20ef0bcc83a6bb0ed2dbc0e19dbf1948438b7..acdd3372954cbebbd15d176a2a72e061f4978ce6 100644 --- a/src/darwin_robot.cpp +++ b/src/darwin_robot.cpp @@ -590,6 +590,40 @@ bool CDarwinRobot::mm_is_power_enabled(void) throw CDarwinRobotException(_HERE_,"Invalid robot device"); } +bool CDarwinRobot::mm_has_fallen(void) +{ + unsigned char value; + + if(this->robot_device!=NULL) + { + this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value); + if(value&(MANAGER_FWD_FALL | MANAGER_BWD_FALL)) + return true; + else + return false; + } + else + throw CDarwinRobotException(_HERE_,"Invalid robot device"); +} + +fall_t CDarwinRobot::mm_get_fallen_position(void) +{ + unsigned char value; + + if(this->robot_device!=NULL) + { + this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value); + if(value&MANAGER_FWD_FALL) + return MM_FWD_FALL; + else if(value&MANAGER_BWD_FALL) + return MM_BWD_FALL; + else + return MM_STANDING; + } + else + throw CDarwinRobotException(_HERE_,"Invalid robot device"); +} + void CDarwinRobot::mm_enable_servo(unsigned char servo_id) { unsigned char value; diff --git a/src/darwin_robot.h b/src/darwin_robot.h index e98f70979a833f1dd1115c8bc201f610b546e2b5..45a925dd0e7989a557085513479fe6749db0888c 100644 --- a/src/darwin_robot.h +++ b/src/darwin_robot.h @@ -50,6 +50,8 @@ typedef struct }TChargerData; #pragma pack (pop) +typedef enum {MM_FWD_FALL=0,MM_BWD_FALL=1,MM_STANDING=2} fall_t; + class CDarwinRobot { private: @@ -109,6 +111,8 @@ class CDarwinRobot void mm_enable_power(void); void mm_disable_power(void); bool mm_is_power_enabled(void); + bool mm_has_fallen(void); + fall_t mm_get_fallen_position(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);