From 302357b798a54f534d14efcf45cddbfa48fe80db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Tue, 23 Aug 2016 22:55:38 +0200
Subject: [PATCH] Removed the walk status attribute. Solved a bug in the
 codification of some of the walk parameters.

---
 src/darwin_robot.cpp | 27 ++++++++++-----------------
 src/darwin_robot.h   |  4 +---
 2 files changed, 11 insertions(+), 20 deletions(-)

diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 46401f3..35df42d 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 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]);
@@ -956,7 +954,7 @@ double CDarwinRobot::walk_get_x_offset(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_byte_register(DARWIN_WALK_X_OFFSET,&offset);
-    return ((double)offset)/1000.0;
+    return ((double)((signed char)offset))/1000.0;
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -982,7 +980,7 @@ double CDarwinRobot::walk_get_y_offset(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_byte_register(DARWIN_WALK_Y_OFFSET,&offset);
-    return ((double)offset)/1000.0;
+    return ((double)((signed char)offset))/1000.0;
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1008,7 +1006,7 @@ double CDarwinRobot::walk_get_z_offset(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_byte_register(DARWIN_WALK_Z_OFFSET,&offset);
-    return ((double)offset)/1000.0;
+    return ((double)((signed char)offset))/1000.0;
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1098,7 +1096,7 @@ void CDarwinRobot::walk_set_hip_pitch_offset(double offset_rad)
 
   if(this->robot_device!=NULL)
   {
-    offset=(unsigned short int)((offset_rad*180.0/PI)*8.0);
+    offset=(unsigned short int)((offset_rad*180.0/PI)*1024.0);
     this->robot_device->write_word_register(DARWIN_WALK_HIP_PITCH_OFF_L,offset);
   }
   else
@@ -1112,7 +1110,7 @@ double CDarwinRobot::walk_get_hip_pitch_offset(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_word_register(DARWIN_WALK_HIP_PITCH_OFF_L,&offset);
-    return (((double)offset)*PI/180.0)/8.0;
+    return (((double)offset)*PI/180.0)/1024.0;
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1124,7 +1122,8 @@ void CDarwinRobot::walk_set_period(double period_s)
 
   if(this->robot_device!=NULL)
   {
-    period=(unsigned char)(period_s*1000.0);
+    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
@@ -1320,7 +1319,7 @@ double CDarwinRobot::walk_get_arm_swing_gain(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_byte_register(DARWIN_WALK_ARM_SWING_GAIN,&gain_int);
-    return ((double)gain_int)*32.0;
+    return ((double)gain_int)/32.0;
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1381,10 +1380,7 @@ double CDarwinRobot::walk_get_rot_speed(void)
 void CDarwinRobot::walk_start(void)
 {
   if(this->robot_device!=NULL)
-  {
-    this->walk_status|=WALK_START;
-    this->robot_device->write_byte_register(DARWIN_WALK_CNTRL,this->walk_status);
-  }
+    this->robot_device->write_byte_register(DARWIN_WALK_CNTRL,WALK_START);
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
@@ -1392,10 +1388,7 @@ void CDarwinRobot::walk_start(void)
 void CDarwinRobot::walk_stop(void)
 {
   if(this->robot_device!=NULL)
-  {
-    this->walk_status|=WALK_STOP;
-    this->robot_device->write_byte_register(DARWIN_WALK_CNTRL,this->walk_status);
-  }
+    this->robot_device->write_byte_register(DARWIN_WALK_CNTRL,WALK_STOP);
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 30d3a9d..80af216 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;
-    /* walk status */
-    unsigned char walk_status;
     /* joint group status */
     unsigned char joints_status[JOINTS_NUM_GROUPS];
     /* head tracking status */
@@ -137,7 +135,7 @@ class CDarwinRobot
     double walk_get_yaw_offset(void);
     void walk_set_hip_pitch_offset(double offset_rad);
     double walk_get_hip_pitch_offset(void);
-    void walk_set_period(double period_ms);
+    void walk_set_period(double period_s);
     double walk_get_period(void);
     void walk_set_ssp_dsp_ratio(double ratio);
     double walk_get_ssp_dsp_ratio(void);
-- 
GitLab