diff --git a/.cproject b/.cproject
index 291a7b8..bd744da 100644
--- a/.cproject
+++ b/.cproject
@@ -22,7 +22,9 @@
-
+
+
+
@@ -108,26 +110,26 @@
-
-
-
-
-
-
+
+
+
+
+
+
-
+
-
+
-
-
-
+
+
-
-
-
-
+
+
-
-
+
@@ -199,6 +201,13 @@
-
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ControleMoteur2.ioc b/ControleMoteur2.ioc
index 1958301..2670ad8 100644
--- a/ControleMoteur2.ioc
+++ b/ControleMoteur2.ioc
@@ -50,12 +50,12 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.SVC_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
NVIC.SysTick_IRQn=true\:0\:0\:true\:false\:true\:true\:true\:false
PA0.GPIOParameters=GPIO_Speed,GPIO_PuPd
-PA0.GPIO_PuPd=GPIO_NOPULL
-PA0.GPIO_Speed=GPIO_SPEED_FREQ_LOW
+PA0.GPIO_PuPd=GPIO_PULLUP
+PA0.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA0.Signal=S_TIM2_CH1
PA1.GPIOParameters=GPIO_Speed,GPIO_PuPd
-PA1.GPIO_PuPd=GPIO_NOPULL
-PA1.GPIO_Speed=GPIO_SPEED_FREQ_LOW
+PA1.GPIO_PuPd=GPIO_PULLUP
+PA1.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA1.Signal=S_TIM2_CH2
PA13.GPIOParameters=GPIO_Label
PA13.GPIO_Label=TMS
@@ -81,11 +81,13 @@ PA5.GPIOParameters=GPIO_Label
PA5.GPIO_Label=LD2 [Green Led]
PA5.Locked=true
PA5.Signal=GPIO_Output
-PB13.GPIOParameters=GPIO_PuPd
-PB13.GPIO_PuPd=GPIO_NOPULL
+PB13.GPIOParameters=GPIO_Speed,GPIO_PuPd
+PB13.GPIO_PuPd=GPIO_PULLUP
+PB13.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PB13.Signal=S_TIM21_CH1
-PB14.GPIOParameters=GPIO_PuPd
-PB14.GPIO_PuPd=GPIO_NOPULL
+PB14.GPIOParameters=GPIO_Speed,GPIO_PuPd
+PB14.GPIO_PuPd=GPIO_PULLUP
+PB14.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PB14.Signal=S_TIM21_CH2
PC10.Locked=true
PC10.Signal=GPIO_Output
diff --git a/Core/Inc/motors.h b/Core/Inc/motors.h
index 4096bab..ea90bbf 100644
--- a/Core/Inc/motors.h
+++ b/Core/Inc/motors.h
@@ -15,8 +15,6 @@ public:
Motor(TIM_TypeDef *timer);
void accelerer(int speed);
void reculer(int speed);
- void ralentir();
- void ralentirEnvers();
void stop();
void update();
};
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 5f09bd1..c3c7394 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -57,7 +57,6 @@ void SystemClock_Config(void);
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
-int32_t counter = 0;
/* USER CODE END 0 */
@@ -110,12 +109,11 @@ int main(void)
while (1)
{
ModelecOdometryLoop();
- counter = __HAL_TIM_GET_COUNTER(&htim2);
- //__HAL_TIM_IS_TIM_COUNTING_DOWN
- /* USER CODE END WHILE */
}
- /* USER CODE BEGIN 3 */
+ /* USER CODE END WHILE */
+
+ /* USER CODE BEGIN 3 */
/* USER CODE END 3 */
}
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 7d1a692..a99d7e9 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -19,12 +19,12 @@ extern "C" {
// Paramètres des roues codeuses
#define N 600 // Impulsions par tour (PPR) du codeur
- #define WHEEL_DIAMETER 0.06 // Diamètre de la roue en mètres (ex: 5 cm)
- #define WHEEL_BASE 0.15 // Entraxe entre les roues en mètres (ex: 15 cm)
+ #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)
// Position du robot
- volatile float X = 0.0, Y = 0.0, theta = 0.0;
+ volatile float X = 0.0, Y = 0.0, theta = 0.0, totalDistance = 0.0;
// Dernières valeurs des encodeurs
volatile int32_t last_pos_right = 0, last_pos_left = 0;
@@ -43,59 +43,60 @@ extern "C" {
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();
+ }
}
}
- // Fonction de mise à jour de l'odométrie
- void ModelecOdometryUpdate() {
- // Lire les compteurs des encodeurs
- int32_t pos_right = __HAL_TIM_GET_COUNTER(&htim2);
- int32_t pos_left = __HAL_TIM_GET_COUNTER(&htim21);
- // Debug
- char msg[128];
- sprintf(msg, "Encodeur droit %ld.\r\n", pos_right);
- sprintf(msg, "Encodeur gauche %ld.\r\n", pos_left);
- HAL_UART_Transmit(&huart2, (uint8_t *)msg, strlen(msg), HAL_MAX_DELAY);
-
- // 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
- float deltaS_right = (deltaP_right / (float)(N * 4)) * M_PI * WHEEL_DIAMETER;
- float deltaS_left = (deltaP_left / (float)(N * 4)) * M_PI * WHEEL_DIAMETER;
-
- // Calcul de la distance totale et du changement d’angle
- float deltaS = (deltaS_right + deltaS_left) / 2.0;
- float deltaTheta = (deltaS_right - deltaS_left) / WHEEL_BASE;
-
- // Mise à jour de la position
- X += deltaS * cos(theta + deltaTheta / 2.0);
- Y += deltaS * sin(theta + deltaTheta / 2.0);
- theta += deltaTheta;
-
- // Conserver les valeurs actuelles pour la prochaine itération
- last_pos_right = pos_right;
- last_pos_left = pos_left;
-
-
- }
}
diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp
index 84b4973..f1fffe9 100644
--- a/Core/Src/motors.cpp
+++ b/Core/Src/motors.cpp
@@ -17,33 +17,25 @@ void Motor::reculer(int speed) {
isAccelerating = false;
}
-void Motor::ralentir() {
- targetSpeed = 0;
- isAccelerating = true;
- isReversing = false;
-}
-
-void Motor::ralentirEnvers() {
- targetSpeed = 0;
- isReversing = true;
- isAccelerating = false;
-}
-
void Motor::stop() {
- currentSpeed = 0;
targetSpeed = 0;
- isAccelerating = false;
- isReversing = false;
}
+
void Motor::update() {
// Gestion de l'accélération/décélération
if (isAccelerating && currentSpeed < targetSpeed) {
currentSpeed++;
} else if (isReversing && currentSpeed > -targetSpeed) {
currentSpeed--;
- } else if (!isAccelerating && !isReversing && currentSpeed > targetSpeed) {
- currentSpeed--;
+ } else if (currentSpeed > targetSpeed) {
+ if(currentSpeed - 25 >= 0) {
+ //Ralentir rapidement
+ currentSpeed-=25;
+ }
+ else {
+ currentSpeed--;
+ }
}
// Mise à jour des registres du timer
@@ -51,17 +43,10 @@ void Motor::update() {
//2 et 3 avance
tim->CCR2 = currentSpeed;
tim->CCR3 = currentSpeed;
-
} else if (isReversing) {
// 1 et 4 recule
tim->CCR1 = currentSpeed;
tim->CCR4 = currentSpeed;
-
}
- // Arrêt si vitesse cible atteinte
- if (currentSpeed == targetSpeed) {
- isAccelerating = false;
- isReversing = false;
- }
}
diff --git a/Core/Src/tim.c b/Core/Src/tim.c
index da3359d..d2678c8 100644
--- a/Core/Src/tim.c
+++ b/Core/Src/tim.c
@@ -206,8 +206,8 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
*/
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
- GPIO_InitStruct.Pull = GPIO_NOPULL;
- GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+ GPIO_InitStruct.Pull = GPIO_PULLUP;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM2;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
@@ -230,8 +230,8 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
*/
GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_14;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
- GPIO_InitStruct.Pull = GPIO_NOPULL;
- GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+ GPIO_InitStruct.Pull = GPIO_PULLUP;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_TIM21;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);