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 22:40:25 +01:00
parent fef934b103
commit f341eb5074
2 changed files with 28 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
Firmware/src/StepGen2.cpp
View File

@@ -29,30 +29,36 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops) uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
{ {
if (!enabled) frequency = 0;
nSteps = 0;
dbg=0;
if (!enabled) // Just .... don't
return updatePos(0); return updatePos(0);
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 commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
if (initialStepPosition == commandedStepPosition) // No movement if (initialStepPosition == commandedStepPosition) // No movement
{
return updatePos(1); return updatePos(1);
}
nSteps = commandedStepPosition - initialStepPosition; nSteps = commandedStepPosition - initialStepPosition;
lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
if (abs(nSteps) < 1) // Some small number if (abs(nSteps) < 1) // Some small number
{ { //
frequency = (abs(nSteps) + 1) / lcncCycleTime; frequency = (abs(nSteps) + 1) / lcncCycleTime; // Distribute steps inside available time
Tpulses = abs(nSteps) / frequency; Tpulses = abs(nSteps) / frequency; //
Tstartf = (lcncCycleTime - Tpulses) / 2.0; Tstartf = (lcncCycleTime - Tpulses) / 2.0; //
} } //
else // Regular step train, up or down else // Regular step train, up or down
{ { //
float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation. position = kTRAJ x time + mTRAJ float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation. position = kTRAJ x time + mTRAJ
float mTRAJ = initialPosition; // Operating on incoming positions (not steps) float mTRAJ = initialPosition; // Operating on incoming positions (not steps)
if (kTRAJ > 0) if (kTRAJ > 0) //
Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing upwards
else else //
Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ; Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing downwards
frequency = fabs(kTRAJ * stepsPerMM); // frequency = fabs(kTRAJ * stepsPerMM); //
Tpulses = abs(nSteps) / frequency; Tpulses = abs(nSteps) / frequency;
} }

10
Firmware/src/main.cpp
View File

@@ -68,12 +68,12 @@ void handleStepper(void)
Step1.enabled = true; Step1.enabled = true;
Step1.commandedPosition = Obj.CommandedPosition1; Step1.commandedPosition = Obj.CommandedPosition1;
Step1.stepsPerMM = Obj.StepsPerMM1; Step1.stepsPerMM = Obj.StepsPerMM1;
Step1.handleStepper(irqTime, nLoops); Step1.handleStepper(irqTime, 1/*nLoops*/);
Step2.enabled = true; Step2.enabled = true;
Step2.commandedPosition = Obj.CommandedPosition2; Step2.commandedPosition = Obj.CommandedPosition2;
Step2.stepsPerMM = Obj.StepsPerMM2; Step2.stepsPerMM = Obj.StepsPerMM2;
Step2.handleStepper(irqTime, nLoops); Step2.handleStepper(irqTime, 1/*nLoops*/);
} }
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -95,10 +95,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
min_Tim = aTim; min_Tim = aTim;
} }
thenTime = irqTime; thenTime = irqTime;
Obj.DiffT = max_Tim - min_Tim; // Debug Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
Obj.D1 = Step2.frequency; Obj.D1 = Step2.frequency;
Obj.D2 = Step2.Tstartf * 1e6; Obj.D2 = abs(Step2.nSteps);
Obj.D3 = Step2.dbg; Obj.D3 = Step2.Tstartu;
Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3; Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3;
} }