Projetos/OmniBot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Firmware: First PID controller version tested without calibration

Browse Source
This commit is contained in:
Gabriel Lima
2025-07-07 22:25:01 -03:00
parent d13798ef8d
commit 7851dea413
6 changed files with 190 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
Firmware/OmniBot_RC/.cproject
View File

@@ -47,6 +47,7 @@
<listOptionValue builtIn="false" value="../USB_DEVICE/Target"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Application/Shared/MotorDC_BTS7960B_H_Bridge/Inc}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.554786154" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool>
@@ -102,6 +103,7 @@
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
<listOptionValue builtIn="false" value="../Application/Shared/RoboFramework/Inc"/>
<listOptionValue builtIn="false" value="../Application/Shared/RoboFramework_STM32/Inc"/>
<listOptionValue builtIn="false" value="../Application/Shared/MotorDC_BTS7960B_H_Bridge/Inc"/>
</option>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.649380152" name="Language standard" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.value.gnupp20" valueType="enumerated"/>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.625013854" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
@@ -176,6 +178,7 @@
<listOptionValue builtIn="false" value="../USB_DEVICE/Target"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Application/Shared/MotorDC_BTS7960B_H_Bridge/Inc}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.354689137" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool>
@@ -201,6 +204,7 @@
<listOptionValue builtIn="false" value="../USB_DEVICE/Target"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Application/Shared/MotorDC_BTS7960B_H_Bridge/Inc}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.724982810" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool>
@@ -226,6 +230,7 @@
<listOptionValue builtIn="false" value="../USB_DEVICE/Target"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Application/Shared/MotorDC_BTS7960B_H_Bridge/Inc}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1618300618" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
</tool>
@@ -279,5 +284,12 @@
<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="refreshScope"/>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Debug">
<resource resourceType="PROJECT" workspacePath="/OmniBot_RC"/>
</configuration>
<configuration configurationName="Release">
<resource resourceType="PROJECT" workspacePath="/OmniBot_RC"/>
</configuration>
</storageModule>
</cproject>

99
Firmware/OmniBot_RC/Application/Components/OmniBotController/OmniBotController.cpp
View File

@@ -6,10 +6,18 @@
*/
#include "OmniBotController.hpp"
#include <cmath>
#include <numbers>
OmniBotController::OmniBotController(std::array<MotorPID*, 4> motors) :
Executable(Log::Level::Debug, "OmniBotController"),
motors(motors)
OmniBotController::OmniBotController(std::array<MotorDC*, 4> motors,
std::array<Encoder*, 4> encoders,
SoftTimer* pidTimer,
Queue<RobotCommandPacket>* robotPacketQueue) :
Executable(Log::Level::Debug, "OmniBotController"),
motors(motors),
encoders(encoders),
pidTimer(pidTimer),
robotPacketQueue(robotPacketQueue)
{
}
@@ -19,12 +27,93 @@ OmniBotController::~OmniBotController() {
}
int32_t OmniBotController::init() {
return 0;
int32_t errors = 0;
log(Log::Level::Informational, "Initializing");
for(uint32_t i=0; i<numMotors; i++){
errors += motors.at(i)->init();
errors += encoders.at(i)->init();
}
errors += pidTimer->init();
log(Log::Level::Informational, "Initialized with status %d", errors);
return errors;
}
int32_t OmniBotController::execute() {
log(Log::Level::Informational, "Starting execution");
if(pidTimer->start(10, true) < 0){
log(Log::Level::Error, "pidTimer failed to start");
return -1;
}
motors.at(0)->setPower(0);
motors.at(1)->setPower(0);
motors.at(2)->setPower(0);
motors.at(3)->setPower(0);
while(true){
RobotCommandPacket command;
if(robotPacketQueue->receive(&command, 0) >= 0){
log(Log::Level::Debug, "PKT RECV: Vt = %f, Vn = %f, Va = %f", command.tangentSpeed, command.normalSpeed, command.angularSpeed);
state = command;
}
processPID();
pidTimer->waitForCompletion(0xFFFFFFFF);
}
return 0;
}
// Private methods
int32_t OmniBotController::processPID() {
int32_t errors = 0;
static int32_t debugLog = 0;
constexpr float powerLimit = 0.5; // FIXME To preserve the hardware
for(uint32_t i=0; i<numMotors; i++){
float targetWheelSpeed = state.normalSpeed * robotGeometry.wheelSines.at(i) +
state.tangentSpeed * robotGeometry.wheelCosines.at(i) +
state.angularSpeed * robotGeometry.robotRadius;
float deltaTime = 0.01f; // FIXME Definir como constante da classe
static std::array<int32_t, numMotors> currentEncoderCountsArray = {0, 0, 0, 0};
int32_t lastEncoderCounts = currentEncoderCountsArray.at(i);
errors += encoders.at(i)->getPosition(&currentEncoderCountsArray.at(i));
currentEncoderCountsArray.at(i) &= 0x0000FFFF; // Take only least significant 16 bits
int16_t deltaEncoderCounts = static_cast<int16_t>(currentEncoderCountsArray.at(i) - lastEncoderCounts);
float deltaRadians = static_cast<float>(deltaEncoderCounts) * 2.0f * std::numbers::pi_v<float> / robotGeometry.wheelGeometry.encoderCPR;
float currentMotorAngularSpeed = deltaRadians / deltaTime;
float currentWheelAngularSpeed = currentMotorAngularSpeed * robotGeometry.wheelGeometry.reductionRatio;
float currentWheelSpeed = currentWheelAngularSpeed * robotGeometry.wheelGeometry.wheelRadius;
constexpr float kp = 10;
constexpr float ki = 100;
constexpr float kd = 0;
static std::array<float, numMotors> perror = {0, 0, 0, 0};
float lastError = perror.at(i);
perror.at(i) = targetWheelSpeed - currentWheelSpeed;
static std::array<float, numMotors> ierror = {0, 0, 0, 0};
ierror.at(i) += perror.at(i) * deltaTime;
// Clamp ierror.at (anti-windup)
constexpr float ilimit = powerLimit/ki;
if(ierror.at(i) > ilimit){
ierror.at(i) = ilimit;
}else if(ierror.at(i) < -ilimit){
ierror.at(i) = -ilimit;
}
float derror = (perror.at(i) - lastError) / deltaTime;
float motorPower = kp*perror.at(i) + kd*derror + ki*ierror.at(i);
// Clamp motorPower
if(motorPower > powerLimit){
motorPower = powerLimit;
}else if(motorPower < -powerLimit){
motorPower = -powerLimit;
}
errors += motors.at(i)->setPower(motorPower);
if(debugLog == 10){
log(Log::Level::Debug, "M%u kp*perror = %f, ki*ierror = %f, kd*derror = %f", i, kp*perror.at(i), ki*ierror.at(i), kd*derror);
debugLog = 0;
}else{
debugLog++;
}
}
return errors;
}

43
Firmware/OmniBot_RC/Application/Components/OmniBotController/OmniBotController.hpp
View File

@@ -9,18 +9,55 @@
#define COMPONENTS_OMNIBOTCONTROLLER_OMNIBOTCONTROLLER_HPP_
#include <Executable.hpp>
#include <MotorPID.hpp>
#include <MotorDC.hpp>
#include <Encoder.hpp>
#include <SoftTimer.hpp>
#include <Queue.hpp>
#include "../../RobotCommandPacket.hpp"
#include <array>
class OmniBotController : public Executable {
private:
static constexpr uint32_t numMotors = 4;
class RobotGeometry{
public:
class WheelGeometry{
public:
// TODO Conferir, pois parece estar 1s mais rápido
float wheelRadius = 0.029f;
float reductionRatio = 1.0f/90.0f;
int32_t encoderCPR = -52;
}wheelGeometry;
float robotRadius = 0.1045f;
static constexpr std::array<float, numMotors> wheelSines = {
0.7071067812f, // sin(45°)
0.7071067812f, // sin(135°)
-0.7071067812f, // sin(225°)
-0.7071067812f // sin(315°)
};
static constexpr std::array<float, numMotors> wheelCosines = {
0.7071067812f, // cos(45°)
-0.7071067812f, // cos(135°)
-0.7071067812f, // cos(225°)
0.7071067812f // cos(315°)
};
}robotGeometry;
public:
OmniBotController(std::array<MotorPID*, 4> motors);
OmniBotController(std::array<MotorDC*, numMotors> motors,
std::array<Encoder*, numMotors> encoders,
SoftTimer* pidTimer,
Queue<RobotCommandPacket>* robotPacketQueue);
virtual ~OmniBotController();
int32_t init();
int32_t execute();
private:
std::array<MotorPID*, 4> motors;
int32_t processPID();
std::array<MotorDC*, numMotors> motors;
std::array<Encoder*, numMotors> encoders;
SoftTimer* pidTimer;
Queue<RobotCommandPacket>* robotPacketQueue;
RobotCommandPacket state;
};
#endif /* COMPONENTS_OMNIBOTCONTROLLER_OMNIBOTCONTROLLER_HPP_ */

2
Firmware/OmniBot_RC/Application/Shared/RoboFramework

Submodule Firmware/OmniBot_RC/Application/Shared/RoboFramework updated: 95d8f33cd4...5e055d272d

2
Firmware/OmniBot_RC/Application/Shared/RoboFramework_STM32

Submodule Firmware/OmniBot_RC/Application/Shared/RoboFramework_STM32 updated: 9094293c51...de8deae0d0

57
Firmware/OmniBot_RC/Application/start.cpp
View File

@@ -16,11 +16,12 @@
#include "UART_STM32.hpp"
#include "Queue_STM32.hpp"
#include "CDC_STM32.hpp"
#include "MotorPID_STM32.hpp"
#include "Encoder_STM32.hpp"
#include "SoftTimer_STM32.hpp"
#include "Components/ComputerComms_CDC/ComputerComms_CDC.hpp"
#include "Components/OmniBotController/OmniBotController.hpp"
#include "MotorDC_BTS7960_H_Bridge.hpp"
#include "RobotCommandPacket.hpp"
// Leds
GPIO_Pin_STM32 ledOrange(GPIOD, GPIO_PIN_13);
@@ -41,23 +42,40 @@ Encoder_STM32 encoder3(&htim5);
std::array<Encoder*, 4> encoders = {&encoder0, &encoder1, &encoder2, &encoder3};
// Motors
MotorPID_STM32::Configuration motorConfig = {
.wheelRadius = 40,
.reductionRatio = 1/72,
.encoderCountsPerRevolution = 52,
};
MotorPID_STM32 motor0(motorConfig);
MotorPID_STM32 motor1(motorConfig);
MotorPID_STM32 motor2(motorConfig);
MotorPID_STM32 motor3(motorConfig);
std::array<MotorPID*, 4> motors = {&motor0, &motor1, &motor2, &motor3};
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim8;
PWM_Pin_STM32 m0_ina(&htim8, TIM_CHANNEL_3);
PWM_Pin_STM32 m0_inb(&htim8, TIM_CHANNEL_4);
PWM_Pin_STM32 m1_ina(&htim1, TIM_CHANNEL_1);
PWM_Pin_STM32 m1_inb(&htim1, TIM_CHANNEL_2);
PWM_Pin_STM32 m2_ina(&htim1, TIM_CHANNEL_3);
PWM_Pin_STM32 m2_inb(&htim1, TIM_CHANNEL_4);
PWM_Pin_STM32 m3_ina(&htim8, TIM_CHANNEL_1);
PWM_Pin_STM32 m3_inb(&htim8, TIM_CHANNEL_2);
GPIO_Pin_STM32 m0_inha(M0_LEN_GPIO_Port, M0_LEN_Pin);
GPIO_Pin_STM32 m0_inhb(M0_REN_GPIO_Port, M0_REN_Pin);
GPIO_Pin_STM32 m1_inha(M1_LEN_GPIO_Port, M1_LEN_Pin);
GPIO_Pin_STM32 m1_inhb(M1_REN_GPIO_Port, M1_REN_Pin);
GPIO_Pin_STM32 m2_inha(M2_LEN_GPIO_Port, M2_LEN_Pin);
GPIO_Pin_STM32 m2_inhb(M2_REN_GPIO_Port, M2_REN_Pin);
GPIO_Pin_STM32 m3_inha(M3_LEN_GPIO_Port, M3_LEN_Pin);
GPIO_Pin_STM32 m3_inhb(M3_REN_GPIO_Port, M3_REN_Pin);
MotorDC_BTS7960B_H_Bridge dcmotor0(&m0_ina, &m0_inb, &m0_inha, &m0_inhb);
MotorDC_BTS7960B_H_Bridge dcmotor1(&m1_ina, &m1_inb, &m1_inha, &m1_inhb);
MotorDC_BTS7960B_H_Bridge dcmotor2(&m2_ina, &m2_inb, &m2_inha, &m2_inhb);
MotorDC_BTS7960B_H_Bridge dcmotor3(&m3_ina, &m3_inb, &m3_inha, &m3_inhb);
std::array<MotorDC*, 4> dcmotors = {&dcmotor0, &dcmotor1, &dcmotor2, &dcmotor3};
// Queues
Queue_STM32<std::vector<uint8_t>, 4> computerPktQueue("computerPktQueue");
Queue_STM32<RobotCommandPacket, 1> robotPacketQueue("robotPacketQueue");
// Timers
SoftTimer_STM32 pidTimer(nullptr, "pidTimer");
// Components
ComputerComms_CDC componentComputerComms(&computerPktQueue, CDC_STM32::getInstance());
OmniBotController componentOmniBotController(motors);
OmniBotController componentOmniBotController(dcmotors, encoders, &pidTimer, &robotPacketQueue);
void executableDispatch(void* _executable){
Executable* executable = static_cast<Executable*>(_executable);
@@ -68,8 +86,8 @@ void executableDispatch(void* _executable){
void start(){
discoveryLeds.set(1);
printf("----------------INIT----------------\n");
printf("X and Y: %lu\n", HAL_GetUIDw0());
printf("Wafer number: %c\n", (uint8_t)HAL_GetUIDw1());
printf("X and Y: %u, %u\n", (uint16_t)HAL_GetUIDw0(), (uint16_t)HAL_GetUIDw0()>>16);
printf("Wafer number: %u\n", (uint8_t)HAL_GetUIDw1());
printf("Lot number: %c%c%c%c%c%c%c\n",
(uint8_t)(HAL_GetUIDw1()>>8),
(uint8_t)(HAL_GetUIDw1()>>16),
@@ -88,6 +106,13 @@ void start(){
// Init queues
computerPktQueue.init();
robotPacketQueue.init();
RobotCommandPacket testPkt = {
.tangentSpeed = 0,
.normalSpeed = 0,
.angularSpeed = 1
};
robotPacketQueue.send(testPkt, 0);
// Init shared resources
CDC_STM32::getInstance()->init();
@@ -101,7 +126,7 @@ void start(){
xTaskCreate(executableDispatch, "ReceiveFromComputer", 256, &componentComputerComms, 20, &hTaskReceiveFromComputer);
TaskHandle_t hTaskOmniBotController;
xTaskCreate(executableDispatch, "OmniBotController", 256, &componentOmniBotController, 20, &hTaskOmniBotController);
xTaskCreate(executableDispatch, "OmniBotController", 256, &componentOmniBotController, 21, &hTaskOmniBotController);
discoveryLeds.set(3);
vTaskDelete(nullptr);