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

Factored Stepgen2, StepGen3 is a copy of Stepgen2

Browse Source
This commit is contained in:
Hakan Bastedt
2024-03-21 16:45:59 +01:00
parent fa34d81e41
commit 23fe81afbe
4 changed files with 210 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
Firmware/include/StepGen3.h Executable file
View File

@@ -0,0 +1,53 @@
#ifndef STEPGEN3
#define STEPGEN3
#include <HardwareTimer.h>
class StepGen3
{
public:
volatile double_t actualPosition;
volatile int32_t nSteps;
volatile uint32_t timerFrequency;
volatile int32_t timerPosition = 0;
volatile int32_t timerEndPosition = 0;
public:
volatile float Tstartf; // Starting delay in secs
volatile uint32_t Tstartu; // Starting delay in usecs
volatile float Tpulses; // Time it takes to do pulses. Debug
HardwareTimer *pulseTimer;
uint32_t pulseTimerChan;
HardwareTimer *startTimer; // Use timers 10,11,13,14
uint8_t dirPin;
PinName dirPinName;
PinName stepPin;
uint32_t Tjitter = 400; // Longest time from IRQ to handling in handleStepper, unit is microseconds
uint64_t dbg;
const uint16_t t2 = 5; // DIR is ahead of PUL with at least 5 usecs
const uint16_t t3 = 5; // Pulse width at least 2.5 usecs
const uint16_t t4 = 5; // Low level width not less than 2.5 usecs
const float maxAllowedFrequency = 1000000 / float(t3 + t4) * 0.9; // 150 kHz for now
public:
volatile double_t commandedPosition; // End position when this cycle is completed
volatile int32_t commandedStepPosition; // End step position when this cycle is completed
volatile double_t initialPosition; // From previous cycle
volatile int32_t initialStepPosition; // From previous cycle
int16_t stepsPerMM; // This many steps per mm
volatile uint8_t enabled; // Enabled step generator
volatile float frequency;
static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds
volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often)
StepGen3(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
uint32_t handleStepper(uint64_t irqTime /* time when irq happened nanosecs */, uint16_t nLoops);
void startTimerCB();
void pulseTimerCB();
uint32_t updatePos(uint32_t i);
};
#endif

14
Firmware/include/extend32to64.h Executable file
View File

@@ -0,0 +1,14 @@
#ifndef EXTEND32TO64
#define EXTEND32TO54
#include <Arduino.h>
class extend32to64
{
public:
int64_t previousTimeValue = 0;
const uint64_t ONE_PERIOD = 4294967296; // almost UINT32_MAX;
const uint64_t HALF_PERIOD = 2147483648; // Half of that
int64_t extendTime(uint32_t in);
};
#endif

125
Firmware/src/StepGen3.cpp Executable file
View File

@@ -0,0 +1,125 @@
#include <Arduino.h>
#include <stdio.h>
#include "StepGen3.h"
#include "extend32to64.h"
extern "C"
{
#include "esc.h"
}
extern extend32to64 longTime;
StepGen3::StepGen3(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
{
actualPosition = 0;
commandedPosition = 0;
commandedStepPosition = 0;
initialPosition = 0;
initialStepPosition = 0;
stepsPerMM = 0;
enabled = 0;
dirPin = _dirPin;
dirPinName = digitalPinToPinName(dirPin);
stepPin = _stepPin;
pulseTimerChan = _timerChannel;
pulseTimer = new HardwareTimer(Timer);
pulseTimer->attachInterrupt(pulseTimerChan, irq); // Capture/compare innterrupt
pinMode(dirPin, OUTPUT);
startTimer = new HardwareTimer(Timer2);
startTimer->attachInterrupt(irq2);
}
uint32_t StepGen3::handleStepper(uint64_t irqTime, uint16_t nLoops)
{
frequency = 0;
nSteps = 0;
dbg = 0;
if (!enabled) // Just .... don't
return updatePos(0);
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
nSteps = commandedStepPosition - initialStepPosition;
if (nSteps == 0) // No movement
{
return updatePos(1);
}
lcncCycleTime = nLoops * StepGen3::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
if (abs(nSteps) < 0) // Some small number
{ //
frequency = (abs(nSteps) + 1) / lcncCycleTime; // Distribute steps inside available time
Tpulses = abs(nSteps) / frequency; //
Tstartf = (lcncCycleTime - Tpulses) / 2.0; //
} //
else // Regular step train, up or down
{ //
float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation. position = kTRAJ x time + mTRAJ
float mTRAJ = initialPosition; // Operating on incoming positions (not steps)
if (kTRAJ > 0) //
Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing upwards
else //
Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing downwards
frequency = fabs(kTRAJ * stepsPerMM); //
Tpulses = abs(nSteps) / frequency;
}
updatePos(5);
uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs)
dbg = timeSinceISR; //
Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq.
if (nSteps == 0) // Can do this much earlier, but want some calculated data for debugging
return updatePos(1);
timerFrequency = uint32_t(ceil(frequency));
startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
startTimer->refresh();
startTimer->resume();
return 1;
}
void StepGen3::startTimerCB()
{
startTimer->pause(); // Once is enough.
digitalWriteFast(dirPinName, nSteps < 0 ? HIGH : LOW); // nSteps negative => decrease, HIGH
// There will be a short break here for t2 usecs, in the future.
timerEndPosition += nSteps;
pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
// pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
pulseTimer->refresh();
pulseTimer->resume();
}
void StepGen3::pulseTimerCB()
{
int16_t dir = digitalReadFast(dirPinName); //
if (dir == HIGH) // The step just taken
timerPosition--;
else
timerPosition++;
int32_t diffPosition = timerEndPosition - timerPosition; // Same "polarity" as nSteps
if (diffPosition == 0)
pulseTimer->pause();
else
{
if (diffPosition < 0 && dir == LOW) // Change direction. Should not end up here, but alas
digitalWriteFast(dirPinName, HIGH); // Normal is to be HIGH when decreasing
if (diffPosition > 0 && dir == HIGH) // Change direction. Should not end up here, but alas
digitalWriteFast(dirPinName, LOW); // Normal is to be LOW when increasing
// Normally nothing is needed
}
}
uint32_t StepGen3::updatePos(uint32_t i)
{ //
initialPosition = commandedPosition; // Save the numeric position for next step
initialStepPosition = commandedStepPosition; // also the step we are at}
return i;
}
uint32_t StepGen3::sync0CycleTime = 0;

18
Firmware/src/extend32to64.cpp Executable file
View File

@@ -0,0 +1,18 @@
#include "extend32to64.h"
// Extend from 32-bit to 64-bit precision
int64_t extend32to64::extendTime(uint32_t in)
{
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
// 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 < int64_t(-HALF_PERIOD))
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int64_t unwrapped = previousTimeValue + mod_dif;
previousTimeValue = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}