modelec/odo_STM32
1
0
Fork 0
mirror of https://github.com/modelec/odo_STM32.git synced 2026-01-18 16:27:25 +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-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" 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.g++"/>
</provider>
@@ -16,7 +16,7 @@
<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.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.g++"/>
</provider>

6
Core/Inc/modelec.h
View File

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

12
Core/Src/CommCallbacks.cpp
View File

@@ -16,15 +16,15 @@
extern DiffBot bot;
void Comm_GetPos(float& x, float& y, float& theta) {
x = bot.pose.x;
y = bot.pose.y;
theta = bot.pose.theta;
x = bot.pos.x * 1000;
y = bot.pos.y * 1000;
theta = bot.pos.theta;
}
void Comm_SetPos(float x, float y, float theta) {
bot.pose.x = x / 1000;
bot.pose.y = y / 1000;
bot.pose.theta = theta;
bot.pos.x = x / 1000;
bot.pos.y = y / 1000;
bot.pos.theta = theta;
}
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
}
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) {
@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
}
void DiffBot::setup() {
pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
pidPos = PID(1, 0.0, 0.0, -V_MAX, V_MAX);
pidTheta = PID(1, 0.0, 0.0, -2.0f, 2);
pidLeft = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX);
pidRight = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX);
pidPos = PID(3, 0.0, 0.0, -V_MAX, V_MAX);
pidTheta = PID(4, 0.0, 0.0, -2.0f, 2);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -57,15 +57,23 @@ void DiffBot::update(float dt) {
float rightVel = readEncoderRight();
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
float v = (leftVel + rightVel) / 2.0f;
float w = (rightVel - leftVel) / WHEEL_BASE;
pose.x += v * cosf(pose.theta - w/2);
pose.y += v * sinf(pose.theta - w/2);
pose.theta += w;
pos.x += v * cosf(pos.theta - w/2);
pos.y += v * sinf(pos.theta - w/2);
pos.theta += w;
while (pose.theta > M_PI) pose.theta -= 2*M_PI;
while (pose.theta < -M_PI) pose.theta += 2*M_PI;
while (pos.theta > M_PI) pos.theta -= 2*M_PI;
while (pos.theta < -M_PI) pos.theta += 2*M_PI;
if (odo_active) {
publishStatus();
}
if (!odo_active || !targets[index].active) {
motor.update();
@@ -73,13 +81,13 @@ void DiffBot::update(float dt) {
}
// pid setup
float dx = targets[index].x - pose.x;
float dy = targets[index].y - pose.y;
float dx = targets[index].x - pos.x;
float dy = targets[index].y - pos.y;
switch (targets[index].state) {
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;
motor.stop(true);
@@ -111,8 +119,8 @@ void DiffBot::update(float dt) {
resetPID();
dx = targets[index].x - pose.x;
dy = targets[index].y - pose.y;
dx = targets[index].x - pos.x;
dy = targets[index].y - pos.y;
}
@@ -124,7 +132,7 @@ void DiffBot::update(float dt) {
float dist = sqrtf(dx*dx + dy*dy);
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;
@@ -145,7 +153,7 @@ void DiffBot::update(float dt) {
float wRef;
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;
}
else {
@@ -197,3 +205,9 @@ void DiffBot::resetPID() {
pidPos.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));
}