diff --git a/CMakeLists.txt b/CMakeLists.txt index 8afb7d4..7828d4f 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/ILocationProvider.hpp b/src/ILocationProvider.hpp new file mode 100644 index 0000000..f472cf5 --- /dev/null +++ b/src/ILocationProvider.hpp @@ -0,0 +1,14 @@ +#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 1d49f2e..4778dd2 100644 --- a/src/PosTracker.cpp +++ b/src/PosTracker.cpp @@ -1,4 +1,5 @@ #include "PosTracker.hpp" +#include 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; +} \ No newline at end of file diff --git a/src/PosTracker.hpp b/src/PosTracker.hpp index 7400634..4ac1808 100644 --- a/src/PosTracker.hpp +++ b/src/PosTracker.hpp @@ -7,6 +7,8 @@ #include #include +# 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; diff --git a/src/SimpleLocTracker.hpp b/src/SimpleLocTracker.hpp new file mode 100644 index 0000000..ce46b49 --- /dev/null +++ b/src/SimpleLocTracker.hpp @@ -0,0 +1,30 @@ +#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 efe2370..d801ce8 100644 --- a/src/main.cpp +++ b/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 #include #include +#include #include #include +#include #include +#include +#include using sf::Vector2f; @@ -39,13 +45,25 @@ 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) { +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; diff --git a/src/robot.cpp b/src/robot.cpp index e2eb84d..16952cf 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1,9 +1,33 @@ #include "robot.hpp" +#include "ILocationProvider.hpp" +#include "SimpleLocTracker.hpp" +#include "utils.hpp" +#include +#include #include -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); diff --git a/src/robot.hpp b/src/robot.hpp index fc9a6fc..bcf4746 100644 --- a/src/robot.hpp +++ b/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 { 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); diff --git a/src/simpleLocTracker.cpp b/src/simpleLocTracker.cpp new file mode 100644 index 0000000..4c58929 --- /dev/null +++ b/src/simpleLocTracker.cpp @@ -0,0 +1,40 @@ +#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 eb02f33..76b1a15 100644 --- a/src/utils.hpp +++ b/src/utils.hpp @@ -1,5 +1,63 @@ #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); } +}