botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

Sensor data implementation

Browse Source
This commit is contained in:
Johnathon Slightham
2026-01-15 01:24:57 -05:00
parent 5b2155985e
commit 3b898b51b4
10 changed files with 218 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
components/flatbuffers/SensorMessageBuilder.cpp
View File

@@ -4,28 +4,40 @@
#include "SensorMessageBuilder.h" #include "SensorMessageBuilder.h"
#include "SerializedMessage.h" #include "SerializedMessage.h"
#include "Variant.h"
#include "flatbuffers_generated/SensorMessage_generated.h" #include "flatbuffers_generated/SensorMessage_generated.h"
namespace Flatbuffers { namespace Flatbuffers {
SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<SensorValueInstance>& values) { SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<sensor_value> &values) {
builder_.Clear(); builder_.Clear();
std::vector<flatbuffers::Offset<void>> values_vec; std::vector<flatbuffers::Offset<void>> values_vec;
std::vector<uint8_t> sensor_values_vec; std::vector<uint8_t> sensor_values_vec;
for (const auto& v : values) { for (const auto &v : values) {
values_vec.push_back(Messaging::CreateAngle(builder_, v.angle).Union()); std::visit(
sensor_values_vec.push_back(Messaging::SensorValue_Angle); overloaded{
} [&](target_angle a) {
values_vec.push_back(Messaging::CreateTargetAngle(builder_, a.angle).Union());
auto values_fb_vec = builder_.CreateVector(values_vec); sensor_values_vec.push_back(Messaging::SensorValue_TargetAngle);
const auto values_type_fb_vec = builder_.CreateVector(sensor_values_vec); },
[&](current_angle a) {
const auto message = Messaging::CreateSensorMessage(builder_, values_type_fb_vec, values_fb_vec); values_vec.push_back(Messaging::CreateCurrentAngle(builder_, a.angle).Union());
sensor_values_vec.push_back(Messaging::SensorValue_CurrentAngle);
builder_.Finish(message); },
},
return { builder_.GetBufferPointer(), builder_.GetSize() }; v);
} }
auto values_fb_vec = builder_.CreateVector(values_vec);
const auto values_type_fb_vec = builder_.CreateVector(sensor_values_vec);
const auto message =
Messaging::CreateSensorMessage(builder_, values_type_fb_vec, values_fb_vec);
builder_.Finish(message);
return {builder_.GetBufferPointer(), builder_.GetSize()};
} }
} // namespace Flatbuffers

30
components/flatbuffers/include/SensorMessageBuilder.h
View File

@@ -6,19 +6,27 @@
#include "flatbuffers_generated/SensorMessage_generated.h" #include "flatbuffers_generated/SensorMessage_generated.h"
namespace Flatbuffers { namespace Flatbuffers {
struct SensorValueInstance {
uint16_t angle; // todo: change to a variant
};
class SensorMessageBuilder{ struct target_angle {
public: int16_t angle;
SensorMessageBuilder() : builder_(128) {} };
SerializedMessage build_sensor_message(std::vector<SensorValueInstance>& values); struct current_angle {
int16_t angle;
};
private: typedef std::variant<target_angle, current_angle> sensor_value;
flatbuffers::FlatBufferBuilder builder_;
}; class SensorMessageBuilder {
} public:
SensorMessageBuilder() : builder_(128) {
}
SerializedMessage build_sensor_message(std::vector<sensor_value> &values);
private:
flatbuffers::FlatBufferBuilder builder_;
};
} // namespace Flatbuffers
#endif //SENSORMESSAGEBUILDER_H #endif //SENSORMESSAGEBUILDER_H

12
components/flatbuffers/include/Variant.h Normal file
View File

@@ -0,0 +1,12 @@
#ifndef VARIANT_H
#define VARIANT_H
#include <variant> // NOLINT
template <class... Ts> struct overloaded : Ts... {
using Ts::operator()...;
};
template <class... Ts> overloaded(Ts...) -> overloaded<Ts...>;
#endif // VARIANT_H

98
components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h
View File

@@ -14,31 +14,37 @@
namespace Messaging { namespace Messaging {
struct Angle; struct TargetAngle;
struct AngleBuilder; struct TargetAngleBuilder;
struct CurrentAngle;
struct CurrentAngleBuilder;
struct SensorMessage; struct SensorMessage;
struct SensorMessageBuilder; struct SensorMessageBuilder;
enum SensorValue : uint8_t { enum SensorValue : uint8_t {
SensorValue_NONE = 0, SensorValue_NONE = 0,
SensorValue_Angle = 1, SensorValue_TargetAngle = 1,
SensorValue_CurrentAngle = 2,
SensorValue_MIN = SensorValue_NONE, SensorValue_MIN = SensorValue_NONE,
SensorValue_MAX = SensorValue_Angle SensorValue_MAX = SensorValue_CurrentAngle
}; };
inline const SensorValue (&EnumValuesSensorValue())[2] { inline const SensorValue (&EnumValuesSensorValue())[3] {
static const SensorValue values[] = {SensorValue_NONE, SensorValue_Angle}; static const SensorValue values[] = {
SensorValue_NONE, SensorValue_TargetAngle, SensorValue_CurrentAngle};
return values; return values;
} }
inline const char *const *EnumNamesSensorValue() { inline const char *const *EnumNamesSensorValue() {
static const char *const names[3] = {"NONE", "Angle", nullptr}; static const char *const names[4] = {"NONE", "TargetAngle", "CurrentAngle",
nullptr};
return names; return names;
} }
inline const char *EnumNameSensorValue(SensorValue e) { inline const char *EnumNameSensorValue(SensorValue e) {
if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_Angle)) if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_CurrentAngle))
return ""; return "";
const size_t index = static_cast<size_t>(e); const size_t index = static_cast<size_t>(e);
return EnumNamesSensorValue()[index]; return EnumNamesSensorValue()[index];
@@ -48,8 +54,12 @@ template <typename T> struct SensorValueTraits {
static const SensorValue enum_value = SensorValue_NONE; static const SensorValue enum_value = SensorValue_NONE;
}; };
template <> struct SensorValueTraits<Messaging::Angle> { template <> struct SensorValueTraits<Messaging::TargetAngle> {
static const SensorValue enum_value = SensorValue_Angle; static const SensorValue enum_value = SensorValue_TargetAngle;
};
template <> struct SensorValueTraits<Messaging::CurrentAngle> {
static const SensorValue enum_value = SensorValue_CurrentAngle;
}; };
bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj, bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj,
@@ -59,8 +69,8 @@ bool VerifySensorValueVector(
const ::flatbuffers::Vector<::flatbuffers::Offset<void>> *values, const ::flatbuffers::Vector<::flatbuffers::Offset<void>> *values,
const ::flatbuffers::Vector<uint8_t> *types); const ::flatbuffers::Vector<uint8_t> *types);
struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { struct TargetAngle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef AngleBuilder Builder; typedef TargetAngleBuilder Builder;
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_VALUE = 4 VT_VALUE = 4
}; };
@@ -71,26 +81,64 @@ struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
} }
}; };
struct AngleBuilder { struct TargetAngleBuilder {
typedef Angle Table; typedef TargetAngle Table;
::flatbuffers::FlatBufferBuilder &fbb_; ::flatbuffers::FlatBufferBuilder &fbb_;
::flatbuffers::uoffset_t start_; ::flatbuffers::uoffset_t start_;
void add_value(int16_t value) { void add_value(int16_t value) {
fbb_.AddElement<int16_t>(Angle::VT_VALUE, value, 0); fbb_.AddElement<int16_t>(TargetAngle::VT_VALUE, value, 0);
} }
explicit AngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) { explicit TargetAngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
: fbb_(_fbb) {
start_ = fbb_.StartTable(); start_ = fbb_.StartTable();
} }
::flatbuffers::Offset<Angle> Finish() { ::flatbuffers::Offset<TargetAngle> Finish() {
const auto end = fbb_.EndTable(start_); const auto end = fbb_.EndTable(start_);
auto o = ::flatbuffers::Offset<Angle>(end); auto o = ::flatbuffers::Offset<TargetAngle>(end);
return o; return o;
} }
}; };
inline ::flatbuffers::Offset<Angle> inline ::flatbuffers::Offset<TargetAngle>
CreateAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) { CreateTargetAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) {
AngleBuilder builder_(_fbb); TargetAngleBuilder builder_(_fbb);
builder_.add_value(value);
return builder_.Finish();
}
struct CurrentAngle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef CurrentAngleBuilder Builder;
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_VALUE = 4
};
int16_t value() const { return GetField<int16_t>(VT_VALUE, 0); }
bool Verify(::flatbuffers::Verifier &verifier) const {
return VerifyTableStart(verifier) &&
VerifyField<int16_t>(verifier, VT_VALUE, 2) && verifier.EndTable();
}
};
struct CurrentAngleBuilder {
typedef CurrentAngle Table;
::flatbuffers::FlatBufferBuilder &fbb_;
::flatbuffers::uoffset_t start_;
void add_value(int16_t value) {
fbb_.AddElement<int16_t>(CurrentAngle::VT_VALUE, value, 0);
}
explicit CurrentAngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
: fbb_(_fbb) {
start_ = fbb_.StartTable();
}
::flatbuffers::Offset<CurrentAngle> Finish() {
const auto end = fbb_.EndTable(start_);
auto o = ::flatbuffers::Offset<CurrentAngle>(end);
return o;
}
};
inline ::flatbuffers::Offset<CurrentAngle>
CreateCurrentAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) {
CurrentAngleBuilder builder_(_fbb);
builder_.add_value(value); builder_.add_value(value);
return builder_.Finish(); return builder_.Finish();
} }
@@ -171,8 +219,12 @@ inline bool VerifySensorValue(::flatbuffers::Verifier &verifier,
case SensorValue_NONE: { case SensorValue_NONE: {
return true; return true;
} }
case SensorValue_Angle: { case SensorValue_TargetAngle: {
auto ptr = reinterpret_cast<const Messaging::Angle *>(obj); auto ptr = reinterpret_cast<const Messaging::TargetAngle *>(obj);
return verifier.VerifyTable(ptr);
}
case SensorValue_CurrentAngle: {
auto ptr = reinterpret_cast<const Messaging::CurrentAngle *>(obj);
return verifier.VerifyTable(ptr); return verifier.VerifyTable(ptr);
} }
default: default:

24
main/LoopManager.cpp
View File

@@ -19,14 +19,14 @@
[[noreturn]] void LoopManager::control_loop() const { [[noreturn]] void LoopManager::control_loop() const {
uint8_t buffer[512]; uint8_t buffer[512];
while (true) { while (true) {
m_messaging_interface->recv(reinterpret_cast<char *>(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG); m_messaging_interface->recv(reinterpret_cast<char *>(buffer), 512, PC_ADDR,
ACTUATOR_CMD_TAG);
m_actuator->actuate(buffer); m_actuator->actuate(buffer);
send_sensor_reading(false); send_sensor_reading(false);
} }
} }
[[noreturn]] void LoopManager::sensor_loop(char *args) {
[[noreturn]] void LoopManager::sensor_loop(char * args) {
const auto that = reinterpret_cast<LoopManager *>(args); const auto that = reinterpret_cast<LoopManager *>(args);
while (true) { while (true) {
@@ -35,12 +35,14 @@
} }
} }
[[noreturn]] void LoopManager::metadata_tx_loop(char * args) { [[noreturn]] void LoopManager::metadata_tx_loop(char *args) {
const auto that = reinterpret_cast<LoopManager *>(args); const auto that = reinterpret_cast<LoopManager *>(args);
const auto topology_message_builder = std::make_unique<Flatbuffers::TopologyMessageBuilder>(); const auto topology_message_builder = std::make_unique<Flatbuffers::TopologyMessageBuilder>();
while (true) { while (true) {
const auto [module_ids, orientations] = that->m_messaging_interface->get_physically_connected_modules(); const auto [module_ids, orientations] =
// todo: this is awful, we can't cast from a vector of orientation to int.... :( that->m_messaging_interface->get_physically_connected_modules();
// todo: this is awful, we can't cast from a vector of orientation to
// int.... :(
std::vector<int8_t> casted_orientations{}; std::vector<int8_t> casted_orientations{};
casted_orientations.reserve(orientations.size()); casted_orientations.reserve(orientations.size());
for (const auto orientation : orientations) { for (const auto orientation : orientations) {
@@ -48,13 +50,11 @@
} }
const auto [data, size] = topology_message_builder->build_topology_message( const auto [data, size] = topology_message_builder->build_topology_message(
that->m_config_manager.get_module_id(), that->m_config_manager.get_module_id(), that->m_config_manager.get_module_type(),
that->m_config_manager.get_module_type(), module_ids, casted_orientations, that->m_messaging_interface->get_connection_type(),
module_ids,
casted_orientations,
that->m_messaging_interface->get_connection_type(),
that->m_messaging_interface->get_leader()); that->m_messaging_interface->get_leader());
that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, false); that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR,
TOPOLOGY_CMD_TAG, false);
vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS); vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
} }
} }

48
main/control/DCMotorActuator.cpp
View File

@@ -1,11 +1,13 @@
#include <cmath> #include <cmath>
#include "control/DCMotorActuator.h"
#include "esp_attr.h"
#include "util/number_utils.h"
#include "driver/ledc.h"
#include "constants/module.h"
#include "AngleControlMessageBuilder.h" #include "AngleControlMessageBuilder.h"
#include "SensorMessageBuilder.h"
#include "constants/module.h"
#include "control/DCMotorActuator.h"
#include "driver/ledc.h"
#include "esp_attr.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"
#define LOW_DUTY 200 #define LOW_DUTY 200
#define HIGH_DUTY 1000 #define HIGH_DUTY 1000
@@ -42,14 +44,14 @@ DCMotorActuator::DCMotorActuator() {
ESP_ERROR_CHECK(ledc_channel_config(&fwd_ledc_channel)); ESP_ERROR_CHECK(ledc_channel_config(&fwd_ledc_channel));
ledc_channel_config_t rev_ledc_channel = { ledc_channel_config_t rev_ledc_channel = {
.gpio_num = DC_MOTOR_PWM_REV, .gpio_num = DC_MOTOR_PWM_REV,
.speed_mode = LEDC_LOW_SPEED_MODE, .speed_mode = LEDC_LOW_SPEED_MODE,
.channel = REV_CHANNEL, .channel = REV_CHANNEL,
.intr_type = LEDC_INTR_DISABLE, .intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0, .timer_sel = LEDC_TIMER_0,
.duty = 0, .duty = 0,
.hpoint = 0, .hpoint = 0,
}; };
ESP_ERROR_CHECK(ledc_channel_config(&rev_ledc_channel)); ESP_ERROR_CHECK(ledc_channel_config(&rev_ledc_channel));
@@ -60,7 +62,8 @@ DCMotorActuator::DCMotorActuator() {
this->m_integral = 0; this->m_integral = 0;
this->m_last_error = 0; this->m_last_error = 0;
xTaskCreate(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 3072, this, 1, &this->m_pid_task); xTaskCreate(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 3072, this, 1,
&this->m_pid_task);
} }
DCMotorActuator::~DCMotorActuator() { DCMotorActuator::~DCMotorActuator() {
@@ -70,7 +73,7 @@ DCMotorActuator::~DCMotorActuator() {
volatile int32_t encoder_ticks = 0; volatile int32_t encoder_ticks = 0;
volatile int8_t direction = 0; volatile int8_t direction = 0;
static void IRAM_ATTR encoder_isr_handler(void* arg) { static void IRAM_ATTR encoder_isr_handler(void *arg) {
const int a = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_A)); const int a = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_A));
const int b = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_B)); const int b = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_B));
@@ -98,12 +101,13 @@ void DCMotorActuator::setup_encoder() {
} }
void DCMotorActuator::actuate(uint8_t *cmd) { void DCMotorActuator::actuate(uint8_t *cmd) {
const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
this->m_target_angle = angleControlCmd->angle(); this->m_target_angle = angleControlCmd->angle();
} }
void DCMotorActuator::pid_task(char* args) { void DCMotorActuator::pid_task(char *args) {
const auto that = reinterpret_cast<DCMotorActuator*>(args); const auto that = reinterpret_cast<DCMotorActuator *>(args);
while (true) { while (true) {
that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION); that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
@@ -119,7 +123,8 @@ void DCMotorActuator::pid_task(char* args) {
} else if (control < -1) { } else if (control < -1) {
control = -1; control = -1;
} }
const auto pwm = util::mapRange<double>(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY); const auto pwm =
util::mapRange<double>(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY);
if (std::abs(control) < DEADZONE) { if (std::abs(control) < DEADZONE) {
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0)); ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0));
@@ -149,7 +154,8 @@ void DCMotorActuator::pid_task(char* args) {
} }
} }
std::vector<Flatbuffers::SensorValueInstance> DCMotorActuator::get_sensor_data() { std::vector<Flatbuffers::sensor_value> DCMotorActuator::get_sensor_data() {
// todo: this really needs to return a int32, should also return two sensor data items, one for target one for current // todo: this really needs to return a int32, should also return two sensor data items, one for target one for current
return {{(uint16_t)(m_current_angle)}}; return {Flatbuffers::target_angle{(int16_t)m_current_angle},
Flatbuffers::current_angle{(int16_t)m_current_angle}};
} }

58
main/control/Servo1Actuator.cpp
View File

@@ -4,52 +4,52 @@
#include "control/Servo1Actuator.h" #include "control/Servo1Actuator.h"
#include "AngleControlMessageBuilder.h" #include "AngleControlMessageBuilder.h"
#include "SensorMessageBuilder.h"
#include "constants/module.h" #include "constants/module.h"
#include "driver/ledc.h" #include "driver/ledc.h"
#include "flatbuffers_generated/SensorMessage_generated.h" #include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h" #include "util/number_utils.h"
#include "SensorMessageBuilder.h"
#define LOW_DUTY 200 #define LOW_DUTY 200
#define HIGH_DUTY 1000 #define HIGH_DUTY 1000
#define PWM_FREQ 50 // 4khz #define PWM_FREQ 50 // 4khz
Servo1Actuator::Servo1Actuator() { Servo1Actuator::Servo1Actuator() {
ledc_timer_config_t ledc_timer = { ledc_timer_config_t ledc_timer = {
.speed_mode = LEDC_LOW_SPEED_MODE, .speed_mode = LEDC_LOW_SPEED_MODE,
.duty_resolution = LEDC_TIMER_13_BIT, .duty_resolution = LEDC_TIMER_13_BIT,
.timer_num = LEDC_TIMER_0, .timer_num = LEDC_TIMER_0,
.freq_hz = PWM_FREQ, .freq_hz = PWM_FREQ,
.clk_cfg = LEDC_AUTO_CLK, .clk_cfg = LEDC_AUTO_CLK,
}; };
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
ledc_channel_config_t ledc_channel = { ledc_channel_config_t ledc_channel = {
.gpio_num = SERVO_GPIO, .gpio_num = SERVO_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE, .speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_0, .channel = LEDC_CHANNEL_0,
.intr_type = LEDC_INTR_DISABLE, .intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0, .timer_sel = LEDC_TIMER_0,
.duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
.hpoint = 0, .hpoint = 0,
}; };
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
} }
void Servo1Actuator::actuate(uint8_t *cmd) { void Servo1Actuator::actuate(uint8_t *cmd) {
const auto *angleControlCmd = const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, const auto newDuty =
LOW_DUTY, HIGH_DUTY); util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
m_target = angleControlCmd->angle(); m_target = angleControlCmd->angle();
std::cout << "actuating to " << angleControlCmd->angle() << std::endl; std::cout << "actuating to " << angleControlCmd->angle() << std::endl;
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
} }
std::vector<Flatbuffers::SensorValueInstance> Servo1Actuator::get_sensor_data() { std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {
return {{m_target}}; return {Flatbuffers::target_angle{(int16_t)m_target}};
} }

11
main/include/control/DCMotorActuator.h
View File

@@ -3,19 +3,20 @@
#ifndef DCMOTORACTUATOR_H #ifndef DCMOTORACTUATOR_H
#define DCMOTORACTUATOR_H #define DCMOTORACTUATOR_H
#include "IActuator.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/task.h" #include "freertos/task.h"
#include "IActuator.h"
class DCMotorActuator final : public IActuator { class DCMotorActuator final : public IActuator {
public: public:
DCMotorActuator(); DCMotorActuator();
~DCMotorActuator() override; ~DCMotorActuator() override;
void actuate(uint8_t *cmd) override; void actuate(uint8_t *cmd) override;
std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() override; std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
private:
private:
void setup_encoder(); void setup_encoder();
static void pid_task(char* args); static void pid_task(char *args);
double m_current_angle; double m_current_angle;
int64_t m_target_angle; int64_t m_target_angle;

7
main/include/control/IActuator.h
View File

@@ -11,10 +11,11 @@
class IActuator { class IActuator {
public: public:
virtual ~IActuator() {} virtual ~IActuator() {
}
virtual void actuate(uint8_t *cmd) = 0; virtual void actuate(uint8_t *cmd) = 0;
virtual std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() = 0; virtual std::vector<Flatbuffers::sensor_value> get_sensor_data() = 0;
}; };
#endif //IACTUATOR_H #endif //IACTUATOR_H

9
main/include/control/Servo1Actuator.h
View File

@@ -7,16 +7,17 @@
#ifndef SERVO1ACTUATOR_H #ifndef SERVO1ACTUATOR_H
#define SERVO1ACTUATOR_H #define SERVO1ACTUATOR_H
#include <cstdint>
#include "IActuator.h" #include "IActuator.h"
#include "ISensor.h" #include "ISensor.h"
#include <cstdint>
class Servo1Actuator final : public IActuator { class Servo1Actuator final : public IActuator {
public: public:
Servo1Actuator(); Servo1Actuator();
void actuate(std::uint8_t *cmd) override; void actuate(std::uint8_t *cmd) override;
std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() override; std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
private:
private:
uint16_t m_target = 90; uint16_t m_target = 90;
}; };