diff --git a/.cproject b/.cproject
index bd744da..0cf684c 100644
--- a/.cproject
+++ b/.cproject
@@ -23,8 +23,8 @@
-
-
+
+
diff --git a/Core/Src/.idea/Src.iml b/Core/Src/.idea/Src.iml
new file mode 100644
index 0000000..bc2cd87
--- /dev/null
+++ b/Core/Src/.idea/Src.iml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Core/Src/.idea/editor.xml b/Core/Src/.idea/editor.xml
new file mode 100644
index 0000000..95d51a7
--- /dev/null
+++ b/Core/Src/.idea/editor.xml
@@ -0,0 +1,580 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Core/Src/.idea/modules.xml b/Core/Src/.idea/modules.xml
new file mode 100644
index 0000000..0899ec0
--- /dev/null
+++ b/Core/Src/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Core/Src/.idea/workspace.xml b/Core/Src/.idea/workspace.xml
new file mode 100644
index 0000000..2f75b62
--- /dev/null
+++ b/Core/Src/.idea/workspace.xml
@@ -0,0 +1,61 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ {
+ "associatedIndex": 5
+}
+
+
+
+
+
+ {
+ "keyToString": {
+ "RunOnceActivity.RadMigrateCodeStyle": "true",
+ "RunOnceActivity.ShowReadmeOnStart": "true",
+ "RunOnceActivity.cidr.known.project.marker": "true",
+ "RunOnceActivity.readMode.enableVisualFormatting": "true",
+ "cf.first.check.clang-format": "false",
+ "cidr.known.project.marker": "true",
+ "nodejs_package_manager_path": "npm",
+ "vue.rearranger.settings.migration": "true"
+ }
+}
+
+
+
+
+ 1743082598937
+
+
+ 1743082598937
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Core/Src/main.c b/Core/Src/main.c
index c3c7394..87ef9dd 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -47,6 +47,7 @@ void ModelecOdometryLoop();
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
+
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
@@ -102,6 +103,7 @@ int main(void)
HAL_TIM_Encoder_Start(&htim21, TIM_CHANNEL_ALL);
//HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL);
ModelecOdometrySetup();
+
/* USER CODE END 2 */
/* Infinite loop */
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 7357ed9..5f0e213 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -1,11 +1,3 @@
-/*
- * modelec.cpp
- *
- * Created on: Mar 14, 2025
- * Author: allan
- */
-
-
#include "motors.h"
#include "main.h"
#include "stm32l0xx_hal.h"
@@ -13,101 +5,91 @@
#include
#include
-
extern "C" {
- // Paramètres des roues codeuses
- #define N 600 // Impulsions par tour (PPR) du codeur
- #define WHEEL_DIAMETER 0.082 // Diamètre de la roue en mètres (ex: 5 cm)
- #define WHEEL_BASE 0.17 // Entraxe entre les roues en mètres (ex: 15 cm)
- #define MAX_COUNT 65535 // Max compteur (16 bits)
+extern TIM_HandleTypeDef htim3;
+extern TIM_HandleTypeDef htim2;
+extern TIM_HandleTypeDef htim21;
+extern UART_HandleTypeDef huart2;
- // Position du robot
- volatile float X = 0.0, Y = 0.0, theta = 0.0, totalDistance = 0.0;
+// Variables globales
+#define IMPULSIONS 600
+#define WHEEL_DIAMETER 0.082
+#define WHEEL_BASE 0.17
+#define MAX_COUNT 65535
- // Dernières valeurs des encodeurs
- volatile int32_t last_pos_right = 0, last_pos_left = 0;
+// Contrôle des moteurs
+Motor motor(TIM3);
- // Variables globales
- Motor motor(TIM3);
-
- extern TIM_HandleTypeDef htim3;
- extern TIM_HandleTypeDef htim2;
- extern TIM_HandleTypeDef htim21;
- extern UART_HandleTypeDef huart2;
-
- uint32_t lastTick = 0; // Variable pour mémoriser l'heure de la dernière action
- // Fonction pour vérifier si le délai est passé (permet de ne pas bloquer l'exécution)
- bool isDelayPassed(uint32_t delay) {
- if (HAL_GetTick() - lastTick >= delay) {
- lastTick = HAL_GetTick(); // Met à jour lastTick
- return true;
- }
- return false;
- }
-
- // Fonction de setup appelee au debut du main.c
- void ModelecOdometrySetup(){
- HAL_UART_Transmit(&huart2, (uint8_t *)"SETUP COMPLETE\n", 15, HAL_MAX_DELAY);
- motor.accelerer(300);
- }
-
- // Fonction de mise à jour de l'odométrie
- void ModelecOdometryUpdate() {
- // Lire les compteurs des encodeurs
- int32_t pos_right = __HAL_TIM_GET_COUNTER(&htim21);
- int32_t pos_left = __HAL_TIM_GET_COUNTER(&htim2);
-
- // Calcul des variations d’impulsions
- int32_t deltaP_right = pos_right - last_pos_right;
- int32_t deltaP_left = pos_left - last_pos_left;
-
- // Gestion du débordement du compteur
- if (deltaP_right > MAX_COUNT / 2) deltaP_right -= MAX_COUNT;
- if (deltaP_right < -MAX_COUNT / 2) deltaP_right += MAX_COUNT;
- if (deltaP_left > MAX_COUNT / 2) deltaP_left -= MAX_COUNT;
- if (deltaP_left < -MAX_COUNT / 2) deltaP_left += MAX_COUNT;
-
- // Convertir en distances parcourues (assurer que c'est en millimètres)
- float deltaS_right = ((deltaP_right / (float)(N * 4)) * M_PI * WHEEL_DIAMETER) * 1000.0;
- float deltaS_left = ((deltaP_left / (float)(N * 4)) * M_PI * WHEEL_DIAMETER) * 1000.0;
-
- // Calcul de la distance totale parcourue
- float deltaS = (deltaS_right + deltaS_left) / 2.0;
-
- // Ajouter à la distance totale parcourue
- totalDistance += fabs(deltaS); // fabs() ensures we only add positive values
-
- // Debugging output
- char msg[128];
- sprintf(msg, "Total Distance: %.3f mm\r\n", totalDistance);
- HAL_UART_Transmit(&huart2, (uint8_t *)msg, strlen(msg), HAL_MAX_DELAY);
-
- // Conserver les valeurs actuelles pour la prochaine itération
- last_pos_right = pos_right;
- last_pos_left = pos_left;
-
- }
-
- // Fonction de loop appelee en boucle dans le main.c
- void ModelecOdometryLoop(){
- GPIOC->ODR ^= (1<<10);
-
- //On actualise toute les 10ms et on effectue tous les controles périodiques
- if(isDelayPassed(10)) {
- ModelecOdometryUpdate();
- //On met à jour le statut des moteurs
- motor.update();
- //15 cm
- if(totalDistance >= 150) {
- motor.stop();
- }
- }
- }
+// Données odométriques
+volatile int32_t lastPosRight = 0, lastPosLeft = 0;
+volatile float totalDistance = 0.0f;
+volatile float posX = 0.0f, posY = 0.0f, theta = 0.0f;
+uint32_t lastTick = 0;
+bool isDelayPassed(uint32_t delay) {
+ if (HAL_GetTick() - lastTick >= delay) {
+ lastTick = HAL_GetTick();
+ return true;
+ }
+ return false;
}
+void ModelecOdometrySetup() {
+ HAL_UART_Transmit(&huart2, (uint8_t*) "SETUP COMPLETE\n", 15, HAL_MAX_DELAY);
+ motor.accelerer(300);
+}
+void ModelecOdometryUpdate() {
+ int32_t posRight = __HAL_TIM_GET_COUNTER(&htim2);
+ int32_t posLeft = __HAL_TIM_GET_COUNTER(&htim21);
+ int32_t deltaP_right = posRight - lastPosRight;
+ int32_t deltaP_left = posLeft - lastPosLeft;
+ if (deltaP_right > MAX_COUNT / 2) deltaP_right -= MAX_COUNT;
+ if (deltaP_right < -MAX_COUNT / 2) deltaP_right += MAX_COUNT;
+ if (deltaP_left > MAX_COUNT / 2) deltaP_left -= MAX_COUNT;
+ if (deltaP_left < -MAX_COUNT / 2) deltaP_left += MAX_COUNT;
+
+ float deltaS_right = ((deltaP_right / (float)(IMPULSIONS * 4)) * M_PI * WHEEL_DIAMETER) * 1000.0f;
+ float deltaS_left = ((deltaP_left / (float)(IMPULSIONS * 4)) * M_PI * WHEEL_DIAMETER) * 1000.0f;
+
+ float deltaS = (deltaS_right + deltaS_left) / 2.0f;
+ //Calcul de l'angle theta
+ float deltaTheta = (deltaS_right - deltaS_left) / WHEEL_BASE;
+
+ //On met à jour la distance parcourue totale
+ totalDistance += fabs(deltaS);
+
+ theta += deltaTheta;
+
+ float deltaX = deltaS * cos(theta);
+ float deltaY = deltaS * sin(theta);
+
+ //Mise à jour de la position X et Y
+ posX += deltaX;
+ posY += deltaY;
+
+ lastPosRight = posRight;
+ lastPosLeft = posLeft;
+
+ char msg[128];
+ sprintf(msg, "X: %.3f mm, Y: %.3f mm, Theta: %.3f rad, Distance: %.3f mm\r\n", posX, posY, theta, totalDistance);
+ HAL_UART_Transmit(&huart2, (uint8_t *)msg, strlen(msg), HAL_MAX_DELAY);
+}
+
+void ModelecOdometryLoop() {
+ GPIOC->ODR ^= (1 << 10);
+
+ if (isDelayPassed(10)) {
+ ModelecOdometryUpdate();
+ motor.update();
+ if (totalDistance >= 150) {
+ motor.stop();
+ }
+ }
+}
+
+}