Simon's and Max' camera steering software.
https://stiefel.tech
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
123 lines
3.5 KiB
123 lines
3.5 KiB
#include <include/a4988.h>
|
|
#include <pigpio.h>
|
|
#include <queue>
|
|
#include <cmath>
|
|
#include <thread>
|
|
#include <string>
|
|
#include <memory>
|
|
|
|
#include <iostream>
|
|
|
|
namespace simon {
|
|
namespace a4988 {
|
|
c_allegro_4988::c_allegro_4988(std::unique_ptr<a4988_settings_t> ptr_set)
|
|
: m_ptr_set(std::move(ptr_set)), m_initialized(false), m_alive(true)
|
|
{
|
|
|
|
}
|
|
void c_allegro_4988::run(void)
|
|
{
|
|
if (!m_initialized) {
|
|
initialize();
|
|
}
|
|
while(m_alive)
|
|
{
|
|
std::cout << "In thread" << std::endl;
|
|
if (!m_queue.empty()) {
|
|
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_ptr_set->gpio_step, m_pwm_freq, PI_HW_PWM_RANGE/2);
|
|
gpioWrite(m_ptr_set->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_ptr_set->gpio_step, m_pwm_freq, 0);
|
|
}
|
|
else if (*cmd == "enable") {
|
|
auto enable = std::static_pointer_cast<c_enable>(cmd);
|
|
gpioWrite(m_ptr_set->gpio_en, enable->enabled);
|
|
}
|
|
else if (*cmd == "kill") {
|
|
m_alive = false;
|
|
}
|
|
else {
|
|
// TODO: Throw an exception here
|
|
}
|
|
m_queue.pop();
|
|
}
|
|
else {
|
|
// Idle
|
|
}
|
|
}
|
|
}
|
|
void c_allegro_4988::post(std::shared_ptr<c_cmd> cmd)
|
|
{
|
|
m_queue.push(cmd);
|
|
}
|
|
void c_allegro_4988::post_move(const unsigned steps, const bool dir,
|
|
const unsigned period_us)
|
|
{
|
|
auto move(std::make_shared<c_move>(steps, dir, period_us));
|
|
auto cmd = std::static_pointer_cast<c_cmd>(move);
|
|
m_queue.push(cmd);
|
|
}
|
|
|
|
void c_allegro_4988::initialize(void)
|
|
{
|
|
// TODO: proper error management throwing exceptions
|
|
if(m_ptr_set == nullptr) {
|
|
return;
|
|
}
|
|
// Configure clock source for PWM
|
|
gpioCfgClock(CFG_SAMPLE_RATE_US, CFG_PERIPHERAL_SRC_PCM, CFG_DEPRECATED_ARG);
|
|
// Initialize pigpio library
|
|
gpioInitialise();
|
|
// Configure as outputs
|
|
gpioSetMode(m_ptr_set->gpio_en, PI_OUTPUT);
|
|
gpioSetMode(m_ptr_set->gpio_dir, PI_OUTPUT);
|
|
// Initialize outputs
|
|
enable();
|
|
gpioWrite(m_ptr_set->gpio_dir, 0);
|
|
// Set PWM to 0
|
|
m_pwm_freq = 1000;
|
|
gpioHardwarePWM(m_ptr_set->gpio_step, m_pwm_freq, 0);
|
|
m_initialized = true;
|
|
}
|
|
int c_allegro_4988::enable(void)
|
|
{
|
|
// TODO: proper error handling
|
|
auto enable(std::make_shared<c_enable>(true));
|
|
auto cmd = std::static_pointer_cast<c_cmd>(enable);
|
|
m_queue.push(cmd);
|
|
return 0;
|
|
}
|
|
int c_allegro_4988::disable(void)
|
|
{
|
|
// TODO: proper error handling
|
|
auto enable(std::make_shared<c_enable>(false));
|
|
auto cmd = std::static_pointer_cast<c_cmd>(enable);
|
|
m_queue.push(cmd);
|
|
return 0;
|
|
}
|
|
void c_allegro_4988::kill(void)
|
|
{
|
|
// TODO: proper error handling
|
|
auto kill(std::make_shared<c_kill>());
|
|
auto cmd = std::static_pointer_cast<c_cmd>(kill);
|
|
m_queue.push(cmd);
|
|
}
|
|
int c_allegro_4988::set_pwm_freq(const unsigned pwm_freq)
|
|
{
|
|
if (pwm_freq > PI_HW_PWM_MAX_FREQ) {
|
|
return 1;
|
|
}
|
|
m_pwm_freq = pwm_freq;
|
|
return 0;
|
|
}
|
|
} // namespace a4988
|
|
} // namespace simon
|
|
|