From 2a964435f43e08439647289cc9a70f1be262e261 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Tue, 23 Aug 2016 23:40:13 +0200
Subject: [PATCH] Removed the head_tracking_status attribute. Removed some
unnecessary cout's. Enhanced some of the examples to fully test all the
features.
---
src/darwin_robot.cpp | 20 +--------
src/darwin_robot.h | 2 -
src/examples/darwin_gpio_test.cpp | 21 +++++++++-
src/examples/darwin_head_tracking_test.cpp | 14 +++++--
src/examples/darwin_joint_motion_test.cpp | 49 ++++++++--------------
src/examples/darwin_walking_test.cpp | 37 +++++++++++++---
6 files changed, 79 insertions(+), 64 deletions(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 3508488..578a6cb 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -80,8 +80,6 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
/* get the current action status */
this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
- /* get the current head tracking status */
- this->robot_device->read_byte_register(DARWIN_HEAD_CNTRL,&this->head_tracking_status);
}
else
{
@@ -1120,7 +1118,6 @@ void CDarwinRobot::walk_set_period(double period_s)
if(this->robot_device!=NULL)
{
period=(unsigned short int)(period_s*1000.0);
- std::cout << period_s << "," << period << std::endl;
this->robot_device->write_word_register(DARWIN_WALK_PERIOD_TIME_L,period);
}
else
@@ -1712,24 +1709,14 @@ void CDarwinRobot::head_get_tilt_range(double *max,double *min)
void CDarwinRobot::head_start_tracking(double pan,double tilt)
{
unsigned short int value;
- unsigned char data[128];
if(this->robot_device!=NULL)
{
value=pan*128.0;
- std::cout << "pan value:" << value << std::endl;
this->robot_device->write_word_register(DARWIN_HEAD_PAN_TARGET_L,value);
value=tilt*128.0;
- std::cout << "tilt value:" << value << std::endl;
this->robot_device->write_word_register(DARWIN_HEAD_TILT_TARGET_L,value);
- this->head_tracking_status|=HEAD_START;
- this->robot_device->write_byte_register(DARWIN_HEAD_CNTRL,this->head_tracking_status);
- this->robot_device->read_registers(DARWIN_HEAD_PAN_P_L,data,16);
- for(unsigned int i=0;i<16;i++)
- std::cout << "register: " << std::dec << (int)(DARWIN_HEAD_PAN_P_L + i) << std::hex << (int)data[i] << std::endl;
- this->robot_device->read_registers(DARWIN_HEAD_CNTRL,data,13);
- for(unsigned int i=0;i<13;i++)
- std::cout << "register: " << std::dec << (int)(DARWIN_HEAD_CNTRL + i) << std::hex << (int)data[i] << std::endl;
+ this->robot_device->write_byte_register(DARWIN_HEAD_CNTRL,HEAD_START);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1738,10 +1725,7 @@ void CDarwinRobot::head_start_tracking(double pan,double tilt)
void CDarwinRobot::head_stop_tracking(void)
{
if(this->robot_device!=NULL)
- {
- this->head_tracking_status|=HEAD_STOP;
- this->robot_device->write_byte_register(DARWIN_HEAD_CNTRL,this->head_tracking_status);
- }
+ this->robot_device->write_byte_register(DARWIN_HEAD_CNTRL,HEAD_STOP);
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 51343b1..597128f 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -52,8 +52,6 @@ class CDarwinRobot
unsigned char manager_status;
/* action status */
unsigned char action_status;
- /* head tracking status */
- unsigned char head_tracking_status;
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
diff --git a/src/examples/darwin_gpio_test.cpp b/src/examples/darwin_gpio_test.cpp
index 85d6c6d..e7a0f91 100644
--- a/src/examples/darwin_gpio_test.cpp
+++ b/src/examples/darwin_gpio_test.cpp
@@ -3,11 +3,13 @@
#include <iostream>
-//std::string robot_device="A603LOBS";
-std::string robot_device="A4008atn";
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
+ unsigned int i=0;
+
try{
CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
std::cout << "found darwin controller" << std::endl;
@@ -18,6 +20,21 @@ int main(int argc, char *argv[])
darwin.gpio_set_led(LED_4);
darwin.gpio_aux1_color(1.0,1.0,1.0);
darwin.gpio_aux2_color(0.5,0.5,0.5);
+ for(i=0;i<100;i++)
+ {
+ if(darwin.is_button_pressed(START_PB))
+ std::cout << "start button pressed" << std::endl;
+ else
+ std::cout << "start button not pressed" << std::endl;
+ usleep(100000);
+ }
+ darwin.gpio_blink_led(LED_TX,0);
+ darwin.gpio_blink_led(LED_RX,0);
+ darwin.gpio_blink_led(LED_2,0);
+ darwin.gpio_blink_led(LED_3,0);
+ darwin.gpio_clear_led(LED_4);
+ darwin.gpio_aux1_color(0.0,0.0,0.0);
+ darwin.gpio_aux2_color(0.0,0.0,0.0);
sleep(10);
}catch(CException &e){
std::cout << e.what() << std::endl;
diff --git a/src/examples/darwin_head_tracking_test.cpp b/src/examples/darwin_head_tracking_test.cpp
index c26b321..ad7b46f 100644
--- a/src/examples/darwin_head_tracking_test.cpp
+++ b/src/examples/darwin_head_tracking_test.cpp
@@ -3,8 +3,8 @@
#include <iostream>
-//std::string robot_device="A603LOBS";
-std::string robot_device="A4008atn";
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
@@ -30,9 +30,15 @@ int main(int argc, char *argv[])
darwin.mm_assign_module(19,DARWIN_MM_HEAD);
darwin.mm_start();
darwin.head_start_tracking(45.0,90.0);
- sleep(10);
+ sleep(3);
darwin.head_set_new_target(0.0,0.0);
- sleep(10);
+ sleep(3);
+ darwin.head_stop_tracking();
+ sleep(1);
+ darwin.head_start_tracking(45.0,90.0);
+ sleep(3);
+ darwin.head_set_new_target(0.0,0.0);
+ sleep(3);
darwin.mm_stop();
darwin.mm_disable_power();
}catch(CException &e){
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index 7fa8fa9..20a39c8 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -3,8 +3,8 @@
#include <iostream>
-//std::string robot_device="A603LOBS";
-std::string robot_device="A4008atn";
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
@@ -35,42 +35,27 @@ int main(int argc, char *argv[])
angles.clear();
speeds.clear();
accels.clear();
- for(i=0;i<MAX_NUM_SERVOS;i++)
- {
- if(present_servos&(0x00000001<<i))
- {
- servos.push_back(i);
- angles.push_back(45.0);
- speeds.push_back(10.0*j);
- accels.push_back(10.0*j);
- }
- }
+ servos.push_back(19);
+ angles.push_back(45.0);
+ speeds.push_back(50.0);
+ accels.push_back(50.0);
std::cout << "Start joint_motion ..." << std::endl;
- darwin.joints_load((joints_grp_t)j,servos,angles,speeds,accels);
- darwin.joints_start((joints_grp_t)j);
- while(darwin.joints_are_moving((joints_grp_t)j))
- {
- std::cout << "moving joints ..." << std::endl;
- usleep(100000);
- }
+ darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels);
+ darwin.joints_start(JOINTS_GRP0);
+ sleep(2);
+ darwin.joints_stop(JOINTS_GRP0);
std::cout << "done" << std::endl;
servos.clear();
angles.clear();
speeds.clear();
accels.clear();
- for(i=0;i<MAX_NUM_SERVOS;i++)
- {
- if(present_servos&(0x00000001<<i))
- {
- servos.push_back(i);
- angles.push_back(-45.0);
- speeds.push_back(10.0*j);
- accels.push_back(10.0*j);
- }
- }
- darwin.joints_load((joints_grp_t)j,servos,angles,speeds,accels);
- darwin.joints_start((joints_grp_t)j);
- while(darwin.joints_are_moving((joints_grp_t)j))
+ servos.push_back(19);
+ angles.push_back(-45.0);
+ speeds.push_back(50.0);
+ accels.push_back(50.0);
+ darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels);
+ darwin.joints_start(JOINTS_GRP0);
+ while(darwin.joints_are_moving(JOINTS_GRP0))
{
std::cout << "moving joints ..." << std::endl;
usleep(100000);
diff --git a/src/examples/darwin_walking_test.cpp b/src/examples/darwin_walking_test.cpp
index 5f474f4..acd149a 100644
--- a/src/examples/darwin_walking_test.cpp
+++ b/src/examples/darwin_walking_test.cpp
@@ -4,21 +4,25 @@
#include <iostream>
-//std::string robot_device="A603LOBS";
-std::string robot_device="A4008atn";
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
int i=0,num_servos;
std::vector<double> angles;
+ 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);
@@ -29,12 +33,33 @@ int main(int argc, char *argv[])
darwin.action_start();
while(darwin.action_is_page_running())
usleep(100000);
- for(i=1;i<=num_servos;i++)
+ std::cout << "starting calibration" << std::endl;
+ darwin.imu_start_gyro_cal();
+ while(darwin.imu_is_gyro_cal_done())
+ usleep(100000);
+ std::cout << "calibration done" << std::endl;
+ darwin.mm_enable_balance();
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ {
+ darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_WALKING);
+ }
std::cout << "Start walking ..." << std::endl;
- darwin.walk_set_x_step(0.02);
+ darwin.walk_set_x_step(0.0);
+ darwin.walk_set_y_step(0.0);
+ darwin.walk_set_turn_step(0.01);
+ darwin.walk_start();
+ sleep(2);
+ std::cout << "Stop walking ..." << std::endl;
+ darwin.walk_stop();
+ while(darwin.is_walking())
+ {
+ std::cout << "Walking ..." << std::endl;
+ usleep(100000);
+ }
+ darwin.walk_set_x_step(0.0);
darwin.walk_set_y_step(0.0);
- darwin.walk_set_turn_step(0.0);
+ darwin.walk_set_turn_step(0.01);
darwin.walk_start();
sleep(2);
std::cout << "Stop walking ..." << std::endl;
--
GitLab