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

Raw classified encoder. Compiles, untested

Browse Source
This commit is contained in:
Hakan Bastedt
2024-01-10 21:58:11 +01:00
parent 6c168d96d9
commit 2e4e768d5e
2 changed files with 129 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
Firmware/include/MyEncoder.h Executable file
View File

@@ -0,0 +1,108 @@
#ifndef MYENCODER
#define MYENCODER
#include "Stm32F4_Encoder.h"
#include <CircularBuffer.h>
#define RINGBUFFERLEN 101
// EncoderInit.SetCount(Tim3, 0);
// EncoderInit.SetCount(Tim4, 0);
// EncoderInit.SetCount(Tim8, 0);
class MyEncoder
{
public:
int32_t previousEncoderCounterValue = 0;
double PosScaleRes = 1.0;
uint32_t CurPosScale = 1;
uint8_t OldLatchCEnable = 0;
volatile uint8_t indexPulseFired = 0;
volatile uint8_t pleaseZeroTheCounter = 0;
Encoder EncoderInit;
uint8_t indexPin;
CircularBuffer<double_t, RINGBUFFERLEN> Pos;
CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
double curPos;
public:
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 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

111
Firmware/src/main.cpp
View File

@@ -6,31 +6,19 @@ extern "C"
#include "ecat_slv.h"
#include "utypes.h"
};
#include <CircularBuffer.h>
#define RINGBUFFERLEN 101
CircularBuffer<double_t, RINGBUFFERLEN> Pos;
CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
#include <Stm32F4_Encoder.h>
int64_t PreviousEncoderCounterValue = 0;
int64_t unwrap_encoder(uint16_t in, int64_t *prev);
Encoder EncoderInit;
Encoder *encP = &EncoderInit;
#define INDEX_PIN PA2
HardwareSerial Serial1(PA10, PA9);
_Objects Obj;
void indexPulse(void);
double PosScaleRes = 1.0;
uint32_t CurPosScale = 1;
uint8_t OldLatchCEnable = 0;
volatile uint8_t indexPulseFired = 0;
volatile uint8_t pleaseZeroTheCounter = 0;
#define STEPPER_DIR_PIN PA12
#define STEPPER_STEP_PIN PA11
HardwareTimer *MyTim;
#include "MyEncoder.h"
#define INDEX_PIN PA2
void indexPulseEncoderCB1(void);
MyEncoder Encoder1(INDEX_PIN, indexPulseEncoderCB1);
void indexPulseEncoderCB1(void)
{
Encoder1.indexPulse();
}
#include "StepGen.h"
void timerCallbackStep1(void);
@@ -40,56 +28,32 @@ void timerCallbackStep1(void)
Step1.timerCB();
}
uint32_t sync0CycleTime = 0; // microseconds
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
{
if (Obj.IndexLatchEnable && !OldLatchCEnable) // Should only happen first time IndexCEnable is set
if (Obj.IndexLatchEnable && !Encoder1.OldLatchCEnable) // Should only happen first time IndexCEnable is set
{
pleaseZeroTheCounter = 1;
Encoder1.pleaseZeroTheCounter = 1;
}
OldLatchCEnable = Obj.IndexLatchEnable;
Encoder1.OldLatchCEnable = Obj.IndexLatchEnable;
if (CurPosScale != Obj.EncPosScale && Obj.EncPosScale != 0)
if (Encoder1.CurPosScale != Obj.EncPosScale && Obj.EncPosScale != 0)
{
CurPosScale = Obj.EncPosScale;
PosScaleRes = 1.0 / double(CurPosScale);
Encoder1.CurPosScale = Obj.EncPosScale;
Encoder1.PosScaleRes = 1.0 / double(Encoder1.CurPosScale);
}
Step1.cmdPos(Obj.StepGenIn1.CommandedPosition);
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{
Obj.IndexStatus = 0;
if (indexPulseFired)
{
Obj.IndexStatus = 1;
indexPulseFired = 0;
PreviousEncoderCounterValue = 0;
}
// Obj.DiffT = sync0CycleTime;
int64_t pos = unwrap_encoder(TIM2->CNT, &PreviousEncoderCounterValue);
double CurPos = pos * PosScaleRes;
Obj.EncPos = CurPos;
double diffT = 0;
double diffPos = 0;
TDelta.push(ESCvar.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());
}
Obj.EncFrequency = diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
Obj.IndexByte = digitalRead(INDEX_PIN);
Obj.IndexStatus = Encoder1.indexHappened();
Obj.EncPos = Encoder1.currentPos();
Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
Obj.IndexByte = Encoder1.getIndexState();
if (Obj.IndexByte)
Serial1.printf("IS 1\n");
Obj.StepGenOut1.ActualPosition = Step1.actPos();
Obj.DiffT = 10000 * Step1.reqPos(); // deltaT;
Obj.DiffT = 10000 * Step1.reqPos();
}
void ESC_interrupt_enable(uint32_t mask);
@@ -127,13 +91,8 @@ void setup(void)
rcc_config();
Step1.setScale(500);
Encoder1.init(Tim2);
// Set starting count value
EncoderInit.SetCount(Tim2, 0);
attachInterrupt(digitalPinToInterrupt(INDEX_PIN), indexPulse, RISING); // When Index triggered
// EncoderInit.SetCount(Tim3, 0);
// EncoderInit.SetCount(Tim4, 0);
// EncoderInit.SetCount(Tim8, 0);
ecat_slv_init(&config);
}
@@ -150,18 +109,6 @@ void loop(void)
ecat_slv_poll();
}
void indexPulse(void)
{
if (pleaseZeroTheCounter)
{
TIM2->CNT = 0;
indexPulseFired = 1;
Pos.clear();
TDelta.clear();
pleaseZeroTheCounter = 0;
}
}
void sync0Handler(void)
{
serveIRQ = 1;
@@ -220,21 +167,3 @@ uint16_t dc_checker(void)
Step1.setCycleTime(ESC_SYNC0cycletime() / 1000); // nsec to usec
return 0;
}
#define ONE_PERIOD 65536
#define HALF_PERIOD 32768
int64_t unwrap_encoder(uint16_t in, int64_t *prev)
{
int64_t c64 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int64_t dif = (c64 - *prev); // 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
int64_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
int64_t unwrapped = *prev + mod_dif;
*prev = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}