diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 5c5b556409fcaf0d1d726c023ed6103531d37fb1..cd4b704523e56b739992dcbee6b8470571df5d7d 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -68,8 +68,12 @@ 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);
+ /* wait for the firmware to finish the scan */
+ do{
+ /* get the current manager status */
+ this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
+ usleep(100000);
+ }while(this->manager_status&MANAGER_SCANNING);
}
else
{
@@ -414,7 +418,7 @@ void CDarwinRobot::mm_set_period(double period_ms)
if(this->robot_device!=NULL)
{
- period=(period_ms/1000.0);
+ period=(period_ms*1000.0);
this->robot_device->write_word_register(DARWIN_MM_PERIOD_L,period);
}
else
@@ -428,7 +432,7 @@ double CDarwinRobot::mm_get_period(void)
if(this->robot_device!=NULL)
{
this->robot_device->read_word_register(DARWIN_MM_PERIOD_L,&period);
- return ((double)period)*1000.0;
+ return ((double)period)/1000.0;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index cc4ce3dca92caa8b5842da5d5f0090701dd174af..71aa7af0b2993f0f7fe2f1c222d3b53b7f573659 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,7 +1,7 @@
# create an example application
-#ADD_EXECUTABLE(darwin_robot_test darwin_robot_test.cpp)
+ADD_EXECUTABLE(darwin_manager_test darwin_manager_test.cpp)
# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_robot_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_manager_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_gpio_test darwin_gpio_test.cpp)
diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ca2edb853b8df2ed221220977fa52cd0019a9611
--- /dev/null
+++ b/src/examples/darwin_manager_test.cpp
@@ -0,0 +1,93 @@
+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+
+#include <iostream>
+
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
+std::string config_file="../src/xml/darwin_action.xml";
+
+int main(int argc, char *argv[])
+{
+ int i=0,num_servos;
+ std::vector<int> servos;
+ std::vector<double> angles,speeds,accels,offsets;
+
+ try{
+ CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+ darwin.mm_load_config(config_file);
+ num_servos=darwin.mm_get_num_servos();
+ std::cout << "Found " << num_servos << " servos" << std::endl;
+ std::cout << "Motion manager period " << darwin.mm_get_period() << " ms" << std::endl;
+ // execute the walk ready action
+ std::cout << "Assign servos to the action module" << std::endl;
+ for(i=1;i<=20;i++)
+ {
+ darwin.mm_enable_servo(i);
+ darwin.mm_assign_module(i,DARWIN_MM_ACTION);
+ }
+ for(i=0;i<=MAX_NUM_SERVOS;i++)
+ {
+ if(darwin.mm_is_servo_enabled(i))
+ std::cout << "Servo " << i << " enabled" << std::endl;
+ else
+ std::cout << "Servo " << i << " disabled" << std::endl;
+ std::cout << "Servo " << i;
+ switch(darwin.mm_get_module(i))
+ {
+ case DARWIN_MM_NONE: std::cout << " not assigned to any module" << std::endl;
+ break;
+ case DARWIN_MM_ACTION: std::cout << " assigned to the action module" << std::endl;
+ break;
+ case DARWIN_MM_WALKING: std::cout << " assigned to the walking module" << std::endl;
+ break;
+ case DARWIN_MM_JOINTS: std::cout << " assigned to the joints module" << std::endl;
+ break;
+ case DARWIN_MM_HEAD: std::cout << " assigned to the head module" << std::endl;
+ break;
+ default: std::cout << " assigned to an unknown module" << std::endl;
+ break;
+ }
+ }
+ if(darwin.mm_is_running())
+ std::cout << "Motion manager is running" << std::endl;
+ else
+ std::cout << "Motion manager is not running" << std::endl;
+ darwin.mm_start();
+ if(darwin.mm_is_running())
+ std::cout << "Motion manager is running" << std::endl;
+ else
+ std::cout << "Motion manager is not running" << std::endl;
+ sleep(10);
+ darwin.mm_stop();
+ if(darwin.mm_is_running())
+ std::cout << "Motion manager is running" << std::endl;
+ else
+ std::cout << "Motion manager is not running" << std::endl;
+ if(darwin.mm_is_power_enabled())
+ std::cout << "Servos are powered" << std::endl;
+ else
+ std::cout << "Servos are not powered" << std::endl;
+ darwin.mm_enable_power();
+ if(darwin.mm_is_power_enabled())
+ std::cout << "Servos are powered" << std::endl;
+ else
+ std::cout << "Servos are not powered" << std::endl;
+ darwin.mm_disable_power();
+ if(darwin.mm_is_power_enabled())
+ std::cout << "Servos are powered" << std::endl;
+ else
+ std::cout << "Servos are not powered" << std::endl;
+ std::cout << "Servo offsets: " << std::endl;
+ offsets=darwin.mm_get_servo_offsets();
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ std::cout << " Servo " << i << ": " << offsets[i] << std::endl;
+ darwin.mm_set_servo_offset(5,3.3);
+ std::cout << "Servo offsets: " << std::endl;
+ offsets=darwin.mm_get_servo_offsets();
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ std::cout << " Servo " << i << ": " << offsets[i] << std::endl;
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}
diff --git a/src/examples/darwin_robot_test.cpp b/src/examples/darwin_robot_test.cpp
deleted file mode 100644
index 907003eabd12f495a631f8f566d7f858faf72e84..0000000000000000000000000000000000000000
--- a/src/examples/darwin_robot_test.cpp
+++ /dev/null
@@ -1,161 +0,0 @@
-#include "darwin_robot.h"
-#include "darwin_robot_exceptions.h"
-
-#include <iostream>
-
-std::string robot_device="A603LOBS";
-std::string config_file="../src/xml/darwin_action.xml";
-
-int main(int argc, char *argv[])
-{
- int i=0,j=0,num_v1_servos,num_v2_servos;
- std::vector<int> servos;
- std::vector<double> angles,speeds,accels;
-
- try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
- darwin.mm_load_config(config_file);
- darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
- std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
- // execute the walk ready action
- std::cout << "Assign servos to the action module" << std::endl;
- darwin.mm_enable_power();
- for(i=1;i<=num_v1_servos;i++)
- {
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_ACTION);
- }
- darwin.mm_start();
- // execute an action
- darwin.action_load_page(9);
- darwin.action_start();
- while(darwin.action_is_page_running())
- {
- std::cout << "Executing action ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(100000);
- }
- // sit down
- darwin.action_load_page(15);
- darwin.action_start();
- while(darwin.action_is_page_running())
- {
- std::cout << "Executing action ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(100000);
- }
- // sit up
- darwin.action_load_page(16);
- darwin.action_start();
- while(darwin.action_is_page_running())
- {
- std::cout << "Executing action ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(100000);
- }
- /* walk forward for a while */
- darwin.mm_stop();
- for(i=1;i<=num_v1_servos;i++)
- {
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_WALKING);
- }
- darwin.mm_start();
- darwin.walking_set_speeds(20,0,0);
- darwin.walking_start();
- sleep(2);
- std::cout << "Stop walking ..." << std::endl;
- darwin.walking_stop();
- while(darwin.is_walking())
- {
- std::cout << "Walking ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(100000);
- }
- /* move joints */
- darwin.mm_stop();
- for(i=1;i<=num_v1_servos;i++)
- {
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_JOINTS);
- }
- darwin.mm_start();
- servos.push_back(1);
- servos.push_back(3);
- servos.push_back(5);
- angles.push_back(45.0);
- angles.push_back(45.0);
- angles.push_back(45.0);
- speeds.push_back(10.0);
- speeds.push_back(10.0);
- speeds.push_back(10.0);
- accels.push_back(10.0);
- accels.push_back(10.0);
- accels.push_back(10.0);
- std::cout << "Start joint_motion ..." << std::endl;
- darwin.joints_load(servos,angles,speeds,accels);
- darwin.joints_start();
- while(darwin.are_joints_moving())
- {
- std::cout << "moving joints ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(100000);
- }
- std::cout << "done" << std::endl;
- /* head tracking */
- darwin.mm_stop();
- for(i=19;i<=20;i++)
- {
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_HEAD);
- }
- darwin.mm_start();
- darwin.head_tracking_set_pan_pid(0.04,0.0,0.0);
- darwin.head_tracking_set_tilt_pid(0.04,0.0,0.0);
- darwin.head_tracking_set_target(0.0,90.0);
- darwin.head_tracking_start();
- for(i=0;i<40;i++)
- {
- std::cout << "head tracking ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(10000);
- }
- darwin.head_tracking_stop();
- darwin.mm_stop();
- for(i=1;i<=num_v1_servos;i++)
- {
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_ACTION);
- }
- darwin.mm_start();
- darwin.action_load_page(15);
- darwin.action_start();
- while(darwin.action_is_page_running())
- {
- std::cout << "Executing action ..." << std::endl;
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- usleep(100000);
- }
- /* disable servos and read position */
- darwin.mm_stop();
- for(i=1;i<=num_v1_servos;i++)
- {
- darwin.mm_disable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_NONE);
- }
- darwin.mm_start();
- for(j=0;j<1000;j++)
- {
- angles=darwin.mm_get_servo_angles();
- std::cout << "Period: " << darwin.mm_get_current_period() << " ms" << std::endl;
- for(i=0;i<angles.size();i++)
- std::cout << "Servo " << i << " angle: " << angles[i] << std::endl;
- usleep(100000);
- }
- // enable all servos and assign them to the action module
- darwin.mm_stop();
- darwin.mm_disable_power();
- }catch(CException &e){
- std::cout << e.what() << std::endl;
- }
-}