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

40
Firmware/src/StepGen2.cpp
View File

@@ -29,31 +29,37 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
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);
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;
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
{
frequency = (abs(nSteps) + 1) / lcncCycleTime;
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;
else
Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ;
frequency = fabs(kTRAJ * stepsPerMM); //
if (abs(nSteps) < 1) // 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);

10
Firmware/src/main.cpp
View File

@@ -68,12 +68,12 @@ void handleStepper(void)
Step1.enabled = true;
Step1.commandedPosition = Obj.CommandedPosition1;
Step1.stepsPerMM = Obj.StepsPerMM1;
Step1.handleStepper(irqTime, nLoops);
Step1.handleStepper(irqTime, 1/*nLoops*/);
Step2.enabled = true;
Step2.commandedPosition = Obj.CommandedPosition2;
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
@@ -95,10 +95,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
min_Tim = aTim;
}
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.D2 = Step2.Tstartf * 1e6;
Obj.D3 = Step2.dbg;
Obj.D2 = abs(Step2.nSteps);
Obj.D3 = Step2.Tstartu;
Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3;
}