From f26d94d03091334047ddfb796c0ea6d75aa23112 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 31 Jul 2016 20:17:04 +0200
Subject: [PATCH] Added a function to get the servos present.
---
src/darwin_robot.cpp | 13 +++++++++++++
src/darwin_robot.h | 1 +
src/examples/darwin_action_test.cpp | 9 ++++++++-
3 files changed, 22 insertions(+), 1 deletion(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 87418a6..b3f91af 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -457,6 +457,19 @@ unsigned char CDarwinRobot::mm_get_num_servos(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
+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_start(void)
{
if(this->robot_device!=NULL)
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 0e46bc3..af91ead 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -83,6 +83,7 @@ class CDarwinRobot
void mm_set_period(double period_ms);
double mm_get_period(void);
unsigned char mm_get_num_servos(void);
+ unsigned int mm_get_present_servos(void);
void mm_start(void);
void mm_stop(void);
bool mm_is_running(void);
diff --git a/src/examples/darwin_action_test.cpp b/src/examples/darwin_action_test.cpp
index 7afacc4..740e75f 100644
--- a/src/examples/darwin_action_test.cpp
+++ b/src/examples/darwin_action_test.cpp
@@ -10,15 +10,19 @@ std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
int i=0,num_servos;
+ unsigned int present_servos;
try{
CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
num_servos=darwin.mm_get_num_servos();
+ present_servos=darwin.mm_get_present_servos();
std::cout << "Found " << num_servos << " servos " << std::endl;
+ std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
// enable all servos and assign them to the action module
darwin.mm_enable_power();
- for(i=1;i<=num_servos;i++)
+ sleep(1);
+ for(i=0;i<MAX_NUM_SERVOS;i++)
{
darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_ACTION);
@@ -28,7 +32,10 @@ int main(int argc, char *argv[])
darwin.action_load_page(WALK_READY);
darwin.action_start();
while(darwin.action_is_page_running())
+ {
+ std::cout << "action running ... " << std::endl;
usleep(100000);
+ }
darwin.mm_stop();
darwin.mm_disable_power();
}catch(CException &e){
--
GitLab