From 76fb32e78e6704c09c73e68cb357782a1cdecf27 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 1 Aug 2016 23:16:17 +0200
Subject: [PATCH] Implemented the head tracking functions. Added an example to
test the head tracking feature.
---
src/darwin_robot.cpp | 194 +++++++++++++++++++--
src/darwin_robot.h | 4 +-
src/examples/CMakeLists.txt | 5 +
src/examples/darwin_head_tracking_test.cpp | 25 +--
4 files changed, 202 insertions(+), 26 deletions(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index f33a62d..46401f3 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -85,6 +85,8 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
/* get the current joints status */
for(i=0;i<JOINTS_NUM_GROUPS;i++)
this->robot_device->read_byte_register(DARWIN_JOINT_GRP0_CNTRL+i*5,&this->joints_status[i]);
+ /* get the current head tracking status */
+ this->robot_device->read_byte_register(DARWIN_HEAD_CNTRL,&this->head_tracking_status);
}
else
{
@@ -1581,67 +1583,229 @@ bool CDarwinRobot::joints_are_moving(joints_grp_t group)
// head tracking interface
void CDarwinRobot::head_set_pan_pid(double p,double i,double d,double i_clamp)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=p*65536.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_PAN_P_L,value);
+ usleep(100000);
+ value=i*65536.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_PAN_I_L,value);
+ usleep(100000);
+ value=d*65536.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_PAN_D_L,value);
+ usleep(100000);
+ value=i_clamp*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_PAN_I_CLAMP_L,value);
+ usleep(100000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_get_pan_pid(double *p,double *i,double *d,double *i_clamp)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_HEAD_PAN_P_L,&value);
+ *p=((double)value)/65536.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_PAN_I_L,&value);
+ *i=((double)value)/65536.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_PAN_D_L,&value);
+ *d=((double)value)/65536.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_PAN_I_CLAMP_L,&value);
+ *i_clamp=((double)value)/128.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_set_tilt_pid(double p,double i,double d,double i_clamp)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=p*65536.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_TILT_P_L,value);
+ usleep(100000);
+ value=i*65536.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_TILT_I_L,value);
+ usleep(100000);
+ value=d*65536.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_TILT_D_L,value);
+ usleep(100000);
+ value=i_clamp*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_TILT_I_CLAMP_L,value);
+ usleep(100000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_get_tilt_pid(double *p,double *i,double *d,double *i_clamp)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_HEAD_TILT_P_L,&value);
+ *p=((double)value)/65536.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_TILT_I_L,&value);
+ *i=((double)value)/65536.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_TILT_D_L,&value);
+ *d=((double)value)/65536.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_TILT_I_CLAMP_L,&value);
+ *i_clamp=((double)value)/128.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_set_pan_range(double max,double min)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=max*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_MAX_PAN_L,value);
+ value=min*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_MIN_PAN_L,value);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_get_pan_range(double *max,double *min)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_HEAD_MAX_PAN_L,&value);
+ *max=((double)value)/128.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_MIN_PAN_L,&value);
+ *min=((double)value)/128.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_set_tilt_range(double max,double min)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=max*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_MAX_TILT_L,value);
+ value=min*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_MIN_TILT_L,value);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_get_tilt_range(double *max,double *min)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_HEAD_MAX_TILT_L,&value);
+ *max=((double)value)/128.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_MIN_TILT_L,&value);
+ *min=((double)value)/128.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
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;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
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);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
bool CDarwinRobot::head_is_tracking(void)
-{
-
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_HEAD_CNTRL,&value);
+ if(value&HEAD_STATUS)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::head_set_new_target(double pan,double tilt)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=pan*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_PAN_TARGET_L,value);
+ value=tilt*128.0;
+ this->robot_device->write_word_register(DARWIN_HEAD_TILT_TARGET_L,value);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::head_get_current_target(double pan,double tilt)
+void CDarwinRobot::head_get_current_target(double *pan,double *tilt)
{
-
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_HEAD_PAN_TARGET_L,&value);
+ *pan=((double)value)/128.0;
+ this->robot_device->read_word_register(DARWIN_HEAD_TILT_TARGET_L,&value);
+ *tilt=((double)value)/128.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 61014a7..76d7043 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -56,6 +56,8 @@ class CDarwinRobot
unsigned char walk_status;
/* joint group status */
unsigned char joints_status[JOINTS_NUM_GROUPS];
+ /* 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
@@ -186,7 +188,7 @@ class CDarwinRobot
void head_stop_tracking(void);
bool head_is_tracking(void);
void head_set_new_target(double pan,double tilt);
- void head_get_current_target(double pan,double tilt);
+ void head_get_current_target(double *pan,double *tilt);
~CDarwinRobot();
};
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 4903591..947eeb0 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -33,6 +33,11 @@ ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+# create an example application
+ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+
# create an example application
#ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
# link necessary libraries
diff --git a/src/examples/darwin_head_tracking_test.cpp b/src/examples/darwin_head_tracking_test.cpp
index 8fc919f..c26b321 100644
--- a/src/examples/darwin_head_tracking_test.cpp
+++ b/src/examples/darwin_head_tracking_test.cpp
@@ -3,30 +3,35 @@
#include <iostream>
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
- int num_v1_servos,num_v2_servos;
+ int num_servos;
+ unsigned int present_servos;
std::vector<int> servos;
std::vector<double> angles,speeds,accels;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
- 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;
+ 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;
darwin.mm_enable_power();
sleep(1);
- 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_set_pan_pid(0.04,0.0,0.0,0.0);
+ darwin.head_set_tilt_pid(0.04,0.0,0.0,0.0);
darwin.mm_enable_servo(20);
darwin.mm_assign_module(20,DARWIN_MM_HEAD);
+ darwin.mm_enable_servo(19);
+ darwin.mm_assign_module(19,DARWIN_MM_HEAD);
darwin.mm_start();
- darwin.head_tracking_set_target(0.0,90.0);
- darwin.head_tracking_start();
+ darwin.head_start_tracking(45.0,90.0);
sleep(10);
- darwin.head_tracking_set_target(0.0,0.0);
+ darwin.head_set_new_target(0.0,0.0);
sleep(10);
darwin.mm_stop();
darwin.mm_disable_power();
--
GitLab