73 lines
1.8 KiB
C++
73 lines
1.8 KiB
C++
#include "motor.hpp"
|
|
#include "hal/gpio.hpp"
|
|
#include "hal/pwm.hpp"
|
|
#include "types.hpp"
|
|
#include <cstdio>
|
|
#include <algorithm>
|
|
|
|
static PWM* pwm_left = nullptr;
|
|
static PWM* pwm_right = nullptr;
|
|
static GPIO* dir_left = nullptr;
|
|
static GPIO* dir_right = nullptr;
|
|
static GPIO* left_in2 = nullptr;
|
|
static GPIO* mortor_en = nullptr;
|
|
static bool initialized = false;
|
|
|
|
void motor_init()
|
|
{
|
|
if (initialized) return;
|
|
|
|
mortor_en = new GPIO(MOTOR_ENABLE_GPIO);
|
|
mortor_en->setDirection("out");
|
|
mortor_en->setValue(1);
|
|
|
|
left_in2 = new GPIO(MOTOR_LEFT_IN2_GPIO);
|
|
left_in2->setDirection("out");
|
|
left_in2->setValue(0);
|
|
|
|
pwm_left = new PWM(MOTOR_PWMCHIP, MOTOR_LEFT_PWMID, MOTOR_PWM_PERIOD_NS);
|
|
pwm_right = new PWM(MOTOR_PWMCHIP, MOTOR_RIGHT_PWMID, MOTOR_PWM_PERIOD_NS);
|
|
pwm_left->enable();
|
|
pwm_right->enable();
|
|
|
|
dir_left = new GPIO(MOTOR_LEFT_DIR_GPIO);
|
|
dir_right = new GPIO(MOTOR_RIGHT_DIR_GPIO);
|
|
dir_left->setDirection("out");
|
|
dir_right->setDirection("out");
|
|
|
|
initialized = true;
|
|
}
|
|
|
|
void motor_set_pwm(int idx, float duty)
|
|
{
|
|
if (!initialized) return;
|
|
duty = std::clamp(duty, -100.0f, 100.0f);
|
|
|
|
PWM* pwm = (idx == MOTOR_LEFT) ? pwm_left : pwm_right;
|
|
GPIO* dir = (idx == MOTOR_LEFT) ? dir_left : dir_right;
|
|
|
|
if (duty >= 0)
|
|
{
|
|
dir->setValue(1);
|
|
unsigned int d_ns = static_cast<unsigned int>(MOTOR_PWM_PERIOD_NS * duty / 100.0f);
|
|
pwm->setDutyCycle(d_ns);
|
|
}
|
|
else
|
|
{
|
|
dir->setValue(0);
|
|
unsigned int d_ns = static_cast<unsigned int>(MOTOR_PWM_PERIOD_NS * (-duty) / 100.0f);
|
|
pwm->setDutyCycle(d_ns);
|
|
}
|
|
}
|
|
|
|
void motor_stop_all()
|
|
{
|
|
motor_set_pwm(MOTOR_LEFT, 0);
|
|
motor_set_pwm(MOTOR_RIGHT, 0);
|
|
}
|
|
|
|
void motor_enable(bool on)
|
|
{
|
|
if (mortor_en) mortor_en->setValue(on ? 1 : 0);
|
|
}
|