diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 1856fbd..08a37ff 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/Core/Inc/encoder.h b/Core/Inc/encoder.h
index 4ae2ea1..080a47b 100644
--- a/Core/Inc/encoder.h
+++ b/Core/Inc/encoder.h
@@ -32,11 +32,13 @@ private:
uint32_t inputAgpioIDRMask;
volatile uint32_t* inputBgpioIDRaddr;
uint32_t inputBgpioIDRMask;
+ EncoderInputState previousAstate;
+ EncoderInputState previousBstate;
void (*handler)(uint16_t totalDistance, uint16_t newDistance, bool direction); // direction = (1 : forward, 0 : backward)
public:
Encoder(uint16_t wheelDiameter, uint16_t encoderResolution, volatile uint32_t* inputAgpioIDRaddr, volatile uint32_t* inputBgpioIDRaddr, uint32_t inputAgpioIDRMask, uint32_t inputBgpioIDRMask, void (*handler)(uint16_t, uint16_t, bool));
- void trigger(bool pinTriggered);// pin = (0 : A, 1 : B)
+ void trigger();
uint32_t getTotalDistance();
uint32_t getDistanceSinceLastCall();
void resetToZero();
diff --git a/Core/Src/encoder.cpp b/Core/Src/encoder.cpp
index 59ece3d..ab6a921 100644
--- a/Core/Src/encoder.cpp
+++ b/Core/Src/encoder.cpp
@@ -7,28 +7,56 @@
#include "encoder.h"
-Encoder::Encoder(uint16_t wheelDiameter, uint16_t encoderResolution, volatile uint32_t* inputAgpioIDRaddr, volatile uint32_t* inputBgpioIDRaddr, uint32_t inputAgpioIDRMask, uint32_t inputBgpioIDRMask, void(*handler)(uint16_t, uint16_t, bool)) : handler(handler), inputAgpioIDRaddr(inputAgpioIDRaddr), inputBgpioIDRaddr(inputBgpioIDRaddr), inputAgpioIDRMask(inputAgpioIDRMask), inputBgpioIDRMask(inputBgpioIDRMask), totalDistance(0), latestHandledDistance(0) {
+Encoder::Encoder(uint16_t wheelDiameter, uint16_t encoderResolution, volatile uint32_t* inputAgpioIDRaddr, volatile uint32_t* inputBgpioIDRaddr, uint32_t inputAgpioIDRMask, uint32_t inputBgpioIDRMask, void(*handler)(uint16_t, uint16_t, bool)) :
+ totalDistance(0),
+ latestHandledDistance(0),
+ inputAgpioIDRaddr(inputAgpioIDRaddr),
+ inputAgpioIDRMask(inputAgpioIDRMask),
+ inputBgpioIDRaddr(inputBgpioIDRaddr),
+ inputBgpioIDRMask(inputBgpioIDRMask),
+ previousAstate(EncoderInputUndefined),
+ previousBstate(EncoderInputUndefined),
+ handler(handler)
+{
this->tickDistance = (uint16_t)(wheelDiameter * 3.14 / encoderResolution);
+ if((*(this->inputAgpioIDRaddr) & inputAgpioIDRMask) == inputAgpioIDRMask){
+ this->previousAstate = EncoderInputHigh;
+ }
+ if((*(this->inputAgpioIDRaddr) & inputAgpioIDRMask) == 0){
+ this->previousAstate = EncoderInputLow;
+ }
+ if((*(this->inputBgpioIDRaddr) & inputBgpioIDRMask) == inputBgpioIDRMask){
+ this->previousBstate = EncoderInputHigh;
+ }
+ if((*(this->inputBgpioIDRaddr) & inputBgpioIDRMask) == 0){
+ this->previousBstate = EncoderInputLow;
+ }
}
-void Encoder::trigger(bool pinTriggered){
- //On sait qu'on avance d'un tick et on connait quel pin vient de changer de valeur : on cherche a determiner la direction de deplacement
+void Encoder::trigger()
+{
+ //On sait qu'on avance d'un tick : on cherche d'abord a determiner quel pin a change de valeur puis on on cherchera a determiner la direction de deplacement
EncoderDirection direction = EncoderDirectionUndefined;
EncoderInputState pinAstate = EncoderInputUndefined;
EncoderInputState pinBstate = EncoderInputUndefined;
- if(*(this->inputAgpioIDRaddr) & inputAgpioIDRMask == inputAgpioIDRMask){
+ // Est ce que le IDR et le mask sont bien transmis ? Est ce qu'on arrive à lire dans le registre IDR ?
+ if((*(this->inputAgpioIDRaddr) & inputAgpioIDRMask) == inputAgpioIDRMask){
pinAstate = EncoderInputHigh;
}
- if(*(this->inputAgpioIDRaddr) & inputAgpioIDRMask == 0){
+ if((*(this->inputAgpioIDRaddr) & inputAgpioIDRMask) == 0){
pinAstate = EncoderInputLow;
}
- if(*(this->inputBgpioIDRaddr) & inputBgpioIDRMask == inputBgpioIDRMask){
+ if((*(this->inputBgpioIDRaddr) & inputBgpioIDRMask) == inputBgpioIDRMask){
pinBstate = EncoderInputHigh;
}
- if(*(this->inputBgpioIDRaddr) & inputBgpioIDRMask == 0){
+ if((*(this->inputBgpioIDRaddr) & inputBgpioIDRMask) == 0){
pinBstate = EncoderInputLow;
}
- if(pinTriggered == 0){ // Interrupt venant du fil A
+ if(pinAstate == EncoderInputUndefined || pinBstate == EncoderInputUndefined){
+ return; // error : on a pas reussi a lire la valeur des pins !
+ }
+ // on considere que les previousXstate sont initialises par le constructeur
+ if(pinAstate != previousAstate){ // Interrupt venant du fil A => quelle direction ?
if(pinAstate == EncoderInputHigh){
if(pinBstate == EncoderInputHigh){
direction = EncoderDirectionBackward;
@@ -42,7 +70,8 @@ void Encoder::trigger(bool pinTriggered){
direction = EncoderDirectionBackward;
}
}
- } else if(pinTriggered == 1){ //Interrupt venant du fil B
+ previousAstate = pinAstate;
+ } else if(pinBstate != previousBstate){ //Interrupt venant du fil B => quelle direction ?
if(pinAstate == EncoderInputHigh){
if(pinBstate == EncoderInputHigh){
direction = EncoderDirectionForward;
@@ -56,8 +85,9 @@ void Encoder::trigger(bool pinTriggered){
direction = EncoderDirectionForward;
}
}
+ previousBstate = pinBstate;
} else {
- return; // error
+ return; // error : A et B n'ont pas change de valeur => pas de mouvement alors !
}
if(direction == EncoderDirectionForward){
// On ajoute la distance parcourue et on appelle le handler
@@ -68,27 +98,31 @@ void Encoder::trigger(bool pinTriggered){
this->totalDistance -= (int32_t)this->tickDistance;
this->handler(this->totalDistance, this->tickDistance, 0);
} else {
- return; //error
+ return; //error : on a pas l'information de direction (ne devrait jamais arriver)
}
}
-uint32_t Encoder::getTotalDistance(){
+uint32_t Encoder::getTotalDistance()
+{
return this->totalDistance;
}
-uint32_t Encoder::getDistanceSinceLastCall(){
+uint32_t Encoder::getDistanceSinceLastCall()
+{
uint32_t result = this->totalDistance - this->latestHandledDistance;
this->latestHandledDistance = this->totalDistance;
return result;
}
-void Encoder::resetToZero(){
+void Encoder::resetToZero()
+{
this->totalDistance = 0;
this->latestHandledDistance = 0;
}
-Encoder::~Encoder() {
+Encoder::~Encoder()
+{
// TODO Auto-generated destructor stub
}
diff --git a/Core/Src/main.cpp b/Core/Src/main.cpp
index 6dc1488..da9a6df 100644
--- a/Core/Src/main.cpp
+++ b/Core/Src/main.cpp
@@ -72,7 +72,8 @@ void handleEncoderProgression(uint16_t totalDistance, uint16_t newDistance, bool
}
HAL_UART_Transmit(&huart2, (uint8_t *)msg, strlen(msg), HAL_MAX_DELAY);
}
-Encoder encoder1(60000, 600, &GPIOC->IDR, &GPIOC->IDR, (1<<0), (1<<2), &handleEncoderProgression);
+Encoder encoder1(60000, 600, &GPIOC->IDR, &GPIOC->IDR, (1<<0), (1<<1), &handleEncoderProgression);
+Encoder encoder2(60000, 600, &GPIOC->IDR, &GPIOC->IDR, (1<<2), (1<<3), &handleEncoderProgression);
/* USER CODE END 0 */
@@ -340,7 +341,14 @@ static void MX_GPIO_Init(void)
}
/* USER CODE BEGIN 4 */
-
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
+{
+ if(GPIO_Pin == GPIO_PIN_0 || GPIO_Pin == GPIO_PIN_1) {
+ encoder1.trigger(); // trigger de l'encodeur 1
+ } else if (GPIO_Pin == GPIO_PIN_2 || GPIO_Pin == GPIO_PIN_3) {
+ encoder2.trigger(); // trigger de l'encodeur 2
+ }
+}
/* USER CODE END 4 */
/**
diff --git a/Core/Src/stm32l0xx_it.cpp b/Core/Src/stm32l0xx_it.cpp
index 56b6811..e9c9147 100644
--- a/Core/Src/stm32l0xx_it.cpp
+++ b/Core/Src/stm32l0xx_it.cpp
@@ -58,7 +58,7 @@
/* External variables --------------------------------------------------------*/
/* USER CODE BEGIN EV */
-extern Encoder encoder1;
+
/* USER CODE END EV */
/******************************************************************************/
@@ -147,12 +147,16 @@ void SysTick_Handler(void)
void EXTI0_1_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_1_IRQn 0 */
- encoder1.trigger(0); // trigger pin A de l'encodeur sur PC0
- /* USER CODE END EXTI0_1_IRQn 0 */
+
+ // On branche le fil A et le fil B du meme encodeur sur la meme interruption et on determine nous meme lequel a trigger l'interruption
+
+ /* USER CODE END EXTI0_1_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_1);
/* USER CODE BEGIN EXTI0_1_IRQn 1 */
+ // Est ce que le callback est appelé deux fois à la suite ??
+
/* USER CODE END EXTI0_1_IRQn 1 */
}
@@ -162,8 +166,8 @@ void EXTI0_1_IRQHandler(void)
void EXTI2_3_IRQHandler(void)
{
/* USER CODE BEGIN EXTI2_3_IRQn 0 */
- encoder1.trigger(1); // trigger pin B de l'encodeur sur PC2
- /* USER CODE END EXTI2_3_IRQn 0 */
+
+ /* USER CODE END EXTI2_3_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3);
/* USER CODE BEGIN EXTI2_3_IRQn 1 */