diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 3508488f6c61b210ffb223920986b7cc1212d4c1..578a6cbe9149eb43688c75e4834e8708f43fc838 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 51343b1171878209a3c7b41a3a2506d8c286a584..597128f3f46817be92cb85f108a7432821483404 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 85d6c6dc45be8e6a7a11af72e593a7bdaa532a70..e7a0f910563fbcb7827d1fe8da3038c7577d5962 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 c26b3213c3a4e4548ea03a75140edcd11ff5281e..ad7b46fa2c391e8f8c6476259550f8a362716cfe 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 7fa8fa9468eda1a1b11ac736c190555d9490a081..20a39c8659599478aadbe789a58590e328385b9c 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 5f474f4da29ce122926fdd0907b9b50e3ed1efff..acd149afbf6af4312520f375bdb413f57adcadde 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;