jeka/MyOwnEtherCATDevice
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

MyEncoder split into cpp and h file

Browse Source
This commit is contained in:
Hakan Bastedt
2024-01-10 22:05:38 +01:00
parent 2e4e768d5e
commit 3bbf089d01
2 changed files with 90 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
Firmware/include/MyEncoder.h
View File

@@ -23,86 +23,14 @@ public:
double curPos; double curPos;
public: public:
MyEncoder(uint8_t _indexPin, void irq(void)) MyEncoder(uint8_t _indexPin, void irq(void));
{ int32_t unwrapEncoder(uint16_t in);
indexPin = _indexPin; void indexPulse(void);
attachInterrupt(digitalPinToInterrupt(_indexPin), irq, RISING); // When Index triggered void init(enum EncTimer timer);
} uint8_t indexHappened();
double currentPos();
#define ONE_PERIOD 65536 double frequency(uint64_t time);
#define HALF_PERIOD 32768 uint8_t getIndexState();
int32_t unwrap_encoder(uint16_t in)
{
int32_t c32 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int32_t dif = (c32 - previousEncoderCounterValue); // core concept: prev + (current - prev) = current
// wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
int32_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
if (dif < -HALF_PERIOD)
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int32_t unwrapped = previousEncoderCounterValue + mod_dif;
previousEncoderCounterValue = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}
void indexPulse(void)
{
if (pleaseZeroTheCounter)
{
TIM2->CNT = 0;
indexPulseFired = 1;
Pos.clear();
TDelta.clear();
pleaseZeroTheCounter = 0;
}
}
void init(enum EncTimer timer)
{
// Set starting count value
EncoderInit.SetCount(timer, 0);
// EncoderInit.SetCount(Tim3, 0);
// EncoderInit.SetCount(Tim4, 0);
// EncoderInit.SetCount(Tim8, 0);
}
uint8_t indexHappened()
{
if (indexPulseFired)
{
indexPulseFired = 0;
previousEncoderCounterValue = 0;
return 1;
}
return 0;
}
double currentPos()
{
curPos = unwrap_encoder(TIM2->CNT) * PosScaleRes;
return curPos;
}
double frequency(uint64_t time)
{
double diffT = 0;
double diffPos = 0;
TDelta.push(time); // Running average over the length of the circular buffer
Pos.push(curPos);
if (Pos.size() >= 2)
{
diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
diffPos = fabs(Pos.last() - Pos.first());
}
return diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
}
uint8_t getIndexState()
{
return digitalRead(indexPin);
}
}; };
#endif #endif

82
Firmware/src/MyEncoder.cpp Executable file
View File

@@ -0,0 +1,82 @@
#include "MyENcoder.h"
MyEncoder::MyEncoder(uint8_t _indexPin, void irq(void))
{
indexPin = _indexPin;
attachInterrupt(digitalPinToInterrupt(indexPin), irq, RISING); // When Index triggered
}
#define ONE_PERIOD 65536
#define HALF_PERIOD 32768
int32_t MyEncoder::unwrapEncoder(uint16_t in)
{
int32_t c32 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int32_t dif = (c32 - previousEncoderCounterValue); // core concept: prev + (current - prev) = current
// wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
int32_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
if (dif < -HALF_PERIOD)
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int32_t unwrapped = previousEncoderCounterValue + mod_dif;
previousEncoderCounterValue = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}
void MyEncoder::indexPulse(void)
{
if (pleaseZeroTheCounter)
{
TIM2->CNT = 0;
indexPulseFired = 1;
Pos.clear();
TDelta.clear();
pleaseZeroTheCounter = 0;
}
}
void MyEncoder::init(enum EncTimer timer)
{
// Set starting count value
EncoderInit.SetCount(timer, 0);
// EncoderInit.SetCount(Tim3, 0);
// EncoderInit.SetCount(Tim4, 0);
// EncoderInit.SetCount(Tim8, 0);
}
uint8_t MyEncoder::indexHappened()
{
if (indexPulseFired)
{
indexPulseFired = 0;
previousEncoderCounterValue = 0;
return 1;
}
return 0;
}
double MyEncoder::currentPos()
{
curPos = unwrapEncoder(TIM2->CNT) * PosScaleRes;
return curPos;
}
double MyEncoder::frequency(uint64_t time)
{
double diffT = 0;
double diffPos = 0;
TDelta.push(time); // Running average over the length of the circular buffer
Pos.push(curPos);
if (Pos.size() >= 2)
{
diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
diffPos = fabs(Pos.last() - Pos.first());
}
return diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
}
uint8_t MyEncoder::getIndexState()
{
return digitalRead(indexPin);
}