Added the joints motion interface to the darwin robot driver.

Added an example of use of the joints motion interface.

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()
 {

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();
 };
 

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

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();