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 diff --git a/diff_controller.cpp b/diff_controller.cpp index 5cb86a5..ba12760 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); @@ -20,8 +18,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,18 +41,48 @@ 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; } +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(); @@ -76,33 +104,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); } @@ -118,6 +139,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); } } @@ -126,9 +150,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..ed5dbab 100644 --- a/diff_controller.h +++ b/diff_controller.h @@ -1,11 +1,10 @@ -#ifndef _DIFF_CONTROLLER_H_ -#define _DIFF_CONTROLLER_H_ +#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(); @@ -25,13 +27,11 @@ 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 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/main.cpp b/main.cpp index ce117e3..06a0abd 100644 --- a/main.cpp +++ b/main.cpp @@ -6,16 +6,16 @@ #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" +#include "utils.h" using namespace hFramework; ros::NodeHandle nh; -hMutex publish_mutex; - std_msgs::Float32 battery; ros::Publisher *battery_pub; bool publish_battery = false; @@ -24,49 +24,14 @@ 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; -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); @@ -86,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 = @@ -116,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); @@ -164,17 +131,34 @@ 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] { + "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]; + 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); } @@ -186,13 +170,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); + } +} + +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]; - publish_mutex.lock(); - publish_odom = true; - publish_mutex.unlock(); + 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); } @@ -225,6 +246,7 @@ void hMain() dc->start(); setupServos(); + setupJoints(); initROS(); LED.setOut(); @@ -232,13 +254,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; @@ -249,7 +270,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); } 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..3377c06 100644 --- a/utils.h +++ b/utils.h @@ -1,5 +1,13 @@ -#ifndef _UTILS_H_ -#define _UTILS_H_ +#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) { @@ -11,4 +19,68 @@ 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; + } +}; + +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 diff --git a/wheel.cpp b/wheel.cpp index 969da65..75cf339 100644 --- a/wheel.cpp +++ b/wheel.cpp @@ -2,105 +2,120 @@ #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), + ticks_now_(0), + ticks_sum_(0), + dt_sum_(0), + v_now_(0.0), + v_target_(0.0), + encoder_buffer_(encoder_buffer_size_) { - vReg.setScale(1); - vReg.setRange(-vRange, vRange); - vReg.setIRange(-vRange, vRange); - vReg.setCoeffs(Kp, Ki, Kd); - - if (pol) { - mot.setMotorPolarity(Polarity::Reversed); - mot.setEncoderPolarity(Polarity::Reversed); + 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 (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(); + int32_t ticks_prev = ticks_now_; + ticks_now_ = motor_.getEncoderCnt(); + + 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; - vNow = (dNow - dPrev) / (dt * 0.001); + v_now_ = static_cast(ticks_sum_) / (dt_sum_ * 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 ticks_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..50c9e1a 100644 --- a/wheel.h +++ b/wheel.h @@ -1,32 +1,17 @@ -#ifndef _WHEEL_H_ -#define _WHEEL_H_ +#ifndef LEO_FIRMWARE_WHEEL_H_ +#define LEO_FIRMWARE_WHEEL_H_ #include #include #include "hFramework.h" #include "hCyclicBuffer.h" +#include "utils.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 +26,30 @@ class Wheel { void reset(); void turnOff(); void turnOn(); + +private: + hMotor& motor_; + hPIDRegulator v_reg_; + CircularBuffer> encoder_buffer_; + + int16_t power_; + + bool turned_on_; + + int32_t ticks_now_; + int32_t ticks_sum_; + uint32_t dt_sum_; + + float v_target_; + float v_now_; + + const bool polarity_; + 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; }; #endif