modelec/odo_STM32
1
0
Fork 0
mirror of https://github.com/modelec/odo_STM32.git synced 2026-03-18 21:30:38 +01:00
Code Issues Packages Projects Releases Wiki Activity

log pos every 10ms when odo active

Browse Source
This commit is contained in:
acki
2025-10-08 09:07:18 +02:00
parent 1dbd11bf86
commit efb7e3e855
4 changed files with 43 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.settings/language.settings.xml
View File

@@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1248651181809206210" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-871438226299096058" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1248651181809206210" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-871438226299096058" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>

6
Core/Inc/modelec.h
View File

@@ -32,7 +32,7 @@ extern TIM_HandleTypeDef htim2;
class DiffBot { class DiffBot {
public: public:
Point pose; Point pos;
Point targets[MAX_WAYPOINTS] = { Point targets[MAX_WAYPOINTS] = {
Point(0, FINAL, 0, 0, 0), Point(0, FINAL, 0, 0, 0),
@@ -64,7 +64,7 @@ public:
float readEncoderLeft(); float readEncoderLeft();
DiffBot(Point pose, float dt); DiffBot(Point pos, float dt);
void stop(bool stop); void stop(bool stop);
@@ -75,6 +75,8 @@ public:
void addTarget(int id, int type, float x, float y, float theta); void addTarget(int id, int type, float x, float y, float theta);
void resetPID(); void resetPID();
void publishStatus();
}; };
#endif // MODELEC_H #endif // MODELEC_H

12
Core/Src/CommCallbacks.cpp
View File

@@ -16,15 +16,15 @@
extern DiffBot bot; extern DiffBot bot;
void Comm_GetPos(float& x, float& y, float& theta) { void Comm_GetPos(float& x, float& y, float& theta) {
x = bot.pose.x; x = bot.pos.x * 1000;
y = bot.pose.y; y = bot.pos.y * 1000;
theta = bot.pose.theta; theta = bot.pos.theta;
} }
void Comm_SetPos(float x, float y, float theta) { void Comm_SetPos(float x, float y, float theta) {
bot.pose.x = x / 1000; bot.pos.x = x / 1000;
bot.pose.y = y / 1000; bot.pos.y = y / 1000;
bot.pose.theta = theta; bot.pos.theta = theta;
} }
void Comm_GetSpeed(float& vx, float& vy, float& omega) { void Comm_GetSpeed(float& vx, float& vy, float& omega) {

48
Core/Src/modelec.cpp
View File

@@ -32,7 +32,7 @@ float DiffBot::readEncoderRight() {
return (2.0f*M_PI*WHEEL_RADIUS*revs); // m return (2.0f*M_PI*WHEEL_RADIUS*revs); // m
} }
DiffBot::DiffBot(Point pose, float dt) : pose(pose), dt(dt) { DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt) {
}; };
void DiffBot::stop(bool stop) { void DiffBot::stop(bool stop) {
@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
} }
void DiffBot::setup() { void DiffBot::setup() {
pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidLeft = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX);
pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidRight = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX);
pidPos = PID(1, 0.0, 0.0, -V_MAX, V_MAX); pidPos = PID(3, 0.0, 0.0, -V_MAX, V_MAX);
pidTheta = PID(1, 0.0, 0.0, -2.0f, 2); pidTheta = PID(4, 0.0, 0.0, -2.0f, 2);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2); prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3); prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -57,15 +57,23 @@ void DiffBot::update(float dt) {
float rightVel = readEncoderRight(); float rightVel = readEncoderRight();
float leftVel = readEncoderLeft(); float leftVel = readEncoderLeft();
if (rightVel == 0 && leftVel == 0 && (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0)) {
// TODO add something when the robot is stuck on the wall so the motor value are >= 0 but the encoder value are = 0
}
// update pos // update pos
float v = (leftVel + rightVel) / 2.0f; float v = (leftVel + rightVel) / 2.0f;
float w = (rightVel - leftVel) / WHEEL_BASE; float w = (rightVel - leftVel) / WHEEL_BASE;
pose.x += v * cosf(pose.theta - w/2); pos.x += v * cosf(pos.theta - w/2);
pose.y += v * sinf(pose.theta - w/2); pos.y += v * sinf(pos.theta - w/2);
pose.theta += w; pos.theta += w;
while (pose.theta > M_PI) pose.theta -= 2*M_PI; while (pos.theta > M_PI) pos.theta -= 2*M_PI;
while (pose.theta < -M_PI) pose.theta += 2*M_PI; while (pos.theta < -M_PI) pos.theta += 2*M_PI;
if (odo_active) {
publishStatus();
}
if (!odo_active || !targets[index].active) { if (!odo_active || !targets[index].active) {
motor.update(); motor.update();
@@ -73,13 +81,13 @@ void DiffBot::update(float dt) {
} }
// pid setup // pid setup
float dx = targets[index].x - pose.x; float dx = targets[index].x - pos.x;
float dy = targets[index].y - pose.y; float dy = targets[index].y - pos.y;
switch (targets[index].state) { switch (targets[index].state) {
case FINAL: case FINAL:
if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pose.theta) < PRECISE_ANGLE) { if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pos.theta) < PRECISE_ANGLE) {
targets[index].active = false; targets[index].active = false;
motor.stop(true); motor.stop(true);
@@ -111,8 +119,8 @@ void DiffBot::update(float dt) {
resetPID(); resetPID();
dx = targets[index].x - pose.x; dx = targets[index].x - pos.x;
dy = targets[index].y - pose.y; dy = targets[index].y - pos.y;
} }
@@ -124,7 +132,7 @@ void DiffBot::update(float dt) {
float dist = sqrtf(dx*dx + dy*dy); float dist = sqrtf(dx*dx + dy*dy);
float angleTarget = atan2f(dy, dx); float angleTarget = atan2f(dy, dx);
float angleError = angleTarget - pose.theta; float angleError = angleTarget - pos.theta;
while (angleError > M_PI) angleError -= 2*M_PI; while (angleError > M_PI) angleError -= 2*M_PI;
while (angleError < -M_PI) angleError += 2*M_PI; while (angleError < -M_PI) angleError += 2*M_PI;
@@ -145,7 +153,7 @@ void DiffBot::update(float dt) {
float wRef; float wRef;
if (targets[index].state == FINAL && std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL) { if (targets[index].state == FINAL && std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL) {
wRef = pidTheta.compute(targets[index].theta, pose.theta, dt); wRef = pidTheta.compute(targets[index].theta, pos.theta, dt);
vRef = 0; vRef = 0;
} }
else { else {
@@ -197,3 +205,9 @@ void DiffBot::resetPID() {
pidPos.reset(); pidPos.reset();
pidTheta.reset(); pidTheta.reset();
} }
void DiffBot::publishStatus() {
char response[64];
snprintf(response, sizeof(response), "SET;POS;%.4f;%.4f;%.4f\n", pos.x*1000, pos.y*1000, pos.theta);
CDC_Transmit_FS((uint8_t*)response, strlen(response));
}