|
|
@ -3,10 +3,13 @@ |
|
|
|
#include <queue> |
|
|
|
#include <cmath> |
|
|
|
#include <thread> |
|
|
|
#include <string> |
|
|
|
#include <memory> |
|
|
|
|
|
|
|
#include <iostream> |
|
|
|
|
|
|
|
namespace simon { |
|
|
|
namespace a4988 { |
|
|
|
c_allegro_4988::c_allegro_4988(a4988_settings_t* pset) |
|
|
|
: m_pset(pset), m_initialized(false), m_alive(true) |
|
|
|
{ |
|
|
@ -21,16 +24,20 @@ namespace simon { |
|
|
|
{ |
|
|
|
std::cout << "In thread" << std::endl; |
|
|
|
if (!m_queue.empty()) { |
|
|
|
auto& move = m_queue.front(); |
|
|
|
if (!set_pwm_freq(std::round(1e6/move.period_us))) { |
|
|
|
auto cmd = m_queue.front(); |
|
|
|
// Distinguish between different tytpes of commands and derive type
|
|
|
|
if (*cmd == "move") { |
|
|
|
auto move = std::static_pointer_cast<c_move>(cmd); |
|
|
|
if (!set_pwm_freq(std::round(1e6/move->period_us))) { |
|
|
|
// TODO: proper error handling
|
|
|
|
} |
|
|
|
gpioHardwarePWM(m_pset->gpio_step, m_pwm_freq, PI_HW_PWM_RANGE/2); |
|
|
|
gpioWrite(m_pset->gpio_dir, move.dir); |
|
|
|
std::cout << move.period_us << " - " << move.steps << std::endl; |
|
|
|
auto t_sleep = move.period_us*move.steps; |
|
|
|
gpioWrite(m_pset->gpio_dir, move->dir); |
|
|
|
std::cout << move->period_us << " - " << move->steps << std::endl; |
|
|
|
auto t_sleep = move->period_us*move->steps; |
|
|
|
std::this_thread::sleep_for(std::chrono::microseconds(t_sleep)); |
|
|
|
gpioHardwarePWM(m_pset->gpio_step, m_pwm_freq, 0); |
|
|
|
} |
|
|
|
m_queue.pop(); |
|
|
|
} |
|
|
|
else { |
|
|
@ -38,9 +45,9 @@ namespace simon { |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
void c_allegro_4988::post(move_t move) |
|
|
|
void c_allegro_4988::post(std::shared_ptr<c_cmd> cmd) |
|
|
|
{ |
|
|
|
m_queue.push(move); |
|
|
|
m_queue.push(cmd); |
|
|
|
} |
|
|
|
void c_allegro_4988::initialize(void) |
|
|
|
{ |
|
|
@ -83,5 +90,5 @@ namespace simon { |
|
|
|
m_pwm_freq = pwm_freq; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
} // namespace a4988
|
|
|
|
} // namespace simon
|
|
|
|