diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp index 2336f96cc02a28258cc024536d7e83c2b3d5e42c..67a32e2faf5cfc96b98fdac3ca3e0dc09f2b6fb1 100644 --- a/src/darwin_robot.cpp +++ b/src/darwin_robot.cpp @@ -1990,7 +1990,7 @@ void CDarwinRobot::stairs_set_roll_shift(double angle) if(this->robot_device!=NULL) { - value=(unsigned char)((angle*180.0/PI)*8.0); + value=(unsigned char)((angle*180.0/PI)*4.0); this->robot_device->write_byte_register(DARWIN_STAIRS_R_SHIFT,value); usleep(10000); } @@ -2005,7 +2005,7 @@ double CDarwinRobot::stairs_get_roll_shift(void) if(this->robot_device!=NULL) { this->robot_device->read_byte_register(DARWIN_STAIRS_R_SHIFT,&value); - return (((double)value)*PI/180.0)/8.0; + return (((double)value)*PI/180.0)/4.0; } else throw CDarwinRobotException(_HERE_,"Invalid robot device"); @@ -2017,7 +2017,7 @@ void CDarwinRobot::stairs_set_pitch_shift(double angle) if(this->robot_device!=NULL) { - value=(unsigned char)((angle*180.0/PI)*8.0); + value=(unsigned char)((angle*180.0/PI)*4.0); this->robot_device->write_byte_register(DARWIN_STAIRS_P_SHIFT,value); usleep(10000); } @@ -2032,7 +2032,7 @@ double CDarwinRobot::stairs_get_pitch_shift(void) if(this->robot_device!=NULL) { this->robot_device->read_byte_register(DARWIN_STAIRS_P_SHIFT,&value); - return (((double)value)*PI/180.0)/8.0; + return (((double)value)*PI/180.0)/4.0; } else throw CDarwinRobotException(_HERE_,"Invalid robot device");