mirror of
https://github.com/modelec/MarcelMoteurSTM32.git
synced 2026-01-19 00:57:26 +01:00
97 lines
3.2 KiB
C++
97 lines
3.2 KiB
C++
/*
|
|
* encoder.cpp
|
|
*
|
|
* Created on: Mar 8, 2025
|
|
* Author: allan
|
|
*/
|
|
|
|
#include <encoder.h>
|
|
#include <cmath>
|
|
|
|
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) {
|
|
this->tickDistance = (uint16_t)(wheelDiameter * 3.14 / encoderResolution);
|
|
}
|
|
|
|
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
|
|
EncoderDirection direction = EncoderDirectionUndefined;
|
|
EncoderInputState pinAstate = EncoderInputUndefined;
|
|
EncoderInputState pinBstate = EncoderInputUndefined;
|
|
if(*(this->inputAgpioIDRaddr) & inputAgpioIDRMask == inputAgpioIDRMask){
|
|
pinAstate = EncoderInputHigh;
|
|
}
|
|
if(*(this->inputAgpioIDRaddr) & inputAgpioIDRMask == 0){
|
|
pinAstate = EncoderInputLow;
|
|
}
|
|
if(*(this->inputBgpioIDRaddr) & inputBgpioIDRMask == inputBgpioIDRMask){
|
|
pinBstate = EncoderInputHigh;
|
|
}
|
|
if(*(this->inputBgpioIDRaddr) & inputBgpioIDRMask == 0){
|
|
pinBstate = EncoderInputLow;
|
|
}
|
|
if(pinTriggered == 0){ // Interrupt venant du fil A
|
|
if(pinAstate == EncoderInputHigh){
|
|
if(pinBstate == EncoderInputHigh){
|
|
direction = EncoderDirectionBackward;
|
|
} else if(pinBstate == EncoderInputLow){
|
|
direction = EncoderDirectionForward;
|
|
}
|
|
} else if(pinAstate == EncoderInputLow) {
|
|
if(pinBstate == EncoderInputHigh){
|
|
direction = EncoderDirectionForward;
|
|
} else if(pinBstate == EncoderInputLow) {
|
|
direction = EncoderDirectionBackward;
|
|
}
|
|
}
|
|
} else if(pinTriggered == 1){ //Interrupt venant du fil B
|
|
if(pinAstate == EncoderInputHigh){
|
|
if(pinBstate == EncoderInputHigh){
|
|
direction = EncoderDirectionForward;
|
|
} else if(pinBstate == EncoderInputLow){
|
|
direction = EncoderDirectionBackward;
|
|
}
|
|
} else if(pinAstate == EncoderInputLow) {
|
|
if(pinBstate == EncoderInputHigh){
|
|
direction = EncoderDirectionBackward;
|
|
} else if(pinBstate == EncoderInputLow) {
|
|
direction = EncoderDirectionForward;
|
|
}
|
|
}
|
|
} else {
|
|
return; // error
|
|
}
|
|
if(direction == EncoderDirectionForward){
|
|
// On ajoute la distance parcourue et on appelle le handler
|
|
this->totalDistance += (int32_t)this->tickDistance;
|
|
this->handler(this->totalDistance, this->tickDiameter, 1);
|
|
} else if(direction == EncoderDirectionBackward){
|
|
// On retire la distance parcourue et on appelle le handler
|
|
this->totalDistance -= (int32_t)this->tickDistance;
|
|
this->handler(this->totalDistance, this->tickDiameter, 0);
|
|
} else {
|
|
return; //error
|
|
}
|
|
|
|
}
|
|
|
|
uint32_t Encoder::getTotalDistance(){
|
|
return this->totalDistance;
|
|
}
|
|
|
|
uint32_t Encoder::getDistanceSinceLastCall(){
|
|
uint32_t result = this->totalDistance - this->latestHandledDistance;
|
|
this->latestHandledDistance = this->totalDistance;
|
|
return result;
|
|
}
|
|
|
|
void Encoder::resetToZero(){
|
|
this->totalDistance = 0;
|
|
this->latestHandledDistance = 0;
|
|
}
|
|
|
|
Encoder::~Encoder() {
|
|
// TODO Auto-generated destructor stub
|
|
}
|
|
|
|
|