58 lines
1.7 KiB
C++
58 lines
1.7 KiB
C++
#include "servo.hpp"
|
|
#include "hal/pwm.hpp"
|
|
#include <algorithm>
|
|
#include <cstdio>
|
|
#include <cmath>
|
|
|
|
static PWM* servo_pwm = nullptr;
|
|
static PID* servo_pid = nullptr;
|
|
static bool serv_initialized = false;
|
|
|
|
static float g_deadband = 0.03f;
|
|
static float g_steer_gain = 1.5f;
|
|
|
|
void servo_init()
|
|
{
|
|
if (serv_initialized) return;
|
|
|
|
servo_pwm = new PWM(SERVO_PWMCHIP, SERVO_PWMID, SERVO_PERIOD_NS);
|
|
servo_pwm->setDutyCycle(SERVO_CENTER_NS);
|
|
servo_pwm->enable();
|
|
|
|
servo_pid = new PID(400000, 200000, 0, 0, PID_POSITIONAL,
|
|
static_cast<float>(SERVO_MAX_DELTA_NS),
|
|
static_cast<float>(SERVO_MAX_DELTA_NS));
|
|
|
|
serv_initialized = true;
|
|
}
|
|
|
|
void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); }
|
|
void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); }
|
|
void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); }
|
|
|
|
void servo_set_angle(float deviation)
|
|
{
|
|
if (!serv_initialized) return;
|
|
deviation = std::clamp(deviation, -1.0f, 1.0f);
|
|
|
|
if (std::abs(deviation) < g_deadband)
|
|
{
|
|
servo_pwm->setDutyCycle(SERVO_CENTER_NS);
|
|
return;
|
|
}
|
|
|
|
float scaled = deviation * g_steer_gain;
|
|
float out = servo_pid->update(scaled);
|
|
float duty_ns = SERVO_CENTER_NS + out;
|
|
duty_ns = std::clamp(duty_ns,
|
|
static_cast<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
|
|
static_cast<float>(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS));
|
|
servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
|
|
}
|
|
|
|
void servo_center()
|
|
{
|
|
if (serv_initialized)
|
|
servo_pwm->setDutyCycle(SERVO_CENTER_NS);
|
|
}
|