From ff83bc4b3cad474530189ca9f1f37671c1e4bbd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 24 Jul 2019 16:04:19 +0200 Subject: [PATCH 1/7] change variable and header guards naming conventions --- diff_controller.cpp | 35 ++++++++------- diff_controller.h | 16 +++---- params.h | 4 +- utils.h | 4 +- wheel.cpp | 101 ++++++++++++++++++++++---------------------- wheel.h | 43 ++++++++++--------- 6 files changed, 102 insertions(+), 101 deletions(-) diff --git a/diff_controller.cpp b/diff_controller.cpp index 5cb86a5..b2419b8 100644 --- a/diff_controller.cpp +++ b/diff_controller.cpp @@ -6,9 +6,9 @@ #include "utils.h" DiffController::DiffController(uint32_t input_timeout) - : _last_wheel_L_ang_pos(0), - _last_wheel_R_ang_pos(0), - _input_timeout(input_timeout) + : last_wheel_L_ang_pos_(0), + last_wheel_R_ang_pos_(0), + input_timeout_(input_timeout) { wheelFL = new Wheel(hMotC, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); wheelRL = new Wheel(hMotD, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); @@ -20,8 +20,8 @@ void DiffController::start() { sys.taskCreate(std::bind(&DiffController::updateWheelLoop, this)); sys.taskCreate(std::bind(&DiffController::updateOdometryLoop, this)); - if (_input_timeout > 0.0) { - _last_update = sys.getRefTime(); + if (input_timeout_ > 0.0) { + last_update_ = sys.getRefTime(); sys.taskCreate(std::bind(&DiffController::inputWatchdog, this)); } #ifdef DEBUG @@ -43,15 +43,15 @@ void DiffController::setSpeed(float linear, float angular) wheelFR->setSpeed(enc_R_speed); wheelRR->setSpeed(enc_R_speed); - if (_input_timeout > 0.0) - _last_update = sys.getRefTime(); + if (input_timeout_ > 0.0) + last_update_ = sys.getRefTime(); } std::vector DiffController::getOdom() { std::vector odom; - odom.push_back(_lin_vel); - odom.push_back(_ang_vel); + odom.push_back(lin_vel_); + odom.push_back(ang_vel_); return odom; } @@ -90,19 +90,19 @@ void DiffController::updateOdometryLoop() float wheel_R_ang_pos = 2 * M_PI * enc_R / ENCODER_RESOLUTION; // velocity in radians per second - float wheel_L_ang_vel = (wheel_L_ang_pos - _last_wheel_L_ang_pos) / (dt / 1000.0); - float wheel_R_ang_vel = (wheel_R_ang_pos - _last_wheel_R_ang_pos) / (dt / 1000.0); + float wheel_L_ang_vel = (wheel_L_ang_pos - last_wheel_L_ang_pos_) / (dt / 1000.0); + float wheel_R_ang_vel = (wheel_R_ang_pos - last_wheel_R_ang_pos_) / (dt / 1000.0); - _last_wheel_L_ang_pos = wheel_L_ang_pos; - _last_wheel_R_ang_pos = wheel_R_ang_pos; + last_wheel_L_ang_pos_ = wheel_L_ang_pos; + last_wheel_R_ang_pos_ = wheel_R_ang_pos; // velocity in meters per second float wheel_L_lin_vel = wheel_L_ang_vel * WHEEL_RADIUS; float wheel_R_lin_vel = wheel_R_ang_vel * WHEEL_RADIUS; // linear (m/s) and angular (r/s) velocities of the robot - _lin_vel = (wheel_L_lin_vel + wheel_R_lin_vel) / 2; - _ang_vel = (wheel_R_lin_vel - wheel_L_lin_vel) / ROBOT_WIDTH; + lin_vel_ = (wheel_L_lin_vel + wheel_R_lin_vel) / 2; + ang_vel_ = (wheel_R_lin_vel - wheel_L_lin_vel) / ROBOT_WIDTH; sys.delaySync(t, dt); } @@ -126,9 +126,8 @@ void DiffController::inputWatchdog() { while (true) { - // TODO possibly not thread safe ? - while (sys.getRefTime() < _last_update + _input_timeout) - sys.delay(_last_update + _input_timeout - sys.getRefTime() + 1); + while (sys.getRefTime() < last_update_ + input_timeout_) + sys.delay(last_update_ + input_timeout_ - sys.getRefTime() + 1); setSpeed(0.0, 0.0); } diff --git a/diff_controller.h b/diff_controller.h index 2285518..ff499eb 100644 --- a/diff_controller.h +++ b/diff_controller.h @@ -1,5 +1,5 @@ -#ifndef _DIFF_CONTROLLER_H_ -#define _DIFF_CONTROLLER_H_ +#ifndef LEO_FIRMWARE_DIFF_CONTROLLER_H_ +#define LEO_FIRMWARE_DIFF_CONTROLLER_H_ #include "wheel.h" @@ -25,13 +25,13 @@ class DiffController Wheel *wheelFR; Wheel *wheelRR; - float _last_wheel_L_ang_pos; - float _last_wheel_R_ang_pos; - float _lin_vel; - float _ang_vel; + float last_wheel_L_ang_pos_; + float last_wheel_R_ang_pos_; + float lin_vel_; + float ang_vel_; - uint32_t _input_timeout; - uint32_t _last_update; + uint32_t input_timeout_; + uint32_t last_update_; }; #endif \ No newline at end of file diff --git a/params.h b/params.h index cdfd1b1..c369d00 100644 --- a/params.h +++ b/params.h @@ -1,5 +1,5 @@ -#ifndef PARAMS_H -#define PARAMS_H +#ifndef LEO_FIRMWARE_PARAMS_H +#define LEO_FIRMWARE_PARAMS_H #include #include "hFramework.h" diff --git a/utils.h b/utils.h index c864167..2fbd58a 100644 --- a/utils.h +++ b/utils.h @@ -1,5 +1,5 @@ -#ifndef _UTILS_H_ -#define _UTILS_H_ +#ifndef LEO_FIRMWARE_UTILS_H_ +#define LEO_FIRMWARE_UTILS_H_ inline float clamp(float value, float limit) { diff --git a/wheel.cpp b/wheel.cpp index 969da65..4c293e3 100644 --- a/wheel.cpp +++ b/wheel.cpp @@ -2,105 +2,106 @@ #include "utils.h" Wheel::Wheel(hMotor& motor, bool polarity, float max_speed, - float Kp, float Ki, float Kd, + float kp, float ki, float kd, uint16_t power_limit = 1000, uint16_t torque_limit = 1000) - : mot(motor), - pol(polarity), - _max_speed(max_speed), - _power_limit(power_limit), - _torque_limit(torque_limit), - turnedOn(true), - dNow(0.0), - vNow(0.0), - vTarget(0.0) + : motor_(motor), + polarity_(polarity), + max_speed_(max_speed), + power_limit_(power_limit), + torque_limit_(torque_limit), + turned_on_(true), + d_now_(0.0), + v_now_(0.0), + v_target_(0.0) { - vReg.setScale(1); - vReg.setRange(-vRange, vRange); - vReg.setIRange(-vRange, vRange); - vReg.setCoeffs(Kp, Ki, Kd); + v_reg_.setScale(1); + v_reg_.setRange(-v_range_, v_range_); + v_reg_.setIRange(-v_range_, v_range_); + v_reg_.setCoeffs(kp, ki, kd); - if (pol) { - mot.setMotorPolarity(Polarity::Reversed); - mot.setEncoderPolarity(Polarity::Reversed); + if (polarity_) { + motor_.setMotorPolarity(Polarity::Reversed); + motor_.setEncoderPolarity(Polarity::Reversed); } - mot.setPowerLimit(power_limit); - mot.resetEncoderCnt(); + motor_.setPowerLimit(power_limit); + motor_.resetEncoderCnt(); } void Wheel::update(uint32_t dt) { - float dPrev = dNow; - dNow = (float)mot.getEncoderCnt(); + float d_prev = d_now_; + d_now_ = (float)motor_.getEncoderCnt(); - vNow = (dNow - dPrev) / (dt * 0.001); + v_now_ = (d_now_ - d_prev) / (dt * 0.001); - float vErr = vNow - vTarget; - float pidOut = vReg.update(vErr, dt); + float v_err = v_now_ - v_target_; + float pid_out = v_reg_.update(v_err, dt); - float est_power = (std::abs(vNow) / _max_speed) * 1000.0; - float max_power = std::min(est_power + static_cast(_torque_limit), (float)1000.0); + float est_power = (std::abs(v_now_) / max_speed_) * 1000.0; + float max_power = std::min( + est_power + static_cast(torque_limit_), (float)1000.0); - if (turnedOn == true) { - if (vNow == 0.0 && vTarget == 0.0){ - vReg.reset(); - _power = 0; + if (turned_on_ == true) { + if (v_now_ == 0.0 && v_target_ == 0.0){ + v_reg_.reset(); + power_ = 0; } else { - float power_limited = clamp(pidOut, max_power); - _power = static_cast(power_limited); + float power_limited = clamp(pid_out, max_power); + power_ = static_cast(power_limited); } - mot.setPower(_power); + motor_.setPower(power_); } } void Wheel::setSpeed(float speed) { - if (speed > _max_speed) { - vTarget = _max_speed; + if (speed > max_speed_) { + v_target_ = max_speed_; } - else if (speed < -_max_speed) { - vTarget = -_max_speed; + else if (speed < -max_speed_) { + v_target_ = -max_speed_; } else { - vTarget = speed; + v_target_ = speed; } } float Wheel::getSpeed() { - return vNow; + return v_now_; } int16_t Wheel::getPower() { - return _power; + return power_; } int32_t Wheel::getDistance() { - return dNow; + return d_now_; } void Wheel::resetDistance() { - mot.resetEncoderCnt(); + motor_.resetEncoderCnt(); } void Wheel::reset() { - mot.resetEncoderCnt(); - vReg.reset(); - vNow = 0; - vTarget = 0; - mot.setPower(0); + motor_.resetEncoderCnt(); + v_reg_.reset(); + v_now_ = 0; + v_target_ = 0; + motor_.setPower(0); } void Wheel::turnOff() { - turnedOn = false; - mot.setPower(0); + turned_on_ = false; + motor_.setPower(0); } void Wheel::turnOn() { - turnedOn = true; + turned_on_ = true; } \ No newline at end of file diff --git a/wheel.h b/wheel.h index e8fb153..484d766 100644 --- a/wheel.h +++ b/wheel.h @@ -1,32 +1,16 @@ -#ifndef _WHEEL_H_ -#define _WHEEL_H_ +#ifndef LEO_FIRMWARE_WHEEL_H_ +#define LEO_FIRMWARE_WHEEL_H_ #include #include #include "hFramework.h" #include "hCyclicBuffer.h" -class Wheel { - - hMotor& mot; - hPIDRegulator vReg; - - bool pol; - int16_t _power; - uint16_t _power_limit; - uint16_t _torque_limit; - float _max_speed; - - bool turnedOn; - float dNow; - float vTarget; - float vNow; - - float vRange = 1000.0; - +class Wheel +{ public: Wheel(hMotor &motor, bool polarity, float max_speed, - float Kp, float Ki, float Kd, + float kp, float ki, float kd, uint16_t power_limit, uint16_t torque_limit); void update(uint32_t dt); @@ -41,6 +25,23 @@ class Wheel { void reset(); void turnOff(); void turnOn(); + +private: + hMotor& motor_; + hPIDRegulator v_reg_; + + int16_t power_; + + bool turned_on_; + float d_now_; + float v_target_; + float v_now_; + + const bool polarity_; + const float max_speed_; + const uint16_t power_limit_; + const uint16_t torque_limit_; + static constexpr float v_range_ = 1000.0; }; #endif From 043d94ca32699212687837533973f208323235a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 24 Jul 2019 17:26:38 +0200 Subject: [PATCH 2/7] more accurate speed measurement --- diff_controller.cpp | 3 +++ utils.h | 25 +++++++++++++++++++++++++ wheel.cpp | 26 ++++++++++++++++++++------ wheel.h | 10 +++++++++- 4 files changed, 57 insertions(+), 7 deletions(-) diff --git a/diff_controller.cpp b/diff_controller.cpp index b2419b8..9b36ad7 100644 --- a/diff_controller.cpp +++ b/diff_controller.cpp @@ -118,6 +118,9 @@ void DiffController::debugLoop() Serial.printf("Motor powers: %d %d %d %d\r\n", wheelFL->getPower(), wheelRL->getPower(), wheelFR->getPower(), wheelRR->getPower()); + Serial.printf("Motor speeds: %f %f %f %f\r\n", + wheelFL->getSpeed(), wheelRL->getSpeed(), + wheelFR->getSpeed(), wheelRR->getSpeed()); sys.delaySync(t, dt); } } diff --git a/utils.h b/utils.h index 2fbd58a..cb50d5b 100644 --- a/utils.h +++ b/utils.h @@ -11,4 +11,29 @@ inline float clamp(float value, float limit) return value; } +template +class CircularBuffer +{ + T *values_; + size_t size_; + size_t iter_; + +public: + CircularBuffer(uint16_t size) + : size_(size), + iter_(0), + values_(new T[size]) + { } + + T push_back(T val) + { + T tmp = values_[iter_]; + values_[iter_++] = val; + if (iter_ >= size_) + iter_ = 0; + return tmp; + } +}; + + #endif \ No newline at end of file diff --git a/wheel.cpp b/wheel.cpp index 4c293e3..75cf339 100644 --- a/wheel.cpp +++ b/wheel.cpp @@ -10,9 +10,12 @@ Wheel::Wheel(hMotor& motor, bool polarity, float max_speed, power_limit_(power_limit), torque_limit_(torque_limit), turned_on_(true), - d_now_(0.0), + ticks_now_(0), + ticks_sum_(0), + dt_sum_(0), v_now_(0.0), - v_target_(0.0) + v_target_(0.0), + encoder_buffer_(encoder_buffer_size_) { v_reg_.setScale(1); v_reg_.setRange(-v_range_, v_range_); @@ -30,10 +33,21 @@ Wheel::Wheel(hMotor& motor, bool polarity, float max_speed, void Wheel::update(uint32_t dt) { - float d_prev = d_now_; - d_now_ = (float)motor_.getEncoderCnt(); + int32_t ticks_prev = ticks_now_; + ticks_now_ = motor_.getEncoderCnt(); - v_now_ = (d_now_ - d_prev) / (dt * 0.001); + int32_t new_ticks = ticks_now_ - ticks_prev; + + std::pair encoder_old = + encoder_buffer_.push_back(std::pair(new_ticks, dt)); + + ticks_sum_ += new_ticks; + dt_sum_ += dt; + + ticks_sum_ -= encoder_old.first; + dt_sum_ -= encoder_old.second; + + v_now_ = static_cast(ticks_sum_) / (dt_sum_ * 0.001); float v_err = v_now_ - v_target_; float pid_out = v_reg_.update(v_err, dt); @@ -79,7 +93,7 @@ int16_t Wheel::getPower() int32_t Wheel::getDistance() { - return d_now_; + return ticks_now_; } void Wheel::resetDistance() diff --git a/wheel.h b/wheel.h index 484d766..50c9e1a 100644 --- a/wheel.h +++ b/wheel.h @@ -5,6 +5,7 @@ #include #include "hFramework.h" #include "hCyclicBuffer.h" +#include "utils.h" class Wheel { @@ -29,11 +30,16 @@ class Wheel private: hMotor& motor_; hPIDRegulator v_reg_; + CircularBuffer> encoder_buffer_; int16_t power_; bool turned_on_; - float d_now_; + + int32_t ticks_now_; + int32_t ticks_sum_; + uint32_t dt_sum_; + float v_target_; float v_now_; @@ -41,6 +47,8 @@ class Wheel const float max_speed_; const uint16_t power_limit_; const uint16_t torque_limit_; + + static const int encoder_buffer_size_ = 10; static constexpr float v_range_ = 1000.0; }; From 87eafc0bc51b2e8e4b49077486b60b612d5d95e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 25 Jul 2019 12:34:00 +0200 Subject: [PATCH 3/7] improve odom loop --- diff_controller.cpp | 37 ++++++++++++++----------------------- diff_controller.h | 2 -- 2 files changed, 14 insertions(+), 25 deletions(-) diff --git a/diff_controller.cpp b/diff_controller.cpp index 9b36ad7..2d29af2 100644 --- a/diff_controller.cpp +++ b/diff_controller.cpp @@ -6,9 +6,7 @@ #include "utils.h" DiffController::DiffController(uint32_t input_timeout) - : last_wheel_L_ang_pos_(0), - last_wheel_R_ang_pos_(0), - input_timeout_(input_timeout) + : input_timeout_(input_timeout) { wheelFL = new Wheel(hMotC, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); wheelRL = new Wheel(hMotD, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); @@ -76,33 +74,26 @@ void DiffController::updateOdometryLoop() while(true) { - // distance in tics - float enc_FL = wheelFL->getDistance(); - float enc_RL = wheelRL->getDistance(); - float enc_FR = wheelFR->getDistance(); - float enc_RR = wheelRR->getDistance(); + // speed in ticks/sec + float FL_speed = wheelFL->getSpeed(); + float RL_speed = wheelRL->getSpeed(); + float FR_speed = wheelFR->getSpeed(); + float RR_speed = wheelRR->getSpeed(); - float enc_L = (enc_FL + enc_RL) / 2; - float enc_R = (enc_FR + enc_RR) / 2; - - // distance in radians - float wheel_L_ang_pos = 2 * M_PI * enc_L / ENCODER_RESOLUTION; - float wheel_R_ang_pos = 2 * M_PI * enc_R / ENCODER_RESOLUTION; + float L_speed = (FL_speed + RL_speed) / 2.0; + float R_speed = (FR_speed + RR_speed) / 2.0; // velocity in radians per second - float wheel_L_ang_vel = (wheel_L_ang_pos - last_wheel_L_ang_pos_) / (dt / 1000.0); - float wheel_R_ang_vel = (wheel_R_ang_pos - last_wheel_R_ang_pos_) / (dt / 1000.0); - - last_wheel_L_ang_pos_ = wheel_L_ang_pos; - last_wheel_R_ang_pos_ = wheel_R_ang_pos; + float L_ang_vel = 2 * M_PI * L_speed / ENCODER_RESOLUTION; + float R_ang_vel = 2 * M_PI * R_speed / ENCODER_RESOLUTION; // velocity in meters per second - float wheel_L_lin_vel = wheel_L_ang_vel * WHEEL_RADIUS; - float wheel_R_lin_vel = wheel_R_ang_vel * WHEEL_RADIUS; + float L_lin_vel = L_ang_vel * WHEEL_RADIUS; + float R_lin_vel = R_ang_vel * WHEEL_RADIUS; // linear (m/s) and angular (r/s) velocities of the robot - lin_vel_ = (wheel_L_lin_vel + wheel_R_lin_vel) / 2; - ang_vel_ = (wheel_R_lin_vel - wheel_L_lin_vel) / ROBOT_WIDTH; + lin_vel_ = (L_lin_vel + R_lin_vel) / 2; + ang_vel_ = (R_lin_vel - L_lin_vel) / ROBOT_WIDTH; sys.delaySync(t, dt); } diff --git a/diff_controller.h b/diff_controller.h index ff499eb..eed2550 100644 --- a/diff_controller.h +++ b/diff_controller.h @@ -25,8 +25,6 @@ class DiffController Wheel *wheelFR; Wheel *wheelRR; - float last_wheel_L_ang_pos_; - float last_wheel_R_ang_pos_; float lin_vel_; float ang_vel_; From e87ffc800afc57f7e8bf6eb0683717a30dc4fb59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 25 Jul 2019 12:40:58 +0200 Subject: [PATCH 4/7] move Servo Wrapper to utils --- main.cpp | 40 +--------------------------------------- utils.h | 47 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 39 deletions(-) diff --git a/main.cpp b/main.cpp index ce117e3..bf96e3c 100644 --- a/main.cpp +++ b/main.cpp @@ -9,6 +9,7 @@ #include "diff_controller.h" #include "params.h" +#include "utils.h" using namespace hFramework; @@ -28,45 +29,6 @@ ros::Subscriber *twist_sub; DiffController *dc; -class ServoWrapper -{ - int num; - int per; - IServo& servo; - -public: - ServoWrapper(int num, IServo& servo) - : num(num), - servo(servo) {} - - void angleCallback(const std_msgs::Int16& msg) - { - if (per!=SERVO_PERIOD) - { - servo.setPeriod(SERVO_PERIOD); - per=SERVO_PERIOD; - } - servo.rotAbs(msg.data); -#ifdef DEBUG - Serial.printf("[servo%dAngleCallback] angle: %d\r\n", num, msg.data); -#endif - } - - void pwmCallback(const std_msgs::UInt16MultiArray& msg) - { - if (msg.data_length >= 2){ - per=msg.data[0]; - servo.setPeriod(msg.data[0]); - servo.setWidth(msg.data[1]); -#ifdef DEBUG - Serial.printf("[servo%dPWMCallback] period: %d width: %d\r\n", num, msg.data[0], msg.data[1]); - } else { - Serial.printf("ERROR: [servo%dPWMCallback] data array should have 2 members\r\n", num); -#endif - } - } -}; - ServoWrapper servo1(1, hServo.servo1); ServoWrapper servo2(2, hServo.servo2); ServoWrapper servo3(3, hServo.servo3); diff --git a/utils.h b/utils.h index cb50d5b..3377c06 100644 --- a/utils.h +++ b/utils.h @@ -1,6 +1,14 @@ #ifndef LEO_FIRMWARE_UTILS_H_ #define LEO_FIRMWARE_UTILS_H_ +#include "hFramework.h" + +#include "ros.h" +#include "std_msgs/Int16.h" +#include "std_msgs/UInt16MultiArray.h" + +#include "params.h" + inline float clamp(float value, float limit) { if (value > limit) @@ -35,5 +43,44 @@ class CircularBuffer } }; +class ServoWrapper +{ + int num; + int per; + IServo& servo; + +public: + ServoWrapper(int num, IServo& servo) + : num(num), + servo(servo) {} + + void angleCallback(const std_msgs::Int16& msg) + { + if (per!=SERVO_PERIOD) + { + servo.setPeriod(SERVO_PERIOD); + per=SERVO_PERIOD; + } + servo.rotAbs(msg.data); +#ifdef DEBUG + Serial.printf("[servo%dAngleCallback] angle: %d\r\n", num, msg.data); +#endif + } + + void pwmCallback(const std_msgs::UInt16MultiArray& msg) + { + if (msg.data_length >= 2){ + per=msg.data[0]; + servo.setPeriod(msg.data[0]); + servo.setWidth(msg.data[1]); +#ifdef DEBUG + Serial.printf("[servo%dPWMCallback] period: %d width: %d\r\n", num, msg.data[0], msg.data[1]); + } else { + Serial.printf("ERROR: [servo%dPWMCallback] data array should have 2 members\r\n", num); +#endif + } + } +}; + #endif \ No newline at end of file From d636ecf26d5382c67029dcfbde69107947072d30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 25 Jul 2019 15:44:36 +0200 Subject: [PATCH 5/7] add joint state publishing --- diff_controller.cpp | 30 +++++++++++++++ diff_controller.h | 8 ++-- main.cpp | 91 +++++++++++++++++++++++++++++++++++++-------- 3 files changed, 110 insertions(+), 19 deletions(-) diff --git a/diff_controller.cpp b/diff_controller.cpp index 2d29af2..ba12760 100644 --- a/diff_controller.cpp +++ b/diff_controller.cpp @@ -53,6 +53,36 @@ std::vector DiffController::getOdom() return odom; } +std::vector DiffController::getWheelPositions() +{ + std::vector positions(4); + positions[0] = 2 * M_PI * wheelFL->getDistance() / ENCODER_RESOLUTION; + positions[1] = 2 * M_PI * wheelRL->getDistance() / ENCODER_RESOLUTION; + positions[2] = 2 * M_PI * wheelFR->getDistance() / ENCODER_RESOLUTION; + positions[3] = 2 * M_PI * wheelRR->getDistance() / ENCODER_RESOLUTION; + return positions; +} + +std::vector DiffController::getWheelVelocities() +{ + std::vector velocities(4); + velocities[0] = 2 * M_PI * wheelFL->getSpeed() / ENCODER_RESOLUTION; + velocities[1] = 2 * M_PI * wheelRL->getSpeed() / ENCODER_RESOLUTION; + velocities[2] = 2 * M_PI * wheelFR->getSpeed() / ENCODER_RESOLUTION; + velocities[3] = 2 * M_PI * wheelRR->getSpeed() / ENCODER_RESOLUTION; + return velocities; +} + +std::vector DiffController::getWheelEfforts() +{ + std::vector efforts(4); + efforts[0] = wheelFL->getPower() * 0.1; + efforts[1] = wheelRL->getPower() * 0.1; + efforts[2] = wheelFR->getPower() * 0.1; + efforts[3] = wheelRR->getPower() * 0.1; + return efforts; +} + void DiffController::updateWheelLoop() { uint32_t t = sys.getRefTime(); diff --git a/diff_controller.h b/diff_controller.h index eed2550..ed5dbab 100644 --- a/diff_controller.h +++ b/diff_controller.h @@ -1,11 +1,10 @@ #ifndef LEO_FIRMWARE_DIFF_CONTROLLER_H_ #define LEO_FIRMWARE_DIFF_CONTROLLER_H_ -#include "wheel.h" - -#include "ros.h" #include +#include "wheel.h" + class DiffController { public: @@ -13,6 +12,9 @@ class DiffController void start(); void setSpeed(float linear, float angular); std::vector getOdom(); + std::vector getWheelPositions(); + std::vector getWheelVelocities(); + std::vector getWheelEfforts(); private: void updateWheelLoop(); diff --git a/main.cpp b/main.cpp index bf96e3c..3c1e423 100644 --- a/main.cpp +++ b/main.cpp @@ -6,6 +6,7 @@ #include "std_msgs/Int16.h" #include "std_msgs/Float32.h" #include "std_msgs/UInt16MultiArray.h" +#include "sensor_msgs/JointState.h" #include "diff_controller.h" #include "params.h" @@ -15,8 +16,6 @@ using namespace hFramework; ros::NodeHandle nh; -hMutex publish_mutex; - std_msgs::Float32 battery; ros::Publisher *battery_pub; bool publish_battery = false; @@ -25,6 +24,10 @@ geometry_msgs::Twist odom; ros::Publisher *odom_pub; bool publish_odom = false; +sensor_msgs::JointState joint_states; +ros::Publisher *joint_states_pub; +bool publish_joint = false; + ros::Subscriber *twist_sub; DiffController *dc; @@ -48,6 +51,7 @@ void initROS() { battery_pub = new ros::Publisher("/battery", &battery); odom_pub = new ros::Publisher("/odom", &odom); + joint_states_pub = new ros::Publisher("/joint_states", &joint_states); twist_sub = new ros::Subscriber("/cmd_vel", &cmdVelCallback); ros::Subscriber *servo1_angle_sub = @@ -78,6 +82,7 @@ void initROS() nh.advertise(*battery_pub); nh.advertise(*odom_pub); + nh.advertise(*joint_states_pub); nh.subscribe(*twist_sub); nh.subscribe(*servo1_angle_sub); nh.subscribe(*servo2_angle_sub); @@ -126,17 +131,31 @@ void setupServos() SERVO_6_ANGLE_MAX, SERVO_6_WIDTH_MAX); } +void setupJoints() +{ + joint_states.header.frame_id = "base_link"; + joint_states.name = new char*[4] {"wheelFL", "wheelRL", "wheelFR", "wheelRR"}; + joint_states.position = new float[4]; + joint_states.velocity = new float[4]; + joint_states.effort = new float[4]; + joint_states.name_length = 4; + joint_states.position_length = 4; + joint_states.velocity_length = 4; + joint_states.effort_length = 4; +} + void batteryLoop() { uint32_t t = sys.getRefTime(); long dt = 1000; while(true) { - battery.data = sys.getSupplyVoltage(); - - publish_mutex.lock(); - publish_battery = true; - publish_mutex.unlock(); + if (!publish_battery) + { + battery.data = sys.getSupplyVoltage(); + + publish_battery = true; + } sys.delaySync(t, dt); } @@ -148,13 +167,50 @@ void odomLoop() long dt = 50; while(true) { - std::vector odo = dc->getOdom(); - odom.linear.x = odo[0]; - odom.angular.z = odo[1]; + if (!publish_odom) + { + std::vector odo = dc->getOdom(); + odom.linear.x = odo[0]; + odom.angular.z = odo[1]; + + publish_odom = true; + } + + sys.delaySync(t, dt); + } +} - publish_mutex.lock(); - publish_odom = true; - publish_mutex.unlock(); +void jointStatesLoop() +{ + uint32_t t = sys.getRefTime(); + long dt = 50; + while(true) + { + if (!publish_joint) + { + std::vector pos = dc->getWheelPositions(); + std::vector vel = dc->getWheelVelocities(); + std::vector eff = dc->getWheelEfforts(); + + joint_states.header.stamp = nh.now(); + + joint_states.position[0] = pos[0]; + joint_states.position[1] = pos[1]; + joint_states.position[2] = pos[2]; + joint_states.position[3] = pos[3]; + + joint_states.velocity[0] = vel[0]; + joint_states.velocity[1] = vel[1]; + joint_states.velocity[2] = vel[2]; + joint_states.velocity[3] = vel[3]; + + joint_states.effort[0] = eff[0]; + joint_states.effort[1] = eff[1]; + joint_states.effort[2] = eff[2]; + joint_states.effort[3] = eff[3]; + + publish_joint = true; + } sys.delaySync(t, dt); } @@ -187,6 +243,7 @@ void hMain() dc->start(); setupServos(); + setupJoints(); initROS(); LED.setOut(); @@ -194,13 +251,12 @@ void hMain() sys.taskCreate(&batteryLoop); sys.taskCreate(&odomLoop); + sys.taskCreate(&jointStatesLoop); while (true) { nh.spinOnce(); - publish_mutex.lock(); - if (publish_battery){ battery_pub->publish(&battery); publish_battery = false; @@ -211,7 +267,10 @@ void hMain() publish_odom = false; } - publish_mutex.unlock(); + if (publish_joint){ + joint_states_pub->publish(&joint_states); + publish_joint = false; + } sys.delaySync(t, 10); } From eaeb795a5ffada743ff7114a58a32b0ffa816413 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 7 Aug 2019 13:32:37 +0200 Subject: [PATCH 6/7] change joint names --- main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/main.cpp b/main.cpp index 3c1e423..06a0abd 100644 --- a/main.cpp +++ b/main.cpp @@ -134,7 +134,10 @@ void setupServos() void setupJoints() { joint_states.header.frame_id = "base_link"; - joint_states.name = new char*[4] {"wheelFL", "wheelRL", "wheelFR", "wheelRR"}; + joint_states.name = new char*[4] { + "wheel_FL_joint", "wheel_RL_joint", + "wheel_FR_joint", "wheel_RR_joint" + }; joint_states.position = new float[4]; joint_states.velocity = new float[4]; joint_states.effort = new float[4]; From e4353648cd2ac12d3953a17b8142946bc5107e45 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 7 Aug 2019 13:37:31 +0200 Subject: [PATCH 7/7] update README --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 97a873a..0958c7d 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,12 @@ Current battery voltage reading +* **`joint_states`** ([sensor_msgs/JointState]) + + Current state of wheel joints. Effort is the percent of applied power (PWM duty) + [geometry_msgs/Twist]: http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html [std_msgs/Int16]: http://docs.ros.org/melodic/api/std_msgs/html/msg/Int16.html [std_msgs/Float32]: http://docs.ros.org/api/std_msgs/html/msg/Float32.html -[std_msgs/UInt16MultiArray]: http://docs.ros.org/api/std_msgs/html/msg/UInt16MultiArray.html \ No newline at end of file +[std_msgs/UInt16MultiArray]: http://docs.ros.org/api/std_msgs/html/msg/UInt16MultiArray.html +[sensor_msgs/JointState]: http://docs.ros.org/melodic/api/sensor_msgs/html/msg/JointState.html \ No newline at end of file