Update sem code

UML1/Makefile

@@ -0,0 +1,9 @@
+CXX = g++
+CXXFLAGS = -Wall -g --std=c++11
+
+main: superpoohMQTT.o
+	$(CXX) -o main superpoohMQTT.o -lpthread -lmosquittopp -lmosquitto -lboost_system
+
+superpoohMQTT.o: superpoohMQTT.cpp
+	clang-format -i --style=webkit superpoohMQTT.cpp
+	$(CXX) $(CXXFLAGS) -c superpoohMQTT.cpp

UML1/superpoohMQTT.cpp

@@ -0,0 +1,352 @@
+#include <boost/array.hpp>
+#include <boost/asio.hpp>
+#include <chrono>
+#include <cmath>
+#include <future>
+#include <iostream>
+#include <mosquitto.h>
+#include <mutex>
+#include <thread>
+
+#define mqtt_host "localhost"
+#define mqtt_port 1883
+
+using namespace std;
+
+class PIDImpl {
+public:
+    // Kp - proportional gain
+    // Ki - Integral gain
+    // Kd - derivative gain
+    // dt - loop interval time
+    // max - maximum value of manipulated variable
+    // min - minimum value of manipulated variable
+    PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki);
+    ~PIDImpl();
+    // Returns the manipulated variable given a setpoint and current process value
+    double calculate(double setpoint, double pv);
+
+private:
+    double _dt;
+    double _max;
+    double _min;
+    double _Kp;
+    double _Kd;
+    double _Ki;
+    double _pre_error;
+    double _integral;
+};
+
+/**
+* Implementation
+*/
+PIDImpl::PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki)
+    : _dt(dt)
+    , _max(max)
+    , _min(min)
+    , _Kp(Kp)
+    , _Kd(Kd)
+    , _Ki(Ki)
+    , _pre_error(0)
+    , _integral(0)
+{
+}
+
+double PIDImpl::calculate(double setpoint, double pv)
+{
+
+    // Calculate error
+    double error = setpoint - pv;
+
+    // Proportional term
+    double Pout = _Kp * error;
+
+    // Integral term
+    _integral += error * _dt;
+    double Iout = _Ki * _integral;
+
+    // Derivative term
+    double derivative = (error - _pre_error) / _dt;
+    double Dout = _Kd * derivative;
+
+    // Calculate total output
+    double output = Pout + Iout + Dout;
+
+    //std::cout<<"out="<<output<< "Pout = "<< Pout <<" " <<Iout <<" "<< Dout <<std::endl;
+    // Restrict to max/min
+    if (output > _max)
+        output = _max;
+    else if (output < _min)
+        output = _min;
+
+    std::cout << "outB=" << output << std::endl;
+
+    // Save error to previous error
+    _pre_error = error;
+
+    return output;
+}
+
+PIDImpl::~PIDImpl()
+{
+}
+
+enum ECmd {
+    ENone,
+    EExit
+};
+
+ECmd cmd = ENone;
+
+int counter = 0;
+
+std::mutex mtx;
+
+float targetH = 10.;
+
+float getHZadFromUser()
+{
+    return 10.;
+}
+
+class Bear {
+    float h;
+    float m;
+    float v;
+    float a;
+
+public:
+    Bear()
+    {
+        v = 0; // m/s
+        a = 0; // m/s2
+        m = 10; // kg
+        h = 0; // Текущая высота
+    }
+    float getH()
+    {
+        return h;
+    }
+    float getV()
+    {
+        return v;
+    }
+    float getM()
+    {
+        return m;
+    }
+    float getA()
+    {
+        return a;
+    }
+    virtual void calc(float dt, float extrnForce)
+    {
+        float dh;
+        v = v + a * dt;
+        dh = v * dt;
+        h = h + dh;
+        a = (extrnForce - 9.8 * m) / m; // m * a = mg - F
+
+        if (h < 0) { // Stay on the ground
+            h = 0.;
+            v = 0.;
+            a = 0.;
+        }
+    }
+
+    void eat(float mass)
+    {
+        m = m + mass;
+    }
+
+    virtual void show()
+    {
+        std::cout << "h=" << h << std::endl;
+    }
+};
+
+class Engine;
+
+class Engine {
+    float enginePower = 0; // n 0..500 n
+    float maxPower; // n
+public:
+    Engine(float imaxPower)
+    {
+        enginePower = 0.;
+        maxPower = imaxPower;
+    }
+
+    void setPower(float enginePowerPercent)
+    {
+        if (enginePowerPercent > 100.)
+            enginePowerPercent = 100;
+        if (enginePowerPercent < -100.)
+            enginePowerPercent = -100;
+
+        enginePower = enginePowerPercent * (maxPower / 100.);
+    }
+
+    float getForce()
+    {
+        return enginePower;
+    }
+};
+
+class SmartFlyingBear : public Bear {
+    PIDImpl* pid;
+    Engine* pengine;
+    float hZad;
+
+public:
+    SmartFlyingBear(PIDImpl* ppid, Engine* ppengine)
+    {
+        pid = ppid;
+        pengine = ppengine;
+        hZad = 0;
+    }
+
+    void setHZad(float val)
+    {
+        hZad = val;
+    }
+
+    virtual void calc(float dt, float extrnForce)
+    {
+        float enginePowerPercent = 0;
+        if (pid != nullptr) {
+            enginePowerPercent = pid->calculate(hZad, getH());
+        }
+
+        if (pengine != nullptr) {
+            pengine->setPower(enginePowerPercent);
+            Bear::calc(dt, extrnForce + pengine->getForce());
+        }
+    }
+};
+
+int model()
+{
+
+    //PIDImpl pid(0.1, 100, -100, 0.05, 0.001, 0.1);
+    //Bear pooh;
+    SmartFlyingBear superpooh(new PIDImpl(0.1, 100, -100, 0.15, 0.001, 0.01), new Engine(500.));
+
+    float t = 0; // Время моделирования
+    float dt = 0.1; // s
+    float lastT = -5;
+
+    float hZad = 0; // Измененение высоты за шаг моделирования
+
+    while (cmd != EExit) {
+        mtx.lock();
+        hZad = targetH; // Неблокирующая функция для получения высоты
+        mtx.unlock();
+        if (abs(superpooh.getH() - hZad) < 1 && abs(t - lastT) > 5) {
+            superpooh.eat(0.1);
+            lastT = t;
+        }
+        superpooh.setHZad(hZad);
+        superpooh.show();
+        superpooh.calc(dt, 0);
+
+        t = t + dt;
+
+        if (t < 200) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        } else {
+            std::cout << "Weight = " << superpooh.getM() << std::endl;
+            std::cout << "Velocity = " << superpooh.getV() << std::endl;
+            std::cout << "Acceleration = " << superpooh.getA() << std::endl;
+            std::cout << "Time = " << t << std::endl;
+            std::cout << "===========" << std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(50));
+        }
+    }
+
+    return 0;
+}
+
+void handle_signal(int s)
+{
+    cmd = EExit;
+}
+
+void connect_callback(struct mosquitto* mosq, void* obj, int result)
+{
+    printf("connect callback, rc=%d\n", result);
+}
+
+void message_callback(struct mosquitto* mosq, void* obj, const struct mosquitto_message* message)
+{
+    bool match = 0;
+    printf("got message '%.*s' for topic '%s'\n", message->payloadlen, (char*)message->payload, message->topic);
+
+    mosquitto_topic_matches_sub("/pooh/height", message->topic, &match);
+    if (match) {
+        std::cout << "Going up!" << std::endl;
+        mtx.lock();
+        targetH = std::atof((char*)message->payload);
+        mtx.unlock();
+    }
+
+    mosquitto_topic_matches_sub("/pooh/land", message->topic, &match);
+    if (match) {
+        std::cout << "Going down!" << std::endl;
+        mtx.lock();
+        targetH = 0.;
+        mtx.unlock();
+    }
+
+    mosquitto_topic_matches_sub("/pooh/quit", message->topic, &match);
+    if (match) {
+        std::cout << "Exiting!" << std::endl;
+        cmd = EExit;
+    }
+}
+
+int main()
+{
+
+    char clientid[24];
+    struct mosquitto* mosq;
+    int rc = 0;
+
+    signal(SIGINT, handle_signal);
+    signal(SIGTERM, handle_signal);
+
+    mosquitto_lib_init();
+
+    memset(clientid, 0, 24);
+    snprintf(clientid, 23, "mysql_log_%d", getpid());
+    mosq = mosquitto_new(clientid, true, 0);
+
+    try {
+        std::thread th(model);
+        if (mosq) {
+            mosquitto_connect_callback_set(mosq, connect_callback);
+            mosquitto_message_callback_set(mosq, message_callback);
+
+            rc = mosquitto_connect(mosq, mqtt_host, mqtt_port, 60);
+
+            mosquitto_subscribe(mosq, NULL, "/pooh/height", 0);
+            mosquitto_subscribe(mosq, NULL, "/pooh/land", 0);
+            mosquitto_subscribe(mosq, NULL, "/pooh/quit", 0);
+
+            while (cmd != EExit) {
+                rc = mosquitto_loop(mosq, -1, 1);
+                if (cmd != EExit && rc) {
+                    printf("connection error!\n");
+                    sleep(10);
+                    mosquitto_reconnect(mosq);
+                }
+            }
+            th.join();
+            mosquitto_destroy(mosq);
+        }
+    } catch (std::exception& e) {
+        std::cerr << e.what() << std::endl;
+    }
+    mosquitto_lib_cleanup();
+    return rc;
+}