Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🏎️ Updates servo controller to reflect rl angles

Browse Source
This commit is contained in:
Rune Harlyk
2024-08-03 14:02:30 +02:00
committed by Rune Harlyk
parent 5ecb2eb9b5
commit 4e75952f57
6 changed files with 106 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
+3 -11
View File
@@ -57,14 +57,12 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server, unsigned int numberEnd
_factoryResetService(server, &ESPFS, &_securitySettingsService),
_systemStatus(server, &_securitySettingsService),
_fileExplorer(server, &_securitySettingsService),
_servoController(server, &ESPFS, &_securitySettingsService, &_peripherals, &_socket),
#if FT_ENABLED(FT_MOTION)
_motionService(_server, &_socket, &_securitySettingsService, &_taskManager),
#endif
#if FT_ENABLED(FT_WS2812)
_ledService(&_taskManager),
#endif
#if FT_ENABLED(FT_SERVO)
_servoController(server, &ESPFS, &_securitySettingsService, &_socket),
#endif
_peripherals(server, &ESPFS, &_securitySettingsService, &_socket) {
}
@@ -186,17 +184,14 @@ void ESP32SvelteKit::startServices() {
#endif
_taskManager.begin();
_fileExplorer.begin();
_peripherals.begin();
_servoController.begin();
#if FT_ENABLED(FT_MOTION)
_motionService.begin();
#endif
#if FT_ENABLED(FT_CAMERA)
_cameraService.begin();
_cameraSettingsService.begin();
#endif
_peripherals.begin();
#if FT_ENABLED(FT_SERVO)
_servoController.begin();
_servoController.configure();
#endif
#if FT_ENABLED(FT_WS2812)
_ledService.begin();
@@ -216,9 +211,6 @@ void IRAM_ATTR ESP32SvelteKit::loop() {
#if FT_ENABLED(FT_MOTION)
_motionService.loop();
#endif
#if FT_ENABLED(FT_SERVO)
_servoController.loop();
#endif
#if FT_ENABLED(FT_WS2812)
_ledService.loop();
#endif
esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h
-2
View File
@@ -176,9 +176,7 @@ class ESP32SvelteKit {
CameraSettingsService _cameraSettingsService;
#endif
Peripherals _peripherals;
#if FT_ENABLED(FT_SERVO)
ServoController _servoController;
#endif
#if FT_ENABLED(FT_WS2812)
LEDService _ledService;
#endif
esp32/lib/ESP32-sveltekit/ServoController.h
+64 -191
View File
@@ -1,211 +1,73 @@
#ifndef ServoController_h
#define ServoController_h
#include <Adafruit_PWMServoDriver.h>
#include <EventEndpoint.h>
#include <FSPersistence.h>
#include <HttpEndpoint.h>
#include <SecurityManager.h>
#include <StatefulService.h>
#include <MathUtils.h>
#include <Timing.h>
#define SERVO_CONFIG_FILE "/config/servoConfig.json"
#define SERVO_CONFIGURATION_SETTINGS_PATH "/api/servo/configuration"
#define SERVO_EVENT_CONFIGURATION_SETTINGS "servoConfiguration"
#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
#define EVENT_SERVO_STATE "servoState"
#ifndef FACTORY_SERVO_NUM
#define FACTORY_SERVO_NUM 12
#endif
#ifndef FACTORY_SERVO_PWM_FREQUENCY
#define FACTORY_SERVO_PWM_FREQUENCY 50
#endif
#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
#endif
#ifndef FACTORY_SERVO_CENTER_ANGLE
#define FACTORY_SERVO_CENTER_ANGLE 90
#endif
#define SERVO_STATE_SPEED_MS 20
#define SERVO_MIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVO_MAX 650 // This is the 'maximum' pulse length count (out of 4096)
enum SERVO_STATE { SERVO_STATE_ACTIVE, SERVO_STATE_SWEEPING_FORWARD, SERVO_STATE_SWEEPING_BACKWARD };
struct servo_t {
String name;
int8_t channel;
bool inverted;
int16_t angle;
int16_t center_angle;
SERVO_STATE state;
};
class ServoConfiguration {
class ServoController {
public:
int32_t servo_oscillator_frequency {FACTORY_SERVO_OSCILLATOR_FREQUENCY};
int32_t servo_pwm_frequency {FACTORY_SERVO_PWM_FREQUENCY};
std::vector<servo_t> servos_config;
bool is_active {true};
const int8_t servo_invert[12] = {-1, 1, 1, -1, -1, -1, 1, 1, 1, 1, -1, -1};
ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, Peripherals *peripherals,
EventSocket *socket)
: _server(server), _securityManager(securityManager), _peripherals(peripherals), _socket(socket) {}
static void read(ServoConfiguration &settings, JsonObject &root) {
root["is_active"] = settings.is_active;
root["servo_pwm_frequency"] = settings.servo_pwm_frequency;
root["servo_oscillator_frequency"] = settings.servo_oscillator_frequency;
void begin() {
_socket->onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
[&](JsonObject &root, int originId) { servoEvent(root, originId); });
_socket->onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
}
JsonArray servos = root["servos"].to<JsonArray>();
void stateUpdate(JsonObject &root, int originId) {
bool active = root["active"].as<bool>();
ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
active ? activate() : deactivate();
}
for (auto &servo : settings.servos_config) {
JsonObject servo_config = servos.add<JsonObject>();
void servoEvent(JsonObject &root, int originId) {
uint8_t servo_id = root["servo_id"];
uint16_t pwm = root["pwm"];
center[servo_id] = pwm;
ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
}
servo_config["name"] = servo.name;
servo_config["channel"] = servo.channel;
servo_config["inverted"] = servo.inverted;
servo_config["angle"] = servo.angle;
servo_config["center_angle"] = servo.center_angle;
servo_config["state"] = servo.state;
void syncAngles(const String &originId) {
char output[100];
snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
angles[10], angles[11]);
_socket->emit("angles", output, String(originId).c_str());
}
void deactivate() { _peripherals->pcaDeactivate(); }
void activate() { _peripherals->pcaActivate(); }
void updateActiveState() { is_active ? activate() : deactivate(); }
void setAngles(float new_angles[12]) {
for (int i = 0; i < 12; i++) {
target_angles[i] = new_angles[i];
}
}
static StateUpdateResult update(JsonObject &root, ServoConfiguration &settings) {
settings.is_active = root["is_active"] | settings.is_active;
settings.servo_pwm_frequency = root["servo_pwm_frequency"] | settings.servo_pwm_frequency;
settings.servo_oscillator_frequency = root["servo_oscillator_frequency"] | settings.servo_oscillator_frequency;
JsonArray servos = root["servos"];
if (!root["servos"].is<JsonArray>() && settings.servos_config.empty()) {
ESP_LOGI("ControllerSettings", "No servos found, adding default servos");
for (int8_t i = 0; i < FACTORY_SERVO_NUM; i++) {
ESP_LOGI("ControllerSettings", "Adding servo %d", i);
settings.servos_config.push_back(servo_t {
.name = "Servo " + String(i),
.channel = i,
.inverted = 1,
.angle = 0,
.center_angle = FACTORY_SERVO_CENTER_ANGLE
// ,
// .state = SERVO_STATE_ACTIVE
});
}
return StateUpdateResult::CHANGED;
}
for (auto new_servo_json : servos) {
JsonObject servo_config = new_servo_json.as<JsonObject>();
int8_t channel = servo_config["channel"] | -1;
servo_t *servo = get_servo_by_channel(settings.servos_config, channel);
if (servo != nullptr) {
servo->name = servo_config["name"].as<String>() || servo->name;
if (servo_config["inverted"]) servo->inverted = servo_config["inverted"];
if (servo_config["angle"].is<int16_t>()) {
servo->angle = servo_config["angle"].as<int16_t>();
servo->state = SERVO_STATE_ACTIVE;
}
if (servo_config["center_angle"].is<int16_t>())
servo->center_angle = servo_config["center_angle"].as<int16_t>();
if (servo_config["sweep"]) servo->state = SERVO_STATE_SWEEPING_FORWARD;
continue;
}
servo_t new_servo;
new_servo.name = servo_config["name"].as<String>();
new_servo.channel = channel;
new_servo.inverted = servo_config["inverted"];
new_servo.angle = servo_config["angle"];
new_servo.center_angle = servo_config["center_angle"];
// new_servo.state = servo_config["state"];
settings.servos_config.push_back(new_servo);
}
return StateUpdateResult::CHANGED;
};
static servo_t *get_servo_by_channel(std::vector<servo_t> &servos_config, int8_t channel_id) {
for (auto &servo : servos_config) {
if (servo.channel == channel_id) {
return &servo;
}
}
return nullptr;
}
};
class ServoController : public Adafruit_PWMServoDriver, public StatefulService<ServoConfiguration> {
public:
ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, EventSocket *socket)
: Adafruit_PWMServoDriver(),
_server(server),
_securityManager(securityManager),
_eventEndpoint(ServoConfiguration::read, ServoConfiguration::update, this, socket,
SERVO_EVENT_CONFIGURATION_SETTINGS),
_fsPersistence(ServoConfiguration::read, ServoConfiguration::update, this, fs, SERVO_CONFIG_FILE) {
addUpdateHandler([&](const String &originId) { updateServos(); }, false);
}
void configure() {
_eventEndpoint.begin();
_fsPersistence.readFromFS();
setOscillatorFrequency(_state.servo_oscillator_frequency);
setPWMFreq(_state.servo_pwm_frequency);
deactivate();
ESP_LOGI("ServoController", "Configured with oscillator frequency %d and PWM frequency %d",
_state.servo_oscillator_frequency, _state.servo_pwm_frequency);
}
void deactivate() {
if (!is_active) return;
_state.is_active = false;
is_active = false;
sleep();
}
void activate() {
if (is_active) return;
_state.is_active = true;
is_active = true;
wakeup();
}
void updateActiveState() { _state.is_active ? activate() : deactivate(); }
void updateServos() {
updateActiveState();
if (!is_active) return;
for (auto &servo : _state.servos_config) {
setAngle(&servo);
}
}
void setAngle(servo_t *servo) {
int8_t channel = servo->channel;
bool invert = servo->inverted;
int16_t angle = invert ? 180 - servo->angle : servo->angle;
ESP_LOGV("ServoController", "Setting servo %d to angle %d", channel, angle);
setPWM(channel, 0, angleToPwm(angle));
}
uint16_t angleToPwm(int angle) { return map(angle, 0, 180, SERVO_MIN, SERVO_MAX); }
void updateServoState() {
for (auto &servo : _state.servos_config) {
if (servo.state == SERVO_STATE::SERVO_STATE_ACTIVE) {
for (int i = 0; i < 12; i++) {
angles[i] = lerp(angles[i], target_angles[i], 0.2);
float angle = dir[i] * angles[i] + center_angle_deg[i];
uint16_t pwm = angle * servo_conversion[i] + center[i];
if (pwm < 125 || pwm > 600) {
ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
continue;
} else if (servo.state == SERVO_STATE::SERVO_STATE_SWEEPING_FORWARD) {
servo.angle += 2;
if (servo.angle >= 180) {
servo.state = SERVO_STATE::SERVO_STATE_SWEEPING_BACKWARD;
}
} else if (servo.state == SERVO_STATE::SERVO_STATE_SWEEPING_BACKWARD) {
servo.angle -= 2;
if (servo.angle <= 0) {
servo.state = SERVO_STATE::SERVO_STATE_ACTIVE;
}
}
setAngle(&servo);
_peripherals->pcaWrite(i, pwm);
}
}
@@ -216,9 +78,20 @@ class ServoController : public Adafruit_PWMServoDriver, public StatefulService<S
private:
PsychicHttpServer *_server;
SecurityManager *_securityManager;
EventEndpoint<ServoConfiguration> _eventEndpoint;
FSPersistence<ServoConfiguration> _fsPersistence;
Peripherals *_peripherals;
EventSocket *_socket;
bool is_active {true};
constexpr static int ServoInterval = 2;
float center[12] = {306, 306, 306, 306, 306, 306, 306, 306, 306, 306, 306, 306};
float dir[12] = {-1, 1, 1, -1, -1, -1, 1, 1, 1, 1, -1, -1};
float center_angle_deg[12] = {0, -45, 90, 0, 45, -90, 0, -45, 90, 0, 45, -90};
float angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
const float servo_conversion[12] {2.2, 2.1055555, 1.96923, 2.2, 2.1055555, 1.96923,
2.2, 2.1055555, 1.96923, 2.2, 2.1055555, 1.96923};
};
#endif