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

wip

Browse Source
This commit is contained in:
Hakan Bastedt
2024-03-10 01:03:50 +01:00
parent 31be067dea
commit fef934b103
3 changed files with 39 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Firmware/include/StepGen2.h
View File

@@ -18,10 +18,10 @@ public:
HardwareTimer *pulseTimer;
uint32_t pulseTimerChan;
HardwareTimer *startTimer; // 10,11,13,14
HardwareTimer *startTimer; // Use timers 10,11,13,14
uint8_t dirPin;
PinName stepPin;
const uint32_t Tjitter = 500; // Time unit is microseconds
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 = 3; // Pulse width at least 2.5 usecs
@@ -37,12 +37,12 @@ public:
volatile uint8_t enabled; // Enabled step generator
volatile float frequency;
static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time
volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often)
static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds
volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often)
StepGen2(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 for irq nanosecs */);
uint32_t handleStepper(uint64_t irqTime /* time when irq happened nanosecs */, uint16_t nLoops);
void startTimerCB();
void pulseTimerCB();
uint32_t updatePos(uint32_t i);

17
Firmware/src/StepGen2.cpp
View File

@@ -26,20 +26,20 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
startTimer = new HardwareTimer(Timer2);
startTimer->attachInterrupt(irq2);
}
extern volatile uint32_t cnt;
uint32_t StepGen2::handleStepper(uint64_t irqTime)
uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
{
if (!enabled)
return updatePos(0);
lcncCycleTime = StepGen2::sync0CycleTime * 1.0e-6; // was usec, became sec
lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // // nLoops is there in case we missed a ethercat cycle. secs
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
if (initialStepPosition == commandedStepPosition) // No movement
return updatePos(1);
nSteps = commandedStepPosition - initialStepPosition;
if (abs(nSteps) < 2) // Some small number
if (abs(nSteps) < 1) // Some small number
{
frequency = (abs(nSteps) + 1) / lcncCycleTime;
Tpulses = abs(nSteps) / frequency;
@@ -58,14 +58,15 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime)
}
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.
dbg = timeSinceISR; //
Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq.
timerFrequency = uint32_t(ceil(frequency));
startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
startTimer->resume();
return 1;
}
void StepGen2::startTimerCB()
{
startTimer->pause(); // Once is enough.
@@ -74,9 +75,11 @@ void StepGen2::startTimerCB()
timerPulseSteps = abs(nSteps);
pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
pulseTimer->setCaptureCompare(pulseTimerChan, 5, MICROSEC_COMPARE_FORMAT); // 5 usecs
// pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
pulseTimer->resume();
}
void StepGen2::pulseTimerCB()
{
--timerPulseSteps;

35
Firmware/src/main.cpp
View File

@@ -19,14 +19,14 @@ void indexPulseEncoderCB1(void)
}
#include "StepGen2.h"
//Stepper 1
// Stepper 1
void pulseTimerCallback1(void);
void startTimerCallback1(void);
StepGen2 Step1(TIM1, 4, PA_11, PA12, pulseTimerCallback1, TIM10, startTimerCallback1);
void pulseTimerCallback1(void) { Step1.pulseTimerCB(); }
void startTimerCallback1(void) { Step1.startTimerCB(); }
//Stepper 2
// Stepper 2
void pulseTimerCallback2(void);
void startTimerCallback2(void);
StepGen2 Step2(TIM3, 4, PC_9, PC10, pulseTimerCallback2, TIM11, startTimerCallback2);
@@ -35,6 +35,8 @@ void startTimerCallback2(void) { Step2.startTimerCB(); }
CircularBuffer<uint32_t, 200> Tim;
volatile uint64_t irqTime = 0, thenTime = 0, nowTime = 0;
volatile uint64_t EcatTimeIRQ = 0, EcatTimeThen = 0, EcatTimeDiff = 0;
;
volatile uint32_t ccnnt = 0;
extend32to64 longTime;
@@ -49,19 +51,29 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos();
Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos();
}
volatile uint32_t cmt;
uint16_t nLoops;
void handleStepper(void)
{
//digitalWrite(Step.dirPin, cmt++ % 2);
if (!(ALEventIRQ & ESCREG_ALEVENT_SM2))
return;
// Catch the case when we miss a loop for some reason
uint64_t EcatTimeNow;
ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeNow, sizeof(EcatTimeNow));
EcatTimeNow = etohl(EcatTimeNow);
EcatTimeDiff = EcatTimeNow - EcatTimeThen;
nLoops = round(EcatTimeDiff / double(StepGen2::sync0CycleTime));
EcatTimeThen = EcatTimeNow;
Step1.enabled = true;
Step1.commandedPosition = Obj.CommandedPosition1;
Step1.stepsPerMM = Obj.StepsPerMM1;
Step1.handleStepper(irqTime);
Step1.handleStepper(irqTime, nLoops);
Step2.enabled = true;
Step2.commandedPosition = Obj.CommandedPosition2;
Step2.stepsPerMM = Obj.StepsPerMM2;
Step2.handleStepper(irqTime);
Step2.handleStepper(irqTime, nLoops);
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -84,10 +96,9 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
}
thenTime = irqTime;
Obj.DiffT = max_Tim - min_Tim; // Debug
Obj.DiffT = abs(Step1.nSteps);
Obj.D1 = Step1.Tjitter;
Obj.D2 = Step1.Tstartf * 1e6;
Obj.D3 = Step1.dbg;
Obj.D1 = Step2.frequency;
Obj.D2 = Step2.Tstartf * 1e6;
Obj.D3 = Step2.dbg;
Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3;
}
@@ -149,6 +160,8 @@ void sync0Handler(void)
ccnnt++;
ALEventIRQ = ESC_ALeventread();
serveIRQ = 1;
ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeIRQ, sizeof(EcatTimeIRQ));
EcatTimeIRQ = etohl(EcatTimeIRQ);
irqTime = longTime.extendTime(micros());
}
@@ -199,6 +212,6 @@ uint16_t dc_checker(void)
{
// Indicate we run DC
ESCvar.dcsync = 1;
StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
StepGen2::sync0CycleTime = ESC_SYNC0cycletime(); // nsecs
return 0;
}