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 550d0643e7
commit 2688ed1290
10 changed files with 218 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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;
};