diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp index 35df42d1c5b051f7b97a1dadc58c3c96d5af1bf3..3508488f6c61b210ffb223920986b7cc1212d4c1 100644 --- a/src/darwin_robot.cpp +++ b/src/darwin_robot.cpp @@ -80,9 +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 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); } @@ -1538,10 +1535,7 @@ std::vector<double> CDarwinRobot::joints_get_group_accels(joints_grp_t group) void CDarwinRobot::joints_start(joints_grp_t group) { if(this->robot_device!=NULL) - { - this->joints_status[(int)group]|=JOINT_START; - this->robot_device->write_byte_register(DARWIN_JOINT_GRP0_CNTRL+((unsigned short int)group*5),this->joints_status[(int)group]); - } + this->robot_device->write_byte_register(DARWIN_JOINT_GRP0_CNTRL+((unsigned short int)group*5),JOINT_START); else throw CDarwinRobotException(_HERE_,"Invalid robot device"); } @@ -1549,10 +1543,7 @@ void CDarwinRobot::joints_start(joints_grp_t group) void CDarwinRobot::joints_stop(joints_grp_t group) { if(this->robot_device!=NULL) - { - this->joints_status[(int)group]|=JOINT_STOP; - this->robot_device->write_byte_register(DARWIN_JOINT_GRP0_CNTRL+((unsigned short int)group*5),this->joints_status[(int)group]); - } + this->robot_device->write_byte_register(DARWIN_JOINT_GRP0_CNTRL+((unsigned short int)group*5),JOINT_STOP); else throw CDarwinRobotException(_HERE_,"Invalid robot device"); } diff --git a/src/darwin_robot.h b/src/darwin_robot.h index 80af216a06dec37eb8cc452b1768a2f608a54597..51343b1171878209a3c7b41a3a2506d8c286a584 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; - /* joint group status */ - unsigned char joints_status[JOINTS_NUM_GROUPS]; /* head tracking status */ unsigned char head_tracking_status; public: