working controller

CMakeLists.txt

@@ -15,6 +15,7 @@ add_executable(robotCommand
     src/jsonrpctcpclient.cpp
     src/PosTracker.cpp
     src/PosHistory.cpp
+    src/simpleLocTracker.cpp
 )
 
 find_package(jsoncpp REQUIRED)

src/ILocationProvider.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <SFML/System/Vector2.hpp>
+#include <cstddef>
+
+class ILocationProvider {
+  public:
+  virtual sf::Vector2f get_pos() = 0;
+  virtual float get_rot() = 0;
+  virtual void update(std::size_t value_left, std::size_t value_right) = 0;
+  virtual void reset(sf::Vector2f pos, float rot) = 0;
+  virtual void hint_right(bool forward) = 0;
+  virtual void hint_left(bool forward) = 0;
+};

src/PosTracker.cpp

@@ -1,4 +1,5 @@
 #include "PosTracker.hpp"
+#include <SFML/System/Vector2.hpp>
 
 using sf::Vector2f;
 using sf::Transform;
@@ -7,12 +8,12 @@ void PosTracker::reset(Vector2f initialPos, float initialRot) {
     transform = transformFromPosRot(initialPos, initialRot);
 }
 
-void PosTracker::update(float newLeftValue, float newRightValue) {
-    auto leftDistance = (newLeftValue - _lastLeftValue) * wheelIncrementDistance();
-    auto rightDistance = (newRightValue - _lastRightValue) * wheelIncrementDistance();
+void PosTracker::update(std::size_t value_left, std::size_t value_right) {
+    auto leftDistance = (value_left - _lastLeftValue) * wheelIncrementDistance() * _wheel_dir_left;
+    auto rightDistance = (value_right - _lastRightValue) * wheelIncrementDistance() * _wheel_dir_right;
     updateTransform(leftDistance, rightDistance);
-    _lastLeftValue = newLeftValue;
-    _lastRightValue = newRightValue;
+    _lastLeftValue = value_left;
+    _lastRightValue = value_right;
 }
 
 void PosTracker::updateTransform(float leftDistance, float rightDistance) {
@@ -26,3 +27,13 @@ void PosTracker::updateTransform(float leftDistance, float rightDistance) {
     );
     transform = new_;
 }
+
+Vector2f PosTracker::get_pos() { return this->transform.getPosition(); }
+float PosTracker::get_rot() { return this->transform.getRotation(); }
+
+void PosTracker::hint_right(bool forward) {
+    _wheel_dir_right = forward ? 1 : -1;
+}
+void PosTracker::hint_left(bool forward) {
+    _wheel_dir_left = forward ? 1 : -1;
+}

src/PosTracker.hpp

@@ -7,6 +7,8 @@
 #include <SFML/System/Vector2.hpp>
 #include <SFML/Window/Event.hpp>
 
+# include "ILocationProvider.hpp"
+
 inline sf::Transformable transformFromPosRot(sf::Vector2f position, float rotation) {
     auto result = sf::Transformable();
     result.setPosition(position);
@@ -14,26 +16,30 @@ inline sf::Transformable transformFromPosRot(sf::Vector2f position, float rotati
     return result;
 }
 
-class PosTracker {
+class PosTracker : public ILocationProvider {
     /// Wheel encoder values.
     float _wheelDistance, _wheelDiameter;
     float _wheelStepsPerTurn;
     float _lastLeftValue, _lastRightValue;
+    float _wheel_dir_left, _wheel_dir_right;
     sf::Transformable transform;
     public:
-        PosTracker(sf::Vector2f initialPos, float initialRot, float wheelDistance, float wheelDiameter, float wheelStepsPerTurn) :
-            _wheelDistance(wheelDistance),
-            _wheelDiameter(wheelDiameter),
-            _wheelStepsPerTurn(wheelStepsPerTurn),
-            _lastLeftValue(0.),
+      PosTracker(sf::Vector2f initialPos, float initialRot, float wheelDistance,
+                 float wheelDiameter, float wheelStepsPerTurn)
+          : _wheelDistance(wheelDistance), _wheelDiameter(wheelDiameter),
+            _wheelStepsPerTurn(wheelStepsPerTurn), _lastLeftValue(0.),
             _lastRightValue(0.),
+            _wheel_dir_left(1.),
+            _wheel_dir_right(1.),
             transform(transformFromPosRot(initialPos, initialRot))
         {}
-        void reset(sf::Vector2f initialPos, float initialRot);
-        void update(float newLeftValue, float newRightValue);
+        void reset(sf::Vector2f initialPos, float initialRot) override;
+        void update(std::size_t value_left, std::size_t value_right) override;
+        virtual void hint_right(bool forward) override;
+        virtual void hint_left(bool forward) override;
 
-        inline sf::Vector2f getPosition() const { return transform.getPosition(); }
-        inline float getRotation() const { return transform.getRotation(); }
+        float get_rot() override;
+        sf::Vector2f get_pos() override;
     private:
         inline float wheelIncrementDistance() const {
             return _wheelDiameter * std::numbers::pi / _wheelStepsPerTurn;

src/SimpleLocTracker.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+
+#include "ILocationProvider.hpp"
+#include <SFML/System/Vector2.hpp>
+#include <optional>
+
+struct Mission {
+    sf::Vector2f from_pos, to_pos;
+    float from_rot, to_rot;
+    int total_encodings;
+    int current_encodings;
+    inline float progress() { return float(current_encodings) / float(total_encodings); }
+};
+
+class SimpleLocTracker : public ILocationProvider {
+        std::optional<Mission> current_mission;
+        sf::Vector2f last_pos;
+        float last_rot;
+        int last_step;
+
+    public:
+        inline SimpleLocTracker(sf::Vector2f pos, float rot) : last_pos(pos), last_rot(rot) {}
+        void start(sf::Vector2f to_pos, float to_rot, int total_encodings);
+        void progress();
+        sf::Vector2f get_pos() override;
+        float get_rot() override;
+        void update(std::size_t value_left, std::size_t value_right) override {};
+        void reset(sf::Vector2f pos, float rot) override{};
+};

src/main.cpp

@@ -1,5 +1,7 @@
+#include "ILocationProvider.hpp"
 #include "PosHistory.hpp"
 #include "PosTracker.hpp"
+#include "SimpleLocTracker.hpp"
 #include "filter.hpp"
 #include "robot.hpp"
 #include "utils.hpp"
@@ -14,9 +16,13 @@
 #include <SFML/System/Vector2.hpp>
 #include <SFML/Window/Event.hpp>
 #include <SFML/Window/Keyboard.hpp>
+#include <cmath>
 #include <csignal>
 #include <iostream>
+#include <numbers>
 #include <sstream>
+#include <thread>
+#include <unistd.h>
 
 using sf::Vector2f;
 
@@ -39,13 +45,25 @@ void controller_follow_border(Robot &robot, float speed_factor) {
     }
 }
 
+std::tuple<float, float> calc_path(sf::Vector2f pos, float rot, sf::Vector2f dest) {
+
+    auto delta = dest - pos;
+    auto abs_angle = std::atan2(delta.y, delta.x);
+    auto angle = abs_angle - rot;
+    auto distance = vectorLength(delta);
+
+    DBG(delta.x);
+    DBG(delta.y);
+    return { angle, distance };
+}
+
 // note : Ne marche pas car remplace le stack du code interrompu.
 static bool do_stop = false;
 void stop_handler(int) {
     do_stop = true;
 }
 
-void event(sf::RenderWindow & window, Robot& robot) {
+void event(sf::RenderWindow & window, Robot& robot, ILocationProvider& pos) {
     bool do_stop_ = do_stop;
     sf::Event event;
     while (window.pollEvent(event)) {
@@ -55,6 +73,15 @@ void event(sf::RenderWindow & window, Robot& robot) {
         }
         if (event.type == sf::Event::Closed) do_stop_ = true;
         if (event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::Escape) do_stop_ = true;
+        if (event.type == sf::Event::MouseButtonPressed) {
+            auto click = event.mouseButton;
+            // auto [rotation, distance] = calc_path(pos.get_pos(), pos.get_rot(), sf::Vector2f(click.x, click.y));
+            auto [rotation, distance] = calc_path({400, 300}, (std::numbers::pi / 2), sf::Vector2f(800 - click.x, 600 - click.y));
+            DBG(rotation);
+            DBG(distance);
+            robot.doRotate(rotation);
+            robot.doStraightLine(distance * 5);
+        }
     }
     if (do_stop_) {
         robot.setMotorPower(Robot::MotorIndex::LEFT, 0);
@@ -74,51 +101,56 @@ class ControlWindow {
         Robot& _robot;
         sf::RenderWindow _window;
         sf::RectangleShape _robotShape;
-        PosTracker _tracker;
+        ILocationProvider& _location;
         PosHistory _history;
-        MotorJitterFilter _filter;
         float _speed_factor;
 
     public:
-        ControlWindow(Robot& robot, Vector2f robot_size, float wheel_diameter, float latice_count, int history_count, float speed_factor, float filter_delay) :
-            _robot(robot),
-            _window(sf::RenderWindow(
-            sf::VideoMode(800u, 600u),
-            "robot-command",
-            sf::Style::Titlebar | sf::Style::Close
-            )),
+      ControlWindow(Robot &robot, Vector2f robot_size, float wheel_diameter,
+                    float latice_count, int history_count, float speed_factor,
+                    ILocationProvider &location)
+          : _robot(robot),
+            _window(sf::RenderWindow(sf::VideoMode(800u, 600u), "robot-command",
+                                     sf::Style::Titlebar | sf::Style::Close)),
             _robotShape(sf::RectangleShape(robot_size)),
-            _tracker(PosTracker(Vector2f(90, 90), 0., robot_size.y, wheel_diameter, latice_count)),
+            _location(location),
             _history(PosHistory(history_count)),
-            _speed_factor(speed_factor),
-            _filter(filter_delay)
+            _speed_factor(speed_factor)
         {
             _window.setVerticalSyncEnabled(true);
             _robotShape.setOrigin(sf::Vector2f(robot_size.x / 2, robot_size.y / 2));
             _robotShape.setFillColor(sf::Color::Blue);
-            _tracker.update(robot.getEncoderWheelValue(1), robot.getEncoderWheelValue(0));
-            _tracker.reset(Vector2f(90, 90), 0.);
+        }
+
+        void reset(Vector2f initial_pos, float initial_rot) {
+            _robot.doStraightLine(20);
+            sleep(2);
+            _location.update(DBG(_robot.getEncoderWheelValue(1)), DBG(_robot.getEncoderWheelValue(0)));
+            _location.reset(initial_pos, initial_rot);
         }
 
         void run() {
             while (_window.isOpen()) {
                 update();
-                controller_follow_border_step(_robot, _speed_factor);
+                // controller_follow_border_step(_robot, _speed_factor);
+                usleep(20 * 1000);
             }
         }
 
     private:
         void update() {
-            _tracker.update(_robot.getEncoderWheelValue(1), _robot.getEncoderWheelValue(0));
+            auto left = _robot.getEncoderWheelValue(1);
+            auto right = _robot.getEncoderWheelValue(0);
+            _location.update(left, right);
 
-            _robotShape.setPosition(_tracker.getPosition());
-            _robotShape.setRotation(_tracker.getRotation());
-            _history.append(_tracker.getPosition());
+            _robotShape.setPosition(scaled(_location.get_pos(), 0.2));
+            _robotShape.setRotation(rad_to_deg(_location.get_rot()));
+            _history.append(_location.get_pos());
 
-            event(_window, _robot);
+            event(_window, _robot, _location);
             _window.clear();
-            _window.draw(_robotShape);
-            _window.draw(_history.vertices());
+            // _window.draw(_robotShape);
+            // _window.draw(_history.vertices());
             _window.display();
         }
 };
@@ -139,15 +171,32 @@ int main(int argc, char ** argv)
     }
 
     std::cout << "Try to connect to " << hostIpAddress << ":" << tcpPort << " ..." << std::endl;
-    Robot robot(hostIpAddress, tcpPort);
-    robot.waitReady();
+    // auto location = new SimpleLocTracker(Vector2f(300, 300), 0);
+    auto location = new PosTracker(Vector2f(300, 300), 0, ROBOT_SIZE, WHEEL_DIAMETER, 20);
+    auto robot = new Robot(hostIpAddress, tcpPort, location);
+    // robot.waitReady();
+    usleep(100 * 1000);
+
+    std::thread([](Robot* robot){
+        usleep(2 * 1000 * 1000);
+        // robot->doStraightLine(500);
+        // robot->doRotate(std::numbers::pi);
+        // robot->doStraightLine(500);
+    }, robot).detach();
 
     try {
         // #define STEP_1
         #ifdef STEP_1
         controller_follow_border(robot);
         #else
-        ControlWindow(robot, Vector2f(ROBOT_SIZE, ROBOT_SIZE), WHEEL_DIAMETER, LATICE_COUNT, 4000, 0.78, 1).run();
+        auto control = ControlWindow(
+            *robot,
+            Vector2f(13, ROBOT_SIZE), WHEEL_DIAMETER, LATICE_COUNT,
+            4000, 0.78,
+            *location
+        );
+        control.reset(Vector2f(300, 300), 0.);
+        control.run();
         #endif
     } catch (std::exception &e) {
       std::cerr << e.what() << std::endl;

src/robot.cpp

@@ -1,9 +1,33 @@
 #include "robot.hpp"
+#include "ILocationProvider.hpp"
+#include "SimpleLocTracker.hpp"
+#include "utils.hpp"
 
+#include <SFML/System/Vector2.hpp>
+#include <cmath>
 #include <json/value.h>
 
 
-Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort)
+sf::Vector2f move_from(sf::Vector2f from, float rot, float distance) {
+    float x = std::cos(rot) * distance;
+    float y = std::sin(rot) * distance;
+    return sf::Vector2f(from.x + x, from.y + y);
+}
+
+
+int distanceToEncodingCount(float distance_mm) {
+    const float distance_per_encoded = 10.21;
+    return int(distance_mm / distance_per_encoded);
+}
+
+
+int angleToEncodingCount(float angle_rad) {
+    const float rad_per_encoded = 0.157;
+    return int(angle_rad / rad_per_encoded);
+}
+
+
+Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort, ILocationProvider* loc_tracker)
     : _jsonRpcTcpClient(hostIpAddress, tcpPort)
     , _irProximitysDistanceDetected(this)
     , _lineTracksIsDetected(this, 1, false)
@@ -14,6 +38,7 @@ Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort)
     , _isReadySemaphore(0)
     , _eventCvMutex()
     , _eventCv()
+    , _loc_tracker(loc_tracker)
 {
     _jsonRpcTcpClient.bindNotification("irProximityDistanceDetected", [this](const Json::Value & params){
         std::size_t index = params["index"].asUInt();
@@ -28,8 +53,10 @@ Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort)
         _lineTracksValue.set(index, params["value"].asUInt(), params["changedCount"].asInt());
     });
     _jsonRpcTcpClient.bindNotification("encoderWheelValue", [this](const Json::Value & params){
+        // this->_loc_tracker->progress();
         std::size_t index = params["index"].asUInt();
-        _encoderWheelsValue.set(index, params["value"].asUInt(), params["changedCount"].asInt());
+        auto steps = params["changedCount"].asInt();
+        _encoderWheelsValue.set(index, params["value"].asUInt(), steps);
     });
     _jsonRpcTcpClient.bindNotification("switchIsDetected", [this](const Json::Value & params){
         std::size_t index = params["index"].asUInt();
@@ -57,6 +84,7 @@ void Robot::setMotorPower(MotorIndex motorIndex, float value)
     _jsonRpcTcpClient.callNotification("setMotorPower", params);
 
 }
+
 void Robot::setMotorsPower(float rightValue, float leftValue)
 {
     Json::Value params;
@@ -65,6 +93,37 @@ void Robot::setMotorsPower(float rightValue, float leftValue)
     _jsonRpcTcpClient.callNotification("setMotorsPower", params);
 }
 
+void Robot::doStraightLine(float distance_mm, float power) {
+    Json::Value params;
+    auto encodingSteps = distanceToEncodingCount(distance_mm);
+    params["steps"] = encodingSteps;
+    params["power"] = power;
+    // DBG(_loc_tracker->get_pos().x);
+    // // DBG(_loc_tracker->get_pos().y);
+    // auto pos = move_from(_loc_tracker->get_pos(), _loc_tracker->get_rot(), distance_mm);
+    // // DBG(pos.x);
+    // // DBG(pos.y);
+    // _loc_tracker->start(pos, _loc_tracker->get_rot(), encodingSteps * 2);
+    _jsonRpcTcpClient.callNotification("straightLine", params);
+    DBG("doStraightLine");
+    DBG(encodingSteps);
+    _loc_tracker->hint_left(true);
+    _loc_tracker->hint_right(true);
+}
+
+void Robot::doRotate(float angle_rad, float power) {
+    Json::Value params;
+    auto encodingSteps = angleToEncodingCount(angle_rad);
+    params["steps"] = encodingSteps;
+    params["power"] = power;
+    // _loc_tracker->start(_loc_tracker->get_pos(), _loc_tracker->get_rot() + angle_rad, encodingSteps * 2);
+    _jsonRpcTcpClient.callNotification("rotate", params);
+    DBG("doRotate");
+    DBG(encodingSteps);
+    _loc_tracker->hint_left(false);
+    _loc_tracker->hint_right(true);
+}
+
 void Robot::waitChanged(EventType eventType) {
     auto eventTypes = waitParamHelper({eventType});
     waitChangedHelper(eventTypes);

src/robot.hpp

@@ -1,6 +1,7 @@
 #ifndef ROBOT_HPP
 #define ROBOT_HPP
 
+#include "SimpleLocTracker.hpp"
 #include "jsonrpctcpclient.hpp"
 #include "values.hpp"
 
@@ -24,8 +25,9 @@ enum class EventType
 class Robot : public IRobot<EventType>
 {
 public:
+    ILocationProvider* _loc_tracker;
     //! @brief Create a new robot connexion with a robot server (simu or reel).
-    Robot(const std::string & hostIpAddress, uint16_t tcpPort);
+    Robot(const std::string & hostIpAddress, uint16_t tcpPort, ILocationProvider* loc_tracker);
 
     //! @brief Close the robot connexion.
     virtual ~Robot();
@@ -66,6 +68,9 @@ public:
     void setMotorsPower(float rightValue, float leftValue);
     //! \}
 
+    void doStraightLine(float distance_mm, float power = 0.25);
+    void doRotate(float angle_rad, float power = 0.25);
+
     //! @brief Wait until this specific event has been received
     void waitChanged(EventType eventType);
 

src/simpleLocTracker.cpp

@@ -0,0 +1,40 @@
+#include "SimpleLocTracker.hpp"
+#include <SFML/System/Vector2.hpp>
+
+
+void SimpleLocTracker::start(sf::Vector2f to_pos, float to_rot, int total_encodings) {
+    last_pos = get_pos();
+    last_rot = get_rot();
+    current_mission = Mission {
+      .from_pos = last_pos, .to_pos = to_pos,
+      .from_rot = last_rot, .to_rot = to_rot,
+      .total_encodings = total_encodings, .current_encodings = 0,
+    };
+}
+
+void SimpleLocTracker::progress() {
+    if (!current_mission.has_value()) return;
+    current_mission.value().current_encodings += 1;
+}
+
+inline float interpolate(float from, float to, float fac) {
+    auto inv = 1. - fac;
+    return from * inv + to * fac;
+}
+
+inline sf::Vector2f interpolate(sf::Vector2f from, sf::Vector2f to, float fac) {
+    return sf::Vector2f(
+        interpolate(from.x, to.x, fac),
+        interpolate(from.y, to.y, fac)
+    );
+}
+
+sf::Vector2f SimpleLocTracker::get_pos() {
+    if (!current_mission.has_value()) return last_pos;
+    return interpolate(current_mission.value().from_pos, current_mission.value().to_pos, current_mission.value().progress());
+}
+
+float SimpleLocTracker::get_rot() {
+    if (!current_mission.has_value()) return last_rot;
+    return interpolate(current_mission.value().from_rot, current_mission.value().to_rot, current_mission.value().progress());
+}

src/utils.hpp

@@ -1,5 +1,63 @@
 #pragma once
 
 #include <iostream>
+#include <SFML/System/Vector2.hpp>
+#include <cmath>
+#include <concepts>
+#include <numbers>
+
 
 #define DBG(X) ({ auto value = X; std::cout << "(DBG) [" << __FILE__ << ":" << __LINE__ << "] (" << #X << ") = " << value << "\n"; value;})
+
+
+inline namespace utils {
+
+    template <typename T>
+    concept is_numeric= std::integral<T> || std::floating_point<T>;
+
+    template<typename T> requires(is_numeric<T>)
+    class RangeIterator {
+        public:
+            inline RangeIterator(T index, T increment): _index(index), _increment(increment) {}
+            const inline T operator*() { return _index; }
+            const inline RangeIterator operator++() { _index += _increment; return *this; }
+            // note : incorrect comparison needed to support saturating iterations.
+            inline friend bool operator== (const RangeIterator<T>& start, const RangeIterator<T>& end) { return start._index >= end._index; }
+            inline friend bool operator!= (const RangeIterator<T>& start, const RangeIterator<T>& end) { return !(start._index >= end._index); }
+        private:
+            T _index;
+            T _increment;
+    };
+
+    template<typename T> requires(is_numeric<T>)
+    class Range {
+        public:
+            inline Range(T to) : _from(0), _to(to), _increment(1) {};
+            inline Range(T from, T to) : _from(from), _to(to), _increment(1) {};
+            inline Range(T from, T to, T increment) : _from(from), _to(to), _increment(increment) {};
+            const inline RangeIterator<T> begin() {
+                return RangeIterator(_from, _increment);
+            }
+            const inline RangeIterator<T> end() {
+                return RangeIterator(_to, _increment);
+            }
+        private:
+            T _from;
+            T _to;
+            T _increment;
+    };
+
+
+    template<typename T> requires(is_numeric<T>)
+    inline T vectorLength(const sf::Vector2<T>& v) {
+        return std::sqrt(v.x * v.x + v.y * v.y);
+    }
+
+
+    inline float deg_to_rad(float deg) { return (deg / 360) * (2 * std::numbers::pi); }
+    inline float rad_to_deg(float rad) {
+      return (rad / (2 * std::numbers::pi)) * (360);
+    }
+
+    inline sf::Vector2f scaled(sf::Vector2f vec, float fac) { return sf::Vector2f(vec.x * fac, vec.y * fac); }
+}