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

Work on sensor

Browse Source
This commit is contained in:
Johnathon Slightham
2026-01-08 15:46:39 -05:00
parent 857057b1a8
commit 827a098bdc
14 changed files with 165 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
main/control/DCMotorActuator.cpp
View File

@@ -1,3 +1,5 @@
#include <cmath>
#include "control/DCMotorActuator.h"
#include "esp_attr.h"
#include "util/number_utils.h"
@@ -104,9 +106,9 @@ void DCMotorActuator::pid_task(char* args) {
const auto that = reinterpret_cast<DCMotorActuator*>(args);
while (true) {
const double degrees = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
const double error = degrees - that->m_target_angle;
const double error = that->m_current_angle - that->m_target_angle;
that->m_integral += error * KI;
const double detivative = (error - that->m_last_error) * KD;
that->m_last_error = error;
@@ -146,3 +148,8 @@ void DCMotorActuator::pid_task(char* args) {
vTaskDelay(75 / portTICK_PERIOD_MS);
}
}
std::vector<Flatbuffers::SensorValueInstance> 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)}};
}

7
main/control/Servo1Actuator.cpp
View File

@@ -6,7 +6,9 @@
#include "AngleControlMessageBuilder.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
@@ -41,8 +43,13 @@ void Servo1Actuator::actuate(uint8_t *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;
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}};
}