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

a cycle's pwm train maight have been too long and run into the start of next cycle's pwm train. That's gone now and it seems to work.

Browse Source 
A more brilliant solution is needed for this.
This commit is contained in:
Hakan Bastedt
2024-02-11 19:55:40 +01:00
parent c04ac0e74b
commit f4a15afa8a
4 changed files with 46 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
Firmware/doc/Stepgen.odp
View File

Binary file not shown.

5
Firmware/include/StepGen2.h
View File

@@ -21,7 +21,8 @@ public:
HardwareTimer *startTimer; // 10,11,13,14 HardwareTimer *startTimer; // 10,11,13,14
uint8_t dirPin; uint8_t dirPin;
PinName stepPin; PinName stepPin;
const float Tjitter = 5.0; // Time unit is microseconds const float Tjitter = 500.0; // Time unit is microseconds
uint64_t dbg;
public: public:
volatile double_t commandedPosition; // End position when this cycle is completed volatile double_t commandedPosition; // End position when this cycle is completed
@@ -37,7 +38,7 @@ public:
StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void)); StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
uint32_t handleStepper(void); uint32_t handleStepper(uint64_t irqTime/* time for irq nanosecs */);
void startTimerCB(); void startTimerCB();
void pulseTimerCB(); void pulseTimerCB();
uint32_t updatePos(uint32_t i); uint32_t updatePos(uint32_t i);

41
Firmware/src/StepGen2.cpp
View File

@@ -1,6 +1,11 @@
#include <Arduino.h> #include <Arduino.h>
#include <stdio.h> #include <stdio.h>
#include "StepGen2.h" #include "StepGen2.h"
extern "C"
{
#include "esc.h"
}
extern int64_t extendTime(uint32_t);
StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void)) StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
{ {
@@ -21,10 +26,10 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
startTimer = new HardwareTimer(Timer2); startTimer = new HardwareTimer(Timer2);
startTimer->attachInterrupt(irq2); startTimer->attachInterrupt(irq2);
} }
uint32_t cnt = 0; extern volatile uint32_t cnt;
uint32_t StepGen2::handleStepper(void) uint32_t StepGen2::handleStepper(uint64_t irqTime)
{ {
digitalWrite(dirPin, cnt++ % 2);
if (!enabled) if (!enabled)
return updatePos(0); return updatePos(0);
@@ -32,39 +37,44 @@ uint32_t StepGen2::handleStepper(void)
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
if (initialStepPosition == commandedStepPosition) // No movement if (initialStepPosition == commandedStepPosition) // No movement
return 1; return 1;
// digitalWrite(dirPin, cnt++ % 2);
float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step
/ lcncCycleTime; // from here on / lcncCycleTime; // from here on
// if (approximateFrequency > maxAllowedFrequency) // Stay on this position // if (approximateFrequency > maxAllowedFrequency) // Stay on this position
// return 1; // return 1;
float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation
float mTRAJ = initialPosition; // position = kTRAJ x time + mTRAJ float mTRAJ = initialPosition; // position = kTRAJ x time + mTRAJ
// Operating on incoming positions (not steps) // Operating on incoming positions (not steps)
if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01) // Very flat slope // if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01) // Very flat slope
nSteps = commandedStepPosition - initialStepPosition; //
if (abs(nSteps) <= 8) // Some small number
{ // { //
Tstartf = 0.5 * lcncCycleTime; // Just take a step in the middle of the cycle Tstartf = 0; // Just take a step in the middle of the cycle
frequency = 10000; // At some suitable frequency frequency = 1000 * (abs(nSteps) + 1); // At some suitable frequency
nSteps = kTRAJ > 0 ? 1 : -1; // Take only one step
} }
else // Regular step train, up or down else // Regular step train, up or down
{ {
if (kTRAJ > 0) if (kTRAJ > 0)
Tstartf = (ceil(initialPosition * stepsPerMM) / stepsPerMM - mTRAJ) / kTRAJ; Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ;
else else
Tstartf = (floor(initialPosition * stepsPerMM) / stepsPerMM - mTRAJ) / kTRAJ; Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ;
frequency = fabs(kTRAJ * stepsPerMM); frequency = fabs(kTRAJ * stepsPerMM);
nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction. nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction.
} }
updatePos(5); updatePos(5);
Tstartu = Tstartf * 1e6; // Was secs, now usecs uint64_t nowTime = extendTime(micros()); // usecs
dbg = nowTime - irqTime;
Tstartu = Tjitter + Tstartf * 1e6 // Was secs, now usecs
- (nowTime - irqTime); // Have already wasted some time since the irq.
startTimer->setOverflow(Tstartu + Tjitter, MICROSEC_FORMAT); // All handled by irqs startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
startTimer->resume(); startTimer->resume();
return 1; return 1;
} }
void StepGen2::startTimerCB() void StepGen2::startTimerCB()
{ {
digitalWrite(dirPin, cnt++ % 2);
startTimer->pause(); // Once is enough. startTimer->pause(); // Once is enough.
// digitalWrite(dirPin, nSteps > 0 ? 1 : -1); // digitalWrite(dirPin, nSteps > 0 ? 1 : -1);
timerPulseSteps = abs(nSteps); timerPulseSteps = abs(nSteps);
@@ -77,7 +87,10 @@ void StepGen2::pulseTimerCB()
{ {
--timerPulseSteps; --timerPulseSteps;
if (timerPulseSteps == 0) if (timerPulseSteps == 0)
{
pulseTimer->pause(); pulseTimer->pause();
digitalWrite(dirPin, cnt++ % 2);
}
} }
uint32_t StepGen2::updatePos(uint32_t i) uint32_t StepGen2::updatePos(uint32_t i)

27
Firmware/src/main.cpp
View File

@@ -26,6 +26,7 @@ void pulseTimerCallback(void) { Step.pulseTimerCB(); }
void startTimerCallback(void) { Step.startTimerCB(); } void startTimerCallback(void) { Step.startTimerCB(); }
CircularBuffer<uint32_t, 200> Tim; CircularBuffer<uint32_t, 200> Tim;
volatile uint64_t irqTime = 0, thenTime = 0; volatile uint64_t irqTime = 0, thenTime = 0;
volatile uint32_t ccnnt = 0;
int64_t extendTime(uint32_t in); // Extend from 32-bit to 64-bit precision int64_t extendTime(uint32_t in); // Extend from 32-bit to 64-bit precision
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
@@ -42,7 +43,7 @@ void handleStepper(void)
Obj.StepGenOut1.ActualPosition = Step.commandedPosition; Obj.StepGenOut1.ActualPosition = Step.commandedPosition;
Step.stepsPerMM = Obj.StepGenIn1.StepsPerMM; Step.stepsPerMM = Obj.StepGenIn1.StepsPerMM;
Step.stepsPerMM = 4000; Step.stepsPerMM = 4000;
Step.handleStepper(); Step.handleStepper(irqTime);
Obj.StepGenOut2.ActualPosition = Obj.StepGenIn2.CommandedPosition; Obj.StepGenOut2.ActualPosition = Obj.StepGenIn2.CommandedPosition;
} }
@@ -54,7 +55,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
Obj.IndexByte = Encoder1.getIndexState(); Obj.IndexByte = Encoder1.getIndexState();
uint32_t dTim = irqTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds uint32_t dTim = extendTime(micros()) - irqTime; // thenTime; // Debug. Getting jitter over the last 200 milliseconds
Tim.push(dTim); Tim.push(dTim);
uint32_t max_Tim = 0, min_Tim = UINT32_MAX; uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++) for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++)
@@ -67,8 +68,8 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
} }
thenTime = irqTime; thenTime = irqTime;
Obj.DiffT = max_Tim - min_Tim; // Debug Obj.DiffT = max_Tim - min_Tim; // Debug
Obj.DiffT = ALEventIRQ; Obj.DiffT = ccnnt--;
//Obj.DiffT = Step.frequency; // Obj.DiffT = Step.frequency;
} }
void ESC_interrupt_enable(uint32_t mask); void ESC_interrupt_enable(uint32_t mask);
@@ -112,7 +113,7 @@ void loop(void)
{ {
CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread()); CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
serveIRQ = 0; serveIRQ = 0;
ESCvar.PrevTime = ESCvar.Time; ESCvar.PrevTime = ESCvar.Time;
} }
@@ -120,14 +121,17 @@ void loop(void)
if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt
ecat_slv_poll(); ecat_slv_poll();
} }
volatile uint32_t cnt = 0;
void sync0Handler(void) void sync0Handler(void)
{ {
irqTime = micros(); ccnnt++;
serveIRQ = 1;
ALEventIRQ = ESC_ALeventread(); ALEventIRQ = ESC_ALeventread();
serveIRQ = 1;
irqTime = extendTime(micros());
digitalWrite(Step.dirPin, cnt++ % 2);
} }
// Enable SM2 interrupts
void ESC_interrupt_enable(uint32_t mask) void ESC_interrupt_enable(uint32_t mask)
{ {
// Enable interrupt for SYNC0 or SM2 or SM3 // Enable interrupt for SYNC0 or SM2 or SM3
@@ -135,9 +139,8 @@ void ESC_interrupt_enable(uint32_t mask)
uint32_t user_int_mask = ESCREG_ALEVENT_SM2; // Only SM2 uint32_t user_int_mask = ESCREG_ALEVENT_SM2; // Only SM2
if (mask & user_int_mask) if (mask & user_int_mask)
{ {
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3));
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask));
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3));
attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING); attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING);
// Set LAN9252 interrupt pin driver as push-pull active high // Set LAN9252 interrupt pin driver as push-pull active high
@@ -150,6 +153,7 @@ void ESC_interrupt_enable(uint32_t mask)
} }
} }
// Disable SM2 interrupts
void ESC_interrupt_disable(uint32_t mask) void ESC_interrupt_disable(uint32_t mask)
{ {
// Enable interrupt for SYNC0 or SM2 or SM3 // Enable interrupt for SYNC0 or SM2 or SM3
@@ -182,7 +186,8 @@ uint16_t dc_checker(void)
#define HALF_PERIOD (UINT32_MAX >> 1) #define HALF_PERIOD (UINT32_MAX >> 1)
static int64_t previousTimeValue = 0; static int64_t previousTimeValue = 0;
int64_t extendTime(uint32_t in) // Extend from 32-bit to 64-bit precision // Extend from 32-bit to 64-bit precision
int64_t extendTime(uint32_t in)
{ {
int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current