diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index b3f91afb04d7c6fb30f37e4e74215be666d1f661..7f2d9939276baaa60b9121e08d4e1128cb2b496c 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -57,7 +57,7 @@ const led_info_t leds[GPIO_NUM_LEDS]={{DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_
CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id)
{
- int num_buses;
+ int num_buses,i;
this->robot_device=NULL;
if(name!="")
@@ -82,6 +82,9 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
/* get the current walking status */
this->robot_device->read_byte_register(DARWIN_WALK_CNTRL,&this->walk_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]);
}
else
{
@@ -1479,27 +1482,70 @@ double CDarwinRobot::walk_get_turn_step(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-/*void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
+// joint motion interface
+void CDarwinRobot::joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
{
- this->robot_device->write_byte_register(DARWIN_MM_KNEE_GAIN,(unsigned char)(knee*64.0));
- this->robot_device->write_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,(unsigned char)(ankle_pitch*64.0));
- this->robot_device->write_byte_register(DARWIN_MM_HIP_ROLL_GAIN,(unsigned char)(hip_roll*64.0));
- this->robot_device->write_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,(unsigned char)(ankle_roll*64.0));
+ unsigned int group_servos=0,i;
+ unsigned short int angle;
+ unsigned char speed,accel;
+
+ if(this->robot_device!=NULL)
+ {
+ if(servos.size()!=angles.size() || servos.size()!=speeds.size() || servos.size()!=accels.size())
+ throw CDarwinRobotException(_HERE_,"Invalid input vector sizes");
+ for(i=0;i<servos.size();i++)
+ {
+ group_servos|=(0x00000001<<servos[i]);
+ angle=(unsigned short int)(angles[i]*128.0);
+ this->robot_device->write_word_register(DARWIN_JOINT_SERVO0_ANGLE_L+servos[i]*2,angle);
+ speed=(unsigned char)(speeds[i]);
+ this->robot_device->write_byte_register(DARWIN_JOINT_SERVO0_SPEED+servos[i],speed);
+ accel=(unsigned char)(accels[i]);
+ this->robot_device->write_byte_register(DARWIN_JOINT_SERVO0_ACCEL+servos[i],accel);
+ }
+ this->robot_device->write_registers(DARWIN_JOINT_GRP0_SERVOS0+((unsigned short int)group)*5,(unsigned char *)&group_servos,4);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
+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]);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+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]);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::joints_are_moving(joints_grp_t group)
{
unsigned char value;
- this->robot_device->read_byte_register(DARWIN_MM_KNEE_GAIN,&value);
- *knee=value/64.0;
- this->robot_device->read_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,&value);
- *ankle_pitch=value/64.0;
- this->robot_device->read_byte_register(DARWIN_MM_HIP_ROLL_GAIN,&value);
- *hip_roll=value/64.0;
- this->robot_device->read_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,&value);
- *ankle_roll=value/64.0;
-}*/
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_JOINT_GRP0_CNTRL+((unsigned short int)group*5),&value);
+ if(value&JOINT_STATUS)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
CDarwinRobot::~CDarwinRobot()
{
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index af91eadc9221bdd59b6ebd15dccbbdf416bc3819..73d5f84d72d0731a6e2acc9f0136913ce29454c6 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -36,6 +36,10 @@ typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
ADC_CH9=8,ADC_CH10=9,ADC_CH11=10,ADC_CH12=11,ADC_CH13=12,ADC_CH14=13,ADC_CH16=15,ADC_CH18=17} adc_t;
+#define JOINTS_NUM_GROUPS 4
+
+typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
+
class CDarwinRobot
{
private:
@@ -50,6 +54,8 @@ class CDarwinRobot
unsigned char action_status;
/* walk status */
unsigned char walk_status;
+ /* joint group status */
+ unsigned char joints_status[JOINTS_NUM_GROUPS];
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
@@ -158,6 +164,11 @@ class CDarwinRobot
double walk_get_y_step(void);
void walk_set_turn_step(double step_rad);
double walk_get_turn_step(void);
+ // joint motion interface
+ void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
+ void joints_start(joints_grp_t group);
+ void joints_stop(joints_grp_t group);
+ bool joints_are_moving(joints_grp_t group);
~CDarwinRobot();
};
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a60b77f414ecc089d7ce6844d570410ad2693b7b..49035917318a162fc397b5fb6055346d07e07875 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -28,6 +28,11 @@ ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
+# create an example application
+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_kin darwin_kin.cpp)
# link necessary libraries
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index ab4de08b33f4902c85c5030bdc2bf0fad7ae5955..7fa8fa9468eda1a1b11ac736c190555d9490a081 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -3,61 +3,78 @@
#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,i;
- std::vector<int> servos;
+ int num_servos,i,j;
+ unsigned int present_servos;
+ std::vector<unsigned char> servos;
std::vector<double> angles,speeds,accels;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
- 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;
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
+ num_servos=darwin.mm_get_num_servos();
+ std::cout << "Found " << num_servos << " servos " << std::endl;
+ present_servos=darwin.mm_get_present_servos();
+ std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
darwin.mm_enable_power();
- for(i=0;i<=27;i++)
+ for(i=0;i<MAX_NUM_SERVOS;i++)
{
- darwin.mm_enable_servo(i);
- darwin.mm_assign_module(i,DARWIN_MM_JOINTS);
+ if(present_servos&(0x00000001<<i))
+ {
+ darwin.mm_enable_servo(i);
+ darwin.mm_assign_module(i,DARWIN_MM_JOINTS);
+ }
}
darwin.mm_start();
- servos.push_back(22);
- servos.push_back(19);
- angles.push_back(45.0);
- angles.push_back(45.0);
- speeds.push_back(10.0);
- speeds.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())
- {
- darwin.adc_get_data();
- angles=darwin.mm_get_servo_angles();
- for(i=0;i<angles.size();i++)
- std::cout << "servo " << i << ": " << angles[i] << std::endl;
- std::cout << "moving joints ..." << std::endl;
- usleep(100000);
- }
- std::cout << "done" << std::endl;
- return 0;
- servos.push_back(20);
- angles.clear();
- angles.push_back(0.0);
- angles.push_back(0.0);
- speeds.push_back(10.0);
- accels.push_back(10.0);
- darwin.joints_load(servos,angles,speeds,accels);
- darwin.joints_start();
- while(darwin.are_joints_moving())
- {
- angles=darwin.mm_get_servo_angles();
- std::cout << angles[19] << "," << angles[20] << std::endl;
- std::cout << "moving joints ..." << std::endl;
- usleep(100000);
+ for(j=0;j<4;j++)
+ {
+ 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);
+ }
+ }
+ 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);
+ }
+ 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))
+ {
+ std::cout << "moving joints ..." << std::endl;
+ usleep(100000);
+ }
}
darwin.mm_stop();
darwin.mm_disable_power();