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

angle between -pi and pi

Browse Source 
value for new wheel
This commit is contained in:
ackimixs
2025-12-16 23:16:16 +01:00
parent 10a56daddd
commit ac68633f01
5 changed files with 13 additions and 11 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="-1135061305286628675" 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="-161583691639640240" 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="-1135061305286628675" 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="-161583691639640240" 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>

4
Core/Inc/modelec.h
View File

@@ -18,9 +18,9 @@
#define MAX_WAYPOINTS 16
#define PWM_MAX 626.0f
#define ENCODER_RES 2400.0f
#define WHEEL_DIAMETER 0.09735f
#define WHEEL_DIAMETER 0.0985f
#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
#define WHEEL_BASE 0.2844f
#define WHEEL_BASE 0.289f
#define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
#define V_MAX 0.643f // m/s

4
Core/Src/CommCallbacks.cpp
View File

@@ -23,6 +23,8 @@ void Comm_GetPos(float& x, float& y, float& theta) {
}
void Comm_SetPos(float x, float y, float theta) {
theta = std::atan2(std::sin(theta), std::cos(theta));
bot.pos.x = x / 1000;
bot.pos.y = y / 1000;
bot.pos.theta = theta;
@@ -76,6 +78,8 @@ void Comm_StartOdometry(bool on) {
}
void Comm_AddWaypoint(int id, int type, float x, float y, float theta) {
theta = std::atan2(std::sin(theta), std::cos(theta));
bot.addTarget(id, type, x / 1000.0f, y / 1000.0f, theta);
}

10
Core/Src/modelec.cpp
View File

@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
}
void DiffBot::setup() {
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, -M_PI, M_PI);
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(2, 0.0, 0.0, -M_PI, M_PI);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -59,13 +59,11 @@ void DiffBot::update(float dt) {
if (rightVel == 0 && leftVel == 0) {
if (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
if (!no_move) {
no_move = true;
publishNotMoved = HAL_GetTick();
}
else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
// BUG : apres ca le robot ne s'arrete plus quand on le relance
motor.stop(true);
targets[index].active = false;

2
Core/Startup/startup_stm32g491keux.s
View File

@@ -59,7 +59,7 @@ defined in linker script */
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
ldr r0, =_estack
ldr r0, =_estack
mov sp, r0 /* set stack pointer */
/* Call the clock system initialization function.*/