From 023dd8c1e1ad7f919893a352b3a1f06452a97194 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu> Date: Sat, 30 Jul 2016 20:26:00 +0200 Subject: [PATCH] Solved some minor bugs. --- src/darwin_robot.cpp | 32 +++++++++++++++++++++++++++++--- src/darwin_robot.h | 2 +- 2 files changed, 30 insertions(+), 4 deletions(-) diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp index a2bcbec..87418a6 100644 --- a/src/darwin_robot.cpp +++ b/src/darwin_robot.cpp @@ -960,13 +960,26 @@ double CDarwinRobot::walk_get_y_offset(void) throw CDarwinRobotException(_HERE_,"Invalid robot device"); } +void CDarwinRobot::walk_set_z_offset(double offset_m) +{ + unsigned char offset; + + if(this->robot_device!=NULL) + { + offset=(unsigned char)(offset_m*1000.0); + this->robot_device->write_byte_register(DARWIN_WALK_Z_OFFSET,offset); + } + else + throw CDarwinRobotException(_HERE_,"Invalid robot device"); +} + double CDarwinRobot::walk_get_z_offset(void) { unsigned char offset; if(this->robot_device!=NULL) { - this->robot_device->read_byte_register(DARWIN_WALK_Y_OFFSET,&offset); + this->robot_device->read_byte_register(DARWIN_WALK_Z_OFFSET,&offset); return ((double)offset)/1000.0; } else @@ -1051,19 +1064,32 @@ double CDarwinRobot::walk_get_yaw_offset(void) throw CDarwinRobotException(_HERE_,"Invalid robot device"); } -void CDarwinRobot::walk_set_hip_picth_offset(double offset_rad) +void CDarwinRobot::walk_set_hip_pitch_offset(double offset_rad) { unsigned short int offset; if(this->robot_device!=NULL) { - offset=(unsigned char)((offset_rad*180.0/PI)*1024.0); + offset=(unsigned short int)((offset_rad*180.0/PI)*8.0); this->robot_device->write_word_register(DARWIN_WALK_HIP_PITCH_OFF_L,offset); } else throw CDarwinRobotException(_HERE_,"Invalid robot device"); } +double CDarwinRobot::walk_get_hip_pitch_offset(void) +{ + unsigned short int offset; + + 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; + } + else + throw CDarwinRobotException(_HERE_,"Invalid robot device"); +} + void CDarwinRobot::walk_set_period(double period_s) { unsigned short int period; diff --git a/src/darwin_robot.h b/src/darwin_robot.h index 7e08847..0e46bc3 100644 --- a/src/darwin_robot.h +++ b/src/darwin_robot.h @@ -126,7 +126,7 @@ class CDarwinRobot double walk_get_pitch_offset(void); void walk_set_yaw_offset(double offset_rad); double walk_get_yaw_offset(void); - void walk_set_hip_picth_offset(double offset_rad); + void walk_set_hip_pitch_offset(double offset_rad); double walk_get_hip_pitch_offset(void); void walk_set_period(double period_ms); double walk_get_period(void); -- GitLab