🧼 Moves led and servo controller
This commit is contained in:
Rune Harlyk
@@ -0,0 +1,56 @@
|
||||
#ifndef LEDService_h
|
||||
#define LEDService_h
|
||||
|
||||
#include <FastLED.h>
|
||||
|
||||
#ifndef WS2812_PIN
|
||||
#define WS2812_PIN 12
|
||||
#endif
|
||||
|
||||
#ifndef WS2812_NUM_LEDS
|
||||
#define WS2812_NUM_LEDS 1 + 12
|
||||
#endif
|
||||
|
||||
#define COLOR_ORDER GRB
|
||||
#define CHIPSET WS2811
|
||||
|
||||
class LEDService {
|
||||
private:
|
||||
CRGB leds[WS2812_NUM_LEDS];
|
||||
CRGBPalette16 currentPalette;
|
||||
TBlendType currentBlending;
|
||||
|
||||
int _brightness = 255;
|
||||
int direction = 1;
|
||||
|
||||
public:
|
||||
LEDService() {
|
||||
FastLED.addLeds<CHIPSET, WS2812_PIN, COLOR_ORDER>(leds, WS2812_NUM_LEDS).setCorrection(TypicalLEDStrip);
|
||||
currentPalette = OceanColors_p;
|
||||
currentBlending = LINEARBLEND;
|
||||
}
|
||||
~LEDService() {}
|
||||
|
||||
void loop() {
|
||||
EXECUTE_EVERY_N_MS(1000 / 60, {
|
||||
if (_brightness >= 200) direction = -5;
|
||||
if (_brightness <= 50) direction = 5;
|
||||
_brightness += direction;
|
||||
if (WiFi.isConnected()) {
|
||||
fillFromPallette(OceanColors_p, 0);
|
||||
} else {
|
||||
fillFromPallette(ForestColors_p, 128);
|
||||
}
|
||||
FastLED.show();
|
||||
});
|
||||
}
|
||||
|
||||
void fillFromPallette(CRGBPalette16 colorPalette, uint8_t colorIndex) {
|
||||
CRGB color = ColorFromPalette(colorPalette, colorIndex, _brightness, currentBlending);
|
||||
for (int i = 0; i < WS2812_NUM_LEDS; ++i) {
|
||||
leds[i] = color;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,287 @@
|
||||
#ifndef Peripherals_h
|
||||
#define Peripherals_h
|
||||
|
||||
#include <EventEndpoint.h>
|
||||
#include <stateful_persistence.h>
|
||||
#include <stateful_service.h>
|
||||
#include <MathUtils.h>
|
||||
#include <utils/timing.h>
|
||||
#include <filesystem.h>
|
||||
#include <features.h>
|
||||
#include <settings/peripherals_settings.h>
|
||||
#include <stateful_service_endpoint.h>
|
||||
|
||||
#include <list>
|
||||
#include <SPI.h>
|
||||
#include <Wire.h>
|
||||
|
||||
#include <Adafruit_BMP085_U.h>
|
||||
#include <Adafruit_HMC5883_U.h>
|
||||
#include <Adafruit_Sensor.h>
|
||||
#include <NewPing.h>
|
||||
#include <peripherals/imu.h>
|
||||
|
||||
#define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
|
||||
|
||||
#define EVENT_I2C_SCAN "i2cScan"
|
||||
|
||||
#define I2C_INTERVAL 250
|
||||
#define MAX_ESP_IMU_SIZE 500
|
||||
#define EVENT_IMU "imu"
|
||||
|
||||
/*
|
||||
* OLED Settings
|
||||
*/
|
||||
#define SCREEN_WIDTH 128
|
||||
#define SCREEN_HEIGHT 64
|
||||
#define SCREEN_RESET -1
|
||||
|
||||
/*
|
||||
* Ultrasonic Sensor Settings
|
||||
*/
|
||||
#define MAX_DISTANCE 200
|
||||
|
||||
class Peripherals : public StatefulService<PeripheralsConfiguration> {
|
||||
public:
|
||||
Peripherals()
|
||||
: endpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this),
|
||||
_eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this,
|
||||
EVENT_CONFIGURATION_SETTINGS),
|
||||
#if FT_ENABLED(USE_MAG)
|
||||
_mag(12345),
|
||||
#endif
|
||||
#if FT_ENABLED(USE_BMP)
|
||||
_bmp(10085),
|
||||
#endif
|
||||
_persistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, DEVICE_CONFIG_FILE) {
|
||||
_accessMutex = xSemaphoreCreateMutex();
|
||||
addUpdateHandler([&](const String &originId) { updatePins(); }, false);
|
||||
};
|
||||
|
||||
void begin() {
|
||||
_eventEndpoint.begin();
|
||||
_persistence.readFromFS();
|
||||
|
||||
socket.onEvent(EVENT_I2C_SCAN, [&](JsonObject &root, int originId) {
|
||||
scanI2C();
|
||||
emitI2C();
|
||||
});
|
||||
|
||||
socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
|
||||
scanI2C();
|
||||
emitI2C(originId, sync);
|
||||
});
|
||||
|
||||
updatePins();
|
||||
|
||||
#if FT_ENABLED(USE_IMU)
|
||||
if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
|
||||
#endif
|
||||
#if FT_ENABLED(USE_MAG)
|
||||
mag_success = _mag.begin();
|
||||
if (!mag_success) {
|
||||
ESP_LOGE("IMUService", "MAG initialize failed");
|
||||
}
|
||||
#endif
|
||||
#if FT_ENABLED(USE_BMP)
|
||||
bmp_success = _bmp.begin();
|
||||
if (!bmp_success) {
|
||||
ESP_LOGE("IMUService", "BMP initialize failed");
|
||||
}
|
||||
#endif
|
||||
|
||||
#if FT_ENABLED(USE_USS)
|
||||
_left_sonar = new NewPing(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
|
||||
_right_sonar = new NewPing(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
|
||||
#endif
|
||||
};
|
||||
|
||||
void loop() {
|
||||
EXECUTE_EVERY_N_MS(_updateInterval, {
|
||||
beginTransaction();
|
||||
emitIMU();
|
||||
readSonar();
|
||||
emitSonar();
|
||||
endTransaction();
|
||||
});
|
||||
}
|
||||
|
||||
void updatePins() {
|
||||
if (i2c_active) {
|
||||
Wire.end();
|
||||
}
|
||||
|
||||
if (_state.sda != -1 && _state.scl != -1) {
|
||||
Wire.begin(_state.sda, _state.scl, _state.frequency);
|
||||
i2c_active = true;
|
||||
}
|
||||
}
|
||||
|
||||
void emitI2C(const String &originId = "", bool sync = false) {
|
||||
char output[150];
|
||||
JsonDocument doc;
|
||||
JsonObject root = doc.to<JsonObject>();
|
||||
root["sda"] = _state.sda;
|
||||
root["scl"] = _state.scl;
|
||||
JsonArray addresses = root["addresses"].to<JsonArray>();
|
||||
for (auto &address : addressList) {
|
||||
addresses.add(address);
|
||||
}
|
||||
serializeJson(root, output);
|
||||
ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync);
|
||||
socket.emit(EVENT_I2C_SCAN, output, originId.c_str(), sync);
|
||||
}
|
||||
|
||||
void scanI2C(uint8_t lower = 1, uint8_t higher = 127) {
|
||||
addressList.clear();
|
||||
for (uint8_t address = lower; address < higher; address++) {
|
||||
Wire.beginTransmission(address);
|
||||
if (Wire.endTransmission() == 0) {
|
||||
addressList.emplace_back(address);
|
||||
ESP_LOGI("Peripherals", "I2C device found at address 0x%02X", address);
|
||||
}
|
||||
}
|
||||
uint8_t nDevices = addressList.size();
|
||||
if (nDevices == 0)
|
||||
ESP_LOGI("Peripherals", "No I2C devices found");
|
||||
else
|
||||
ESP_LOGI("Peripherals", "Scan complete - Found %d devices", nDevices);
|
||||
}
|
||||
|
||||
/* IMU FUNCTIONS */
|
||||
bool readIMU() {
|
||||
bool updated = false;
|
||||
#if FT_ENABLED(USE_IMU)
|
||||
beginTransaction();
|
||||
updated = _imu.readIMU();
|
||||
endTransaction();
|
||||
#endif
|
||||
return updated;
|
||||
}
|
||||
|
||||
/* MAG FUNCTIONS */
|
||||
float getHeading() {
|
||||
float heading = 0;
|
||||
#if FT_ENABLED(USE_MAG)
|
||||
sensors_event_t event;
|
||||
_mag.getEvent(&event);
|
||||
heading = atan2(event.magnetic.y, event.magnetic.x);
|
||||
float declinationAngle = 0.22;
|
||||
heading += declinationAngle;
|
||||
if (heading < 0) heading += 2 * PI;
|
||||
if (heading > 2 * PI) heading -= 2 * PI;
|
||||
heading *= 180 / M_PI;
|
||||
#endif
|
||||
return heading;
|
||||
}
|
||||
|
||||
/* BMP FUNCTIONS */
|
||||
float getAltitude() {
|
||||
float altitude = -1;
|
||||
#if FT_ENABLED(USE_MAG)
|
||||
sensors_event_t event;
|
||||
_bmp.getEvent(&event);
|
||||
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
|
||||
altitude = bmp_success && event.pressure ? _bmp.pressureToAltitude(seaLevelPressure, event.pressure) : -1;
|
||||
#endif
|
||||
return altitude;
|
||||
}
|
||||
|
||||
float getPressure() {
|
||||
float pressure = -1;
|
||||
#if FT_ENABLED(USE_BMP)
|
||||
sensors_event_t event;
|
||||
_bmp.getEvent(&event);
|
||||
pressure = bmp_success && event.pressure ? event.pressure : -1;
|
||||
#endif
|
||||
return pressure;
|
||||
}
|
||||
|
||||
float getTemperature() {
|
||||
float temperature = 0;
|
||||
#if FT_ENABLED(USE_BMP)
|
||||
_bmp.getTemperature(&temperature);
|
||||
temperature = bmp_success ? temperature : -1;
|
||||
#endif
|
||||
return temperature;
|
||||
}
|
||||
|
||||
void readSonar() {
|
||||
#if FT_ENABLED(USE_USS)
|
||||
_left_distance = _left_sonar->ping_cm();
|
||||
delay(50);
|
||||
_right_distance = _right_sonar->ping_cm();
|
||||
#endif
|
||||
}
|
||||
|
||||
float leftDistance() { return _left_distance; }
|
||||
float rightDistance() { return _right_distance; }
|
||||
|
||||
StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
|
||||
|
||||
void emitIMU() {
|
||||
doc.clear();
|
||||
JsonObject root = doc.to<JsonObject>();
|
||||
#if FT_ENABLED(USE_IMU)
|
||||
_imu.readIMU(root);
|
||||
#endif
|
||||
#if FT_ENABLED(USE_MAG)
|
||||
if (mag_success) {
|
||||
doc["heading"] = round2(getHeading());
|
||||
}
|
||||
#endif
|
||||
#if FT_ENABLED(USE_BMP)
|
||||
if (bmp_success) {
|
||||
doc["pressure"] = round2(getPressure());
|
||||
doc["altitude"] = round2(getAltitude());
|
||||
doc["bmp_temp"] = round2(getTemperature());
|
||||
}
|
||||
#endif
|
||||
serializeJson(doc, message);
|
||||
socket.emit(EVENT_IMU, message);
|
||||
}
|
||||
|
||||
void emitSonar() {
|
||||
#if FT_ENABLED(USE_USS)
|
||||
|
||||
char output[16];
|
||||
snprintf(output, sizeof(output), "[%.1f,%.1f]", _left_distance, _right_distance);
|
||||
socket.emit("sonar", output);
|
||||
#endif
|
||||
}
|
||||
|
||||
private:
|
||||
EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
|
||||
FSPersistence<PeripheralsConfiguration> _persistence;
|
||||
|
||||
SemaphoreHandle_t _accessMutex;
|
||||
inline void beginTransaction() { xSemaphoreTakeRecursive(_accessMutex, portMAX_DELAY); }
|
||||
|
||||
inline void endTransaction() { xSemaphoreGiveRecursive(_accessMutex); }
|
||||
|
||||
JsonDocument doc;
|
||||
char message[MAX_ESP_IMU_SIZE];
|
||||
#if FT_ENABLED(USE_IMU)
|
||||
IMU _imu;
|
||||
#endif
|
||||
#if FT_ENABLED(USE_MAG)
|
||||
Adafruit_HMC5883_Unified _mag;
|
||||
bool mag_success {false};
|
||||
#endif
|
||||
#if FT_ENABLED(USE_BMP)
|
||||
Adafruit_BMP085_Unified _bmp;
|
||||
bool bmp_success {false};
|
||||
#endif
|
||||
#if FT_ENABLED(USE_USS)
|
||||
NewPing *_left_sonar;
|
||||
NewPing *_right_sonar;
|
||||
#endif
|
||||
float _left_distance {MAX_DISTANCE};
|
||||
float _right_distance {MAX_DISTANCE};
|
||||
|
||||
std::list<uint8_t> addressList;
|
||||
bool i2c_active = false;
|
||||
unsigned long _updateInterval {I2C_INTERVAL};
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,123 @@
|
||||
#ifndef ServoController_h
|
||||
#define ServoController_h
|
||||
|
||||
#include <Adafruit_PWMServoDriver.h>
|
||||
#include <event_socket.h>
|
||||
#include <stateful_persistence.h>
|
||||
#include <stateful_service.h>
|
||||
#include <stateful_service_endpoint.h>
|
||||
#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()
|
||||
: endpoint(ServoSettings::read, ServoSettings::update, this),
|
||||
_persistence(ServoSettings::read, ServoSettings::update, this, SERVO_SETTINGS_FILE) {}
|
||||
|
||||
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); });
|
||||
_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) {
|
||||
bool active = root["active"].as<bool>();
|
||||
ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
|
||||
active ? activate() : deactivate();
|
||||
}
|
||||
|
||||
void servoEvent(JsonObject &root, int originId) {
|
||||
uint8_t servo_id = root["servo_id"];
|
||||
uint16_t pwm = root["pwm"];
|
||||
ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
|
||||
}
|
||||
|
||||
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 updateActiveState() { is_active ? activate() : deactivate(); }
|
||||
|
||||
void setAngles(float new_angles[12]) {
|
||||
for (int i = 0; i < 12; i++) {
|
||||
target_angles[i] = new_angles[i];
|
||||
}
|
||||
}
|
||||
|
||||
void updateServoState() {
|
||||
for (int i = 0; i < 12; i++) {
|
||||
angles[i] = lerp(angles[i], target_angles[i], 0.2);
|
||||
auto &servo = _state.servos[i];
|
||||
float angle = servo.direction * angles[i] + servo.centerAngle;
|
||||
uint16_t pwm = angle * servo.conversion + servo.centerPwm;
|
||||
if (pwm < 125 || pwm > 600) {
|
||||
ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
|
||||
continue;
|
||||
}
|
||||
pcaWrite(i, pwm);
|
||||
}
|
||||
}
|
||||
|
||||
StatefulHttpEndpoint<ServoSettings> endpoint;
|
||||
|
||||
private:
|
||||
FSPersistence<ServoSettings> _persistence;
|
||||
|
||||
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};
|
||||
};
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user