80 lines
1.6 KiB
C++
80 lines
1.6 KiB
C++
#include "engine.hpp"
|
|
|
|
#include <chrono>
|
|
#include <utility>
|
|
|
|
using namespace std;
|
|
using namespace std::chrono;
|
|
|
|
engine::engine(gpio&& pin_forward, gpio&& pin_reverse)
|
|
: speed(0),
|
|
pin_forward(move(pin_forward)),
|
|
pin_reverse(move(pin_reverse)),
|
|
stop_thread(false),
|
|
measurement(string("engine") + to_string(this->pin_forward.get_pin()))
|
|
{
|
|
this->pin_forward.set_value(false);
|
|
this->pin_reverse.set_value(false);
|
|
pwm_thread = thread(&engine::pwm_loop, this);
|
|
}
|
|
|
|
engine::~engine()
|
|
{
|
|
stop_thread = true;
|
|
pwm_thread.join();
|
|
}
|
|
|
|
void engine::set_speed(int speed)
|
|
{
|
|
this->speed.store(speed, memory_order_relaxed);
|
|
}
|
|
|
|
void engine::pwm_loop()
|
|
{
|
|
this_thread::sleep_for(milliseconds(100));
|
|
const int sleep_msec = 1;
|
|
int counter = 0;
|
|
high_resolution_clock::time_point next_sleep_target = high_resolution_clock::now();
|
|
while (!stop_thread)
|
|
{
|
|
measurement.start();
|
|
direction target_direction;
|
|
int speed_val = speed.load(memory_order_relaxed);
|
|
if (speed_val < 0)
|
|
{
|
|
target_direction = direction::REVERSE;
|
|
speed_val *= -1;
|
|
}
|
|
else
|
|
{
|
|
target_direction = direction::FORWARD;
|
|
}
|
|
counter += sleep_msec;
|
|
if (counter >= 100)
|
|
counter = 0;
|
|
if (counter < speed_val)
|
|
{
|
|
if (target_direction == direction::FORWARD)
|
|
{
|
|
pin_reverse.set_value(false);
|
|
pin_forward.set_value(true);
|
|
}
|
|
else
|
|
{
|
|
pin_forward.set_value(false);
|
|
pin_reverse.set_value(true);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pin_forward.set_value(false);
|
|
pin_reverse.set_value(false);
|
|
}
|
|
measurement.stop();
|
|
next_sleep_target += 500us;
|
|
this_thread::sleep_until(next_sleep_target);
|
|
}
|
|
pin_forward.set_value(false);
|
|
pin_reverse.set_value(false);
|
|
}
|