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 "SerializedMessage.h"
#include "Variant.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
namespace Flatbuffers {
SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<SensorValueInstance>& values) {
builder_.Clear();
SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<sensor_value> &values) {
builder_.Clear();
std::vector<flatbuffers::Offset<void>> values_vec;
std::vector<uint8_t> sensor_values_vec;
std::vector<flatbuffers::Offset<void>> values_vec;
std::vector<uint8_t> sensor_values_vec;
for (const auto& v : values) {
values_vec.push_back(Messaging::CreateAngle(builder_, v.angle).Union());
sensor_values_vec.push_back(Messaging::SensorValue_Angle);
}
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() };
for (const auto &v : values) {
std::visit(
overloaded{
[&](target_angle a) {
values_vec.push_back(Messaging::CreateTargetAngle(builder_, a.angle).Union());
sensor_values_vec.push_back(Messaging::SensorValue_TargetAngle);
},
[&](current_angle a) {
values_vec.push_back(Messaging::CreateCurrentAngle(builder_, a.angle).Union());
sensor_values_vec.push_back(Messaging::SensorValue_CurrentAngle);
},
},
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"
namespace Flatbuffers {
struct SensorValueInstance {
uint16_t angle; // todo: change to a variant
};
class SensorMessageBuilder{
public:
SensorMessageBuilder() : builder_(128) {}
struct target_angle {
int16_t angle;
};
SerializedMessage build_sensor_message(std::vector<SensorValueInstance>& values);
struct current_angle {
int16_t angle;
};
private:
flatbuffers::FlatBufferBuilder builder_;
};
}
typedef std::variant<target_angle, current_angle> sensor_value;
class SensorMessageBuilder {
public:
SensorMessageBuilder() : builder_(128) {
}
SerializedMessage build_sensor_message(std::vector<sensor_value> &values);
private:
flatbuffers::FlatBufferBuilder builder_;
};
} // namespace Flatbuffers
#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 {
struct Angle;
struct AngleBuilder;
struct TargetAngle;
struct TargetAngleBuilder;
struct CurrentAngle;
struct CurrentAngleBuilder;
struct SensorMessage;
struct SensorMessageBuilder;
enum SensorValue : uint8_t {
SensorValue_NONE = 0,
SensorValue_Angle = 1,
SensorValue_TargetAngle = 1,
SensorValue_CurrentAngle = 2,
SensorValue_MIN = SensorValue_NONE,
SensorValue_MAX = SensorValue_Angle
SensorValue_MAX = SensorValue_CurrentAngle
};
inline const SensorValue (&EnumValuesSensorValue())[2] {
static const SensorValue values[] = {SensorValue_NONE, SensorValue_Angle};
inline const SensorValue (&EnumValuesSensorValue())[3] {
static const SensorValue values[] = {
SensorValue_NONE, SensorValue_TargetAngle, SensorValue_CurrentAngle};
return values;
}
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;
}
inline const char *EnumNameSensorValue(SensorValue e) {
if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_Angle))
if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_CurrentAngle))
return "";
const size_t index = static_cast<size_t>(e);
return EnumNamesSensorValue()[index];
@@ -48,8 +54,12 @@ template <typename T> struct SensorValueTraits {
static const SensorValue enum_value = SensorValue_NONE;
};
template <> struct SensorValueTraits<Messaging::Angle> {
static const SensorValue enum_value = SensorValue_Angle;
template <> struct SensorValueTraits<Messaging::TargetAngle> {
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,
@@ -59,8 +69,8 @@ bool VerifySensorValueVector(
const ::flatbuffers::Vector<::flatbuffers::Offset<void>> *values,
const ::flatbuffers::Vector<uint8_t> *types);
struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef AngleBuilder Builder;
struct TargetAngle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef TargetAngleBuilder Builder;
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_VALUE = 4
};
@@ -71,26 +81,64 @@ struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
}
};
struct AngleBuilder {
typedef Angle Table;
struct TargetAngleBuilder {
typedef TargetAngle Table;
::flatbuffers::FlatBufferBuilder &fbb_;
::flatbuffers::uoffset_t start_;
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();
}
::flatbuffers::Offset<Angle> Finish() {
::flatbuffers::Offset<TargetAngle> Finish() {
const auto end = fbb_.EndTable(start_);
auto o = ::flatbuffers::Offset<Angle>(end);
auto o = ::flatbuffers::Offset<TargetAngle>(end);
return o;
}
};
inline ::flatbuffers::Offset<Angle>
CreateAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) {
AngleBuilder builder_(_fbb);
inline ::flatbuffers::Offset<TargetAngle>
CreateTargetAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) {
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);
return builder_.Finish();
}
@@ -171,8 +219,12 @@ inline bool VerifySensorValue(::flatbuffers::Verifier &verifier,
case SensorValue_NONE: {
return true;
}
case SensorValue_Angle: {
auto ptr = reinterpret_cast<const Messaging::Angle *>(obj);
case SensorValue_TargetAngle: {
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);
}
default:

24
main/LoopManager.cpp
View File

@@ -19,14 +19,14 @@
[[noreturn]] void LoopManager::control_loop() const {
uint8_t buffer[512];
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);
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);
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 topology_message_builder = std::make_unique<Flatbuffers::TopologyMessageBuilder>();
while (true) {
const auto [module_ids, orientations] = that->m_messaging_interface->get_physically_connected_modules();
// todo: this is awful, we can't cast from a vector of orientation to int.... :(
const auto [module_ids, orientations] =
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{};
casted_orientations.reserve(orientations.size());
for (const auto orientation : orientations) {
@@ -48,13 +50,11 @@
}
const auto [data, size] = topology_message_builder->build_topology_message(
that->m_config_manager.get_module_id(),
that->m_config_manager.get_module_type(),
module_ids,
casted_orientations,
that->m_messaging_interface->get_connection_type(),
that->m_config_manager.get_module_id(), that->m_config_manager.get_module_type(),
module_ids, casted_orientations, that->m_messaging_interface->get_connection_type(),
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);
}
}

48
main/control/DCMotorActuator.cpp
View File

@@ -1,11 +1,13 @@
#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 "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 HIGH_DUTY 1000
@@ -42,14 +44,14 @@ DCMotorActuator::DCMotorActuator() {
ESP_ERROR_CHECK(ledc_channel_config(&fwd_ledc_channel));
ledc_channel_config_t rev_ledc_channel = {
.gpio_num = DC_MOTOR_PWM_REV,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = REV_CHANNEL,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = 0,
.hpoint = 0,
};
.gpio_num = DC_MOTOR_PWM_REV,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = REV_CHANNEL,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = 0,
.hpoint = 0,
};
ESP_ERROR_CHECK(ledc_channel_config(&rev_ledc_channel));
@@ -60,7 +62,8 @@ DCMotorActuator::DCMotorActuator() {
this->m_integral = 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() {
@@ -70,7 +73,7 @@ DCMotorActuator::~DCMotorActuator() {
volatile int32_t encoder_ticks = 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 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) {
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();
}
void DCMotorActuator::pid_task(char* args) {
const auto that = reinterpret_cast<DCMotorActuator*>(args);
void DCMotorActuator::pid_task(char *args) {
const auto that = reinterpret_cast<DCMotorActuator *>(args);
while (true) {
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) {
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) {
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
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 "AngleControlMessageBuilder.h"
#include "SensorMessageBuilder.h"
#include "constants/module.h"
#include "driver/ledc.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"
#include "SensorMessageBuilder.h"
#define LOW_DUTY 200
#define HIGH_DUTY 1000
#define PWM_FREQ 50 // 4khz
Servo1Actuator::Servo1Actuator() {
ledc_timer_config_t ledc_timer = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.duty_resolution = LEDC_TIMER_13_BIT,
.timer_num = LEDC_TIMER_0,
.freq_hz = PWM_FREQ,
.clk_cfg = LEDC_AUTO_CLK,
};
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
ledc_timer_config_t ledc_timer = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.duty_resolution = LEDC_TIMER_13_BIT,
.timer_num = LEDC_TIMER_0,
.freq_hz = PWM_FREQ,
.clk_cfg = LEDC_AUTO_CLK,
};
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
ledc_channel_config_t ledc_channel = {
.gpio_num = SERVO_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_0,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
.hpoint = 0,
};
ledc_channel_config_t ledc_channel = {
.gpio_num = SERVO_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_0,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
.hpoint = 0,
};
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
}
void Servo1Actuator::actuate(uint8_t *cmd) {
const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180,
LOW_DUTY, HIGH_DUTY);
const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
const auto newDuty =
util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
m_target = angleControlCmd->angle();
std::cout << "actuating to " << angleControlCmd->angle() << std::endl;
m_target = angleControlCmd->angle();
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_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
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));
}
std::vector<Flatbuffers::SensorValueInstance> Servo1Actuator::get_sensor_data() {
return {{m_target}};
std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {
return {Flatbuffers::target_angle{(int16_t)m_target}};
}

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

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

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

@@ -11,10 +11,11 @@
class IActuator {
public:
virtual ~IActuator() {}
public:
virtual ~IActuator() {
}
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

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

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