66 lines
1.7 KiB
C++
66 lines
1.7 KiB
C++
#include <Arduino.h>
|
||
#include "servo.h"
|
||
|
||
// ===== SERVO CONFIG =====
|
||
#define SERVO_PIN 27
|
||
#define SERVO_CH 4
|
||
|
||
const uint32_t servoFreq = 50;
|
||
const uint8_t servoRes = 16;
|
||
|
||
// PWM диапазон (у тебя уже рабочий)
|
||
const uint32_t SERVO_MIN = 1638;
|
||
const uint32_t SERVO_MAX = 8192;
|
||
|
||
// ===== OFFSET CALIBRATION =====
|
||
// ПОЛОЖИТЕЛЬНОЕ значение → вправо
|
||
// ОТРИЦАТЕЛЬНОЕ → влево
|
||
const int SERVO_OFFSET_DEG = -6; // ← подбирается опытно
|
||
|
||
// логические пределы
|
||
const int SERVO_LOGICAL_MIN = 0;
|
||
const int SERVO_LOGICAL_MAX = 180;
|
||
|
||
static int currentAngle = 90;
|
||
|
||
// угол → duty
|
||
static uint32_t angleToDuty(int logicalAngle) {
|
||
// применяем offset
|
||
int calibratedAngle = logicalAngle + SERVO_OFFSET_DEG;
|
||
|
||
// защита от выхода за пределы
|
||
calibratedAngle = constrain(calibratedAngle,
|
||
SERVO_LOGICAL_MIN,
|
||
SERVO_LOGICAL_MAX);
|
||
|
||
return map(calibratedAngle,
|
||
SERVO_LOGICAL_MIN, SERVO_LOGICAL_MAX,
|
||
SERVO_MIN, SERVO_MAX);
|
||
}
|
||
|
||
bool servoInit() {
|
||
ledcSetup(SERVO_CH, servoFreq, servoRes);
|
||
ledcAttachPin(SERVO_PIN, SERVO_CH);
|
||
|
||
Serial.println("Servo initialized");
|
||
Serial.printf(" PIN → GPIO%d\n", SERVO_PIN);
|
||
Serial.printf(" CH → %d\n", SERVO_CH);
|
||
|
||
servoCenter();
|
||
return true;
|
||
}
|
||
|
||
void servoCenter() {
|
||
currentAngle = 90;
|
||
ledcWrite(SERVO_CH, angleToDuty(currentAngle));
|
||
}
|
||
|
||
void servoSetAngle(int angle) {
|
||
currentAngle = constrain(angle, SERVO_LOGICAL_MIN, SERVO_LOGICAL_MAX);
|
||
ledcWrite(SERVO_CH, angleToDuty(currentAngle));
|
||
}
|
||
|
||
int servoGetAngle() {
|
||
return currentAngle;
|
||
}
|