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

📦 Moves all i2c servo control to controller

Browse Source
This commit is contained in:
Rune Harlyk
2024-11-14 15:38:19 +01:00
parent c9548e2da1
commit b346e104d4
3 changed files with 48 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/Peripherals.h
-82
View File
@@ -15,7 +15,6 @@
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
@@ -29,18 +28,6 @@
#define I2C_INTERVAL 250
#define MAX_ESP_IMU_SIZE 500
#define EVENT_IMU "imu"
#define EVENT_SERVO_STATE "servoState"
/*
* Servo Settings
*/
#ifndef FACTORY_SERVO_PWM_FREQUENCY
#define FACTORY_SERVO_PWM_FREQUENCY 50
#endif
#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
#endif
/*
* OLED Settings
@@ -87,17 +74,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
updatePins();
#if FT_ENABLED(USE_SERVO)
_pca.begin();
_pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
_pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
_pca.sleep();
socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
_pca_active = root["active"] | false;
_pca_active ? pcaActivate() : pcaDeactivate();
});
#endif
#if FT_ENABLED(USE_IMU)
if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
#endif
@@ -172,57 +148,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
ESP_LOGI("Peripherals", "Scan complete - Found %d devices", nDevices);
}
/* SERVO FUNCTIONS*/
void pcaLerpTo(int index, int value, float t) {
#if FT_ENABLED(USE_SERVO)
target_pwm[index] = lerp(pwm[index], value, t);
#endif
}
void pcaWrite(int index, int value) {
#if FT_ENABLED(USE_SERVO)
if (value < 0 || value > 4096) {
ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
return;
}
pwm[index] = value;
target_pwm[index] = value;
beginTransaction();
_pca.setPWM(index, 0, value);
endTransaction();
#endif
}
void pcaWriteAngle(int index, int angle) {
#if FT_ENABLED(USE_SERVO)
_pca.setPWM(index, 0, 125 + angle * 2);
#endif
}
void pcaWriteAngles(float *angles, int numServos, int offset = 0) {
#if FT_ENABLED(USE_SERVO)
for (int i = 0; i < numServos; i++) {
pcaWriteAngle(i + offset, angles[i]);
}
#endif
}
void pcaActivate() {
#if FT_ENABLED(USE_SERVO)
if (_pca_active) return;
_pca_active = true;
_pca.wakeup();
#endif
}
void pcaDeactivate() {
#if FT_ENABLED(USE_SERVO)
if (!_pca_active) return;
_pca_active = false;
_pca.sleep();
#endif
}
/* IMU FUNCTIONS */
bool readIMU() {
bool updated = false;
@@ -336,13 +261,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
JsonDocument doc;
char message[MAX_ESP_IMU_SIZE];
#if FT_ENABLED(USE_SERVO)
Adafruit_PWMServoDriver _pca;
bool _pca_active {false};
uint16_t pwm[16] = {0};
uint16_t target_pwm[16] = {0};
#endif
#if FT_ENABLED(USE_IMU)
IMU _imu;
#endif
esp32/lib/ESP32-sveltekit/ServoController.h
+47 -10
View File
@@ -9,14 +9,24 @@
#include <MathUtils.h>
#include <settings/servo_settings.h>
/*
* Servo Settings
*/
#ifndef FACTORY_SERVO_PWM_FREQUENCY
#define FACTORY_SERVO_PWM_FREQUENCY 50
#endif
#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
#endif
#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
#define EVENT_SERVO_STATE "servoState"
class ServoController : public StatefulService<ServoSettings> {
public:
ServoController(Peripherals *peripherals)
: _peripherals(peripherals),
endpoint(ServoSettings::read, ServoSettings::update, this),
ServoController()
: endpoint(ServoSettings::read, ServoSettings::update, this),
_persistence(ServoSettings::read, ServoSettings::update, this, SERVO_SETTINGS_FILE) {}
void begin() {
@@ -24,6 +34,35 @@ class ServoController : public StatefulService<ServoSettings> {
[&](JsonObject &root, int originId) { servoEvent(root, originId); });
socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
_persistence.readFromFS();
_pca.begin();
_pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
_pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
_pca.sleep();
socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
is_active = root["active"] | false;
is_active ? activate() : deactivate();
});
}
void pcaWrite(int index, int value) {
if (value < 0 || value > 4096) {
ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
return;
}
_pca.setPWM(index, 0, value);
}
void activate() {
if (is_active) return;
is_active = true;
_pca.wakeup();
}
void deactivate() {
if (!is_active) return;
is_active = false;
_pca.sleep();
}
void stateUpdate(JsonObject &root, int originId) {
@@ -46,10 +85,6 @@ class ServoController : public StatefulService<ServoSettings> {
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]) {
@@ -68,17 +103,19 @@ class ServoController : public StatefulService<ServoSettings> {
ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
continue;
}
_peripherals->pcaWrite(i, pwm);
pcaWrite(i, pwm);
}
}
StatefulHttpEndpoint<ServoSettings> endpoint;
private:
Peripherals *_peripherals;
FSPersistence<ServoSettings> _persistence;
bool is_active {true};
Adafruit_PWMServoDriver _pca;
uint16_t pwm[16] = {0};
bool is_active {false};
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};
};
esp32/src/spot.cpp
+1 -1
View File
@@ -3,7 +3,7 @@
static const char *TAG = "Spot";
Spot::Spot(PsychicHttpServer *server)
: _servoController(&_peripherals),
:
#if FT_ENABLED(USE_MOTION)
_motionService(&_servoController),
#endif