From 5783733a06c444a4a8afeeb0e044e56b28f32797 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 31 May 2017 14:36:49 +0200
Subject: [PATCH] Added support to read the fallen status from the firmware.
---
src/darwin_robot.cpp | 34 ++++++++++++++++++++++++++++++++++
src/darwin_robot.h | 4 ++++
2 files changed, 38 insertions(+)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 27c20ef..acdd337 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 e98f709..45a925d 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);
--
GitLab