Merge branch 'devel'

LeoRover · Aug 7, 2019 · 8b8f4b6 · 8b8f4b6
2 parents d267886 + e435364
commit 8b8f4b6
Show file tree

Hide file tree

Showing 8 changed files with 323 additions and 175 deletions.
diff --git 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
+[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
diff --git 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<float> DiffController::getOdom()
 {
     std::vector<float> 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<float> DiffController::getWheelPositions()
+{
+    std::vector<float> 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<float> DiffController::getWheelVelocities()
+{
+    std::vector<float> 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<float> DiffController::getWheelEfforts()
+{
+    std::vector<float> 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
@@ -1,18 +1,20 @@
-#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 <vector>
 
+#include "wheel.h"
+
 class DiffController
 {
 public:
     DiffController(uint32_t input_timeout = 0);
     void start();
     void setSpeed(float linear, float angular);
     std::vector<float> getOdom();
+    std::vector<float> getWheelPositions();
+    std::vector<float> getWheelVelocities();
+    std::vector<float> 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