From 07aab5d7964574ce38ae4cfda60bdca5cf9ba1d8 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sun, 30 Apr 2017 09:20:10 +0200 Subject: [PATCH] use bang-bang controller for speed control --- config/control.yaml | 1 + include/robo_explorer_hardware.h | 31 +++++++++++++++++++++---------- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/config/control.yaml b/config/control.yaml index 990d411..c6ad1c6 100644 --- a/config/control.yaml +++ b/config/control.yaml @@ -9,3 +9,4 @@ diff_drive_controller: pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] publish_rate: 10 + cmd_vel_timeout: 1.0 diff --git a/include/robo_explorer_hardware.h b/include/robo_explorer_hardware.h index 5c144d5..859f81c 100644 --- a/include/robo_explorer_hardware.h +++ b/include/robo_explorer_hardware.h @@ -4,6 +4,9 @@ #include "roboint/Inputs.h" #include "roboint/Motor.h" +// When to increase/decrease speed based on current wheel speed difference +#define SPEED_CONTROL_THRESH 0.5 + class RoboExplorer : public hardware_interface::RobotHW { public: @@ -31,8 +34,9 @@ class RoboExplorer : public hardware_interface::RobotHW void read(ros::Duration period) { static double pos_last[2]; - pos[0] = pos_encoder[0] * M_PI/8; - pos[1] = pos_encoder[1] * M_PI/8; + // gear ration 1:2, 8 ticks/revolution, 2*8=16 + pos[0] = pos_encoder[0] * 2*M_PI/16; + pos[1] = pos_encoder[1] * 2*M_PI/16; vel[0] = (pos[0] - pos_last[0])/period.toSec(); vel[1] = (pos[1] - pos_last[1])/period.toSec(); @@ -41,19 +45,26 @@ class RoboExplorer : public hardware_interface::RobotHW // Writes current velocity command to hardware void write() { - double speed_l = 0; - double speed_r = 0; + static double speed_l = 0; + static double speed_r = 0; double wish_speed_left = cmd[0]; double wish_speed_right = cmd[1]; roboint::Motor msg; - if (fabs(wish_speed_left) > 0) { - speed_l = 64.3*fabs(wish_speed_left) -1.7; - if (wish_speed_left < 0) speed_l*=-1; + if (wish_speed_left == 0) { + speed_l = 0; + } else if (vel[0] - wish_speed_left < -SPEED_CONTROL_THRESH) { + speed_l++; + } else if (vel[0] - wish_speed_left > SPEED_CONTROL_THRESH) { + speed_l--; } - if (fabs(wish_speed_right) > 0) { - speed_r = 64.3*fabs(wish_speed_right) -1.7; - if (wish_speed_right < 0) speed_r*=-1; + + if (wish_speed_right == 0) { + speed_r = 0; + } else if (vel[1] - wish_speed_right < -SPEED_CONTROL_THRESH) { + speed_r++; + } else if (vel[1] - wish_speed_right > SPEED_CONTROL_THRESH) { + speed_r--; } // check limits -- 2.39.2