diff --git a/CMakeLists.txt b/CMakeLists.txt index 7828d4f..8afb7d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ add_executable(robotCommand src/jsonrpctcpclient.cpp src/PosTracker.cpp src/PosHistory.cpp - src/simpleLocTracker.cpp ) find_package(jsoncpp REQUIRED) diff --git a/src/ILocationProvider.hpp b/src/ILocationProvider.hpp deleted file mode 100644 index f472cf5..0000000 --- a/src/ILocationProvider.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include -#include - -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; -}; diff --git a/src/PosTracker.cpp b/src/PosTracker.cpp index 4778dd2..1d49f2e 100644 --- a/src/PosTracker.cpp +++ b/src/PosTracker.cpp @@ -1,5 +1,4 @@ #include "PosTracker.hpp" -#include using sf::Vector2f; using sf::Transform; @@ -8,12 +7,12 @@ void PosTracker::reset(Vector2f initialPos, float initialRot) { transform = transformFromPosRot(initialPos, initialRot); } -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; +void PosTracker::update(float newLeftValue, float newRightValue) { + auto leftDistance = (newLeftValue - _lastLeftValue) * wheelIncrementDistance(); + auto rightDistance = (newRightValue - _lastRightValue) * wheelIncrementDistance(); updateTransform(leftDistance, rightDistance); - _lastLeftValue = value_left; - _lastRightValue = value_right; + _lastLeftValue = newLeftValue; + _lastRightValue = newRightValue; } void PosTracker::updateTransform(float leftDistance, float rightDistance) { @@ -27,13 +26,3 @@ 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; -} \ No newline at end of file diff --git a/src/PosTracker.hpp b/src/PosTracker.hpp index 4ac1808..7400634 100644 --- a/src/PosTracker.hpp +++ b/src/PosTracker.hpp @@ -7,8 +7,6 @@ #include #include -# include "ILocationProvider.hpp" - inline sf::Transformable transformFromPosRot(sf::Vector2f position, float rotation) { auto result = sf::Transformable(); result.setPosition(position); @@ -16,30 +14,26 @@ inline sf::Transformable transformFromPosRot(sf::Vector2f position, float rotati return result; } -class PosTracker : public ILocationProvider { +class PosTracker { /// 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) 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; + void reset(sf::Vector2f initialPos, float initialRot); + void update(float newLeftValue, float newRightValue); - float get_rot() override; - sf::Vector2f get_pos() override; + inline sf::Vector2f getPosition() const { return transform.getPosition(); } + inline float getRotation() const { return transform.getRotation(); } private: inline float wheelIncrementDistance() const { return _wheelDiameter * std::numbers::pi / _wheelStepsPerTurn; diff --git a/src/SimpleLocTracker.hpp b/src/SimpleLocTracker.hpp deleted file mode 100644 index ce46b49..0000000 --- a/src/SimpleLocTracker.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - - -#include "ILocationProvider.hpp" -#include -#include - -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 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{}; -}; diff --git a/src/main.cpp b/src/main.cpp index d801ce8..efe2370 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,5 @@ -#include "ILocationProvider.hpp" #include "PosHistory.hpp" #include "PosTracker.hpp" -#include "SimpleLocTracker.hpp" #include "filter.hpp" #include "robot.hpp" #include "utils.hpp" @@ -16,13 +14,9 @@ #include #include #include -#include #include #include -#include #include -#include -#include using sf::Vector2f; @@ -45,25 +39,13 @@ void controller_follow_border(Robot &robot, float speed_factor) { } } -std::tuple 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, ILocationProvider& pos) { +void event(sf::RenderWindow & window, Robot& robot) { bool do_stop_ = do_stop; sf::Event event; while (window.pollEvent(event)) { @@ -73,15 +55,6 @@ void event(sf::RenderWindow & window, Robot& robot, ILocationProvider& pos) { } 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); @@ -101,56 +74,51 @@ class ControlWindow { Robot& _robot; sf::RenderWindow _window; sf::RectangleShape _robotShape; - ILocationProvider& _location; + PosTracker _tracker; 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, - ILocationProvider &location) - : _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, float filter_delay) : + _robot(robot), + _window(sf::RenderWindow( + sf::VideoMode(800u, 600u), + "robot-command", + sf::Style::Titlebar | sf::Style::Close + )), _robotShape(sf::RectangleShape(robot_size)), - _location(location), + _tracker(PosTracker(Vector2f(90, 90), 0., robot_size.y, wheel_diameter, latice_count)), _history(PosHistory(history_count)), - _speed_factor(speed_factor) + _speed_factor(speed_factor), + _filter(filter_delay) { _window.setVerticalSyncEnabled(true); _robotShape.setOrigin(sf::Vector2f(robot_size.x / 2, robot_size.y / 2)); _robotShape.setFillColor(sf::Color::Blue); - } - - 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); + _tracker.update(robot.getEncoderWheelValue(1), robot.getEncoderWheelValue(0)); + _tracker.reset(Vector2f(90, 90), 0.); } void run() { while (_window.isOpen()) { update(); - // controller_follow_border_step(_robot, _speed_factor); - usleep(20 * 1000); + controller_follow_border_step(_robot, _speed_factor); } } private: void update() { - auto left = _robot.getEncoderWheelValue(1); - auto right = _robot.getEncoderWheelValue(0); - _location.update(left, right); + _tracker.update(_robot.getEncoderWheelValue(1), _robot.getEncoderWheelValue(0)); - _robotShape.setPosition(scaled(_location.get_pos(), 0.2)); - _robotShape.setRotation(rad_to_deg(_location.get_rot())); - _history.append(_location.get_pos()); + _robotShape.setPosition(_tracker.getPosition()); + _robotShape.setRotation(_tracker.getRotation()); + _history.append(_tracker.getPosition()); - event(_window, _robot, _location); + event(_window, _robot); _window.clear(); - // _window.draw(_robotShape); - // _window.draw(_history.vertices()); + _window.draw(_robotShape); + _window.draw(_history.vertices()); _window.display(); } }; @@ -171,32 +139,15 @@ int main(int argc, char ** argv) } std::cout << "Try to connect to " << hostIpAddress << ":" << tcpPort << " ..." << std::endl; - // 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(); + Robot robot(hostIpAddress, tcpPort); + robot.waitReady(); try { // #define STEP_1 #ifdef STEP_1 controller_follow_border(robot); #else - auto control = ControlWindow( - *robot, - Vector2f(13, ROBOT_SIZE), WHEEL_DIAMETER, LATICE_COUNT, - 4000, 0.78, - *location - ); - control.reset(Vector2f(300, 300), 0.); - control.run(); + ControlWindow(robot, Vector2f(ROBOT_SIZE, ROBOT_SIZE), WHEEL_DIAMETER, LATICE_COUNT, 4000, 0.78, 1).run(); #endif } catch (std::exception &e) { std::cerr << e.what() << std::endl; diff --git a/src/robot.cpp b/src/robot.cpp index 16952cf..e2eb84d 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1,33 +1,9 @@ #include "robot.hpp" -#include "ILocationProvider.hpp" -#include "SimpleLocTracker.hpp" -#include "utils.hpp" -#include -#include #include -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) +Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort) : _jsonRpcTcpClient(hostIpAddress, tcpPort) , _irProximitysDistanceDetected(this) , _lineTracksIsDetected(this, 1, false) @@ -38,7 +14,6 @@ Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort, ILocationProvi , _isReadySemaphore(0) , _eventCvMutex() , _eventCv() - , _loc_tracker(loc_tracker) { _jsonRpcTcpClient.bindNotification("irProximityDistanceDetected", [this](const Json::Value & params){ std::size_t index = params["index"].asUInt(); @@ -53,10 +28,8 @@ Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort, ILocationProvi _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(); - auto steps = params["changedCount"].asInt(); - _encoderWheelsValue.set(index, params["value"].asUInt(), steps); + _encoderWheelsValue.set(index, params["value"].asUInt(), params["changedCount"].asInt()); }); _jsonRpcTcpClient.bindNotification("switchIsDetected", [this](const Json::Value & params){ std::size_t index = params["index"].asUInt(); @@ -84,7 +57,6 @@ void Robot::setMotorPower(MotorIndex motorIndex, float value) _jsonRpcTcpClient.callNotification("setMotorPower", params); } - void Robot::setMotorsPower(float rightValue, float leftValue) { Json::Value params; @@ -93,37 +65,6 @@ 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); diff --git a/src/robot.hpp b/src/robot.hpp index bcf4746..fc9a6fc 100644 --- a/src/robot.hpp +++ b/src/robot.hpp @@ -1,7 +1,6 @@ #ifndef ROBOT_HPP #define ROBOT_HPP -#include "SimpleLocTracker.hpp" #include "jsonrpctcpclient.hpp" #include "values.hpp" @@ -25,9 +24,8 @@ enum class EventType class Robot : public IRobot { 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, ILocationProvider* loc_tracker); + Robot(const std::string & hostIpAddress, uint16_t tcpPort); //! @brief Close the robot connexion. virtual ~Robot(); @@ -68,9 +66,6 @@ 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); diff --git a/src/simpleLocTracker.cpp b/src/simpleLocTracker.cpp deleted file mode 100644 index 4c58929..0000000 --- a/src/simpleLocTracker.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "SimpleLocTracker.hpp" -#include - - -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()); -} diff --git a/src/utils.hpp b/src/utils.hpp index 76b1a15..eb02f33 100644 --- a/src/utils.hpp +++ b/src/utils.hpp @@ -1,63 +1,5 @@ #pragma once #include -#include -#include -#include -#include - #define DBG(X) ({ auto value = X; std::cout << "(DBG) [" << __FILE__ << ":" << __LINE__ << "] (" << #X << ") = " << value << "\n"; value;}) - - -inline namespace utils { - - template - concept is_numeric= std::integral || std::floating_point; - - template requires(is_numeric) - 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& start, const RangeIterator& end) { return start._index >= end._index; } - inline friend bool operator!= (const RangeIterator& start, const RangeIterator& end) { return !(start._index >= end._index); } - private: - T _index; - T _increment; - }; - - template requires(is_numeric) - 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 begin() { - return RangeIterator(_from, _increment); - } - const inline RangeIterator end() { - return RangeIterator(_to, _increment); - } - private: - T _from; - T _to; - T _increment; - }; - - - template requires(is_numeric) - inline T vectorLength(const sf::Vector2& 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); } -}