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

wip, before timer stuff

Browse Source
This commit is contained in:
Hakan Bastedt
2024-02-02 12:09:49 +01:00
parent d1eb1d9a40
commit 133df5662d
2 changed files with 47 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Firmware/include/StepGen2.h
View File

@@ -34,7 +34,7 @@ public:
StepGen2(TIM_TypeDef *Timer, uint32_t timerChannel, PinName stepPin, uint8_t dirPin, void irq(void)); StepGen2(TIM_TypeDef *Timer, uint32_t timerChannel, PinName stepPin, uint8_t dirPin, void irq(void));
void handleStepper(void); uint32_t handleStepper(void);
void timerCB(); void timerCB();
void enable(uint8_t yes); void enable(uint8_t yes);
@@ -48,6 +48,8 @@ public:
double actPos() { return actualPosition; }; double actPos() { return actualPosition; };
void setScale(int16_t spm) { stepsPerMM = spm; } void setScale(int16_t spm) { stepsPerMM = spm; }
int16_t getScale() { return stepsPerMM; } int16_t getScale() { return stepsPerMM; }
uint32_t updatePosAndReturn(int32_t stepPosStop, uint32_t i);
}; };
#endif #endif

91
Firmware/src/StepGen2.cpp
View File

@@ -24,57 +24,48 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
pinMode(dirPin, OUTPUT); pinMode(dirPin, OUTPUT);
} }
void StepGen2::handleStepper(void) uint32_t StepGen2::handleStepper(void)
{ {
if (!enabled) if (!enabled)
return; return 1;
lcncCycleTime = StepGen2::sync0CycleTime; lcncCycleTime = StepGen2::sync0CycleTime;
float y0TRAJ = oldPos() * getScale(); // Straight line equation between old and new point float y0TRAJ = oldPos() * getScale(); // Straight line equation between old and new point
float y1TRAJ = reqPos() * getScale(); // Time runs between 0 and lcncCycleTime (1 ms) float y1TRAJ = reqPos() * getScale(); // Time runs between 0 and lcncCycleTime (1 ms)
float kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime; // Slope float kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime; // Slope
float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime; // Intercept float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime; // Intercept
int32_t stepPosStart = floor(y0TRAJ); // First step position, integer value of first point position int32_t stepPosStart = floor(y0TRAJ); // First step position, integer value of first point position
int32_t stepPosStop = floor(y1TRAJ); // End step position int32_t stepPosStop = floor(y1TRAJ); // End step position
//
float Tstart = (stepPosStart - mTRAJ) / kTRAJ; // First step at this time float Tstart = (stepPosStart - mTRAJ) / kTRAJ; // First step at this time
float Tstop = (stepPosStop - mTRAJ) / kTRAJ; // And the last step float Tstop = (stepPosStop - mTRAJ) / kTRAJ; // And the last step
float Tstep = fabs(1.0 / kTRAJ); // Time between steps float Tstep = fabs(1.0 / kTRAJ); // Time between steps
float stepFrequency = fabs(kTRAJ); // 1/Tstep - which is kTRAJ float stepFrequency = fabs(kTRAJ); // 1/Tstep - which is kTRAJ
// //
oldPos(reqPos()); // Save the numeric position for next step if (Tstart > lcncCycleTime) // Not enough movement to make a step
oldStepPos(stepPosStop); // also the step we are at return updatePosAndReturn(stepPosStop, 2); //
// if (/* 1.0 / Tstep */ kTRAJ > 200000) //
if (Tstart > lcncCycleTime) // Not enough movement to make a step { // Too high frequency, deal with this later.
return; // return updatePosAndReturn(stepPosStop, 3); //
if (/* 1.0 / Tstep */ kTRAJ > 200000) // } //
{ // Too high frequency, deal with this later. int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in
err = 1; // //
return; // if (abs(stepPosStart - oldStepPos()) == 0) // StepPosStart and oldStepPos() are often the same, but don't redo the step
} // { //
int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in stepPosStart += dir; // New first step
// Tstart += Tstep; //
switch (abs(stepPosStart - oldStepPos())) // if (Tstart > lcncCycleTime) // Not enough movement to make a step
{ // return updatePosAndReturn(stepPosStop, 4); //
case 0: // StepPosStart and oldStepPos() are often the same, but don't redo the step } //
stepPosStart += dir; // New first step if (abs(stepPosStart - oldStepPos()) > 1) // Shouldn't happen
Tstart += Tstep; // { //
if (Tstart > lcncCycleTime) // Not enough movement to make a step return updatePosAndReturn(stepPosStop, 5); //
return; // } //
break; // // Now the old point and the start point should be separate.
case 1: // if (Tstart > lcncCycleTime) // Not enough movement to make a step
// Let it slide through and deal with it after the case switch return updatePosAndReturn(stepPosStop, 6); // Check this again
break; // // Tstart, Tstep and Tstop defines the coming pwm-sequence.
default: // return 0; // Always do one pulse at Tstart when we come here. Next Tstart+Tstep and so on until Tstop.
err = 2; //
return; //
break; //
} //
// Now the old point and the start point should be separate.
if (Tstart > lcncCycleTime) // Not enough movement to make a step
return; //
// Tstart, Tstep and Tstop defines the coming pwm-sequence.
// Always do one pulse at Tstart when we come here. Next Tstart+Tstep and so on until Tstop.
} }
void StepGen2::timerCB() void StepGen2::timerCB()
{ {
@@ -110,5 +101,11 @@ void StepGen2::timerCB()
} }
#endif #endif
} }
uint32_t StepGen2::updatePosAndReturn(int32_t stepPosStop, uint32_t i)
{ //
oldPos(reqPos()); // Save the numeric position for next step
oldStepPos(stepPosStop); // also the step we are at}
return i;
}
uint32_t StepGen2::sync0CycleTime = 0; uint32_t StepGen2::sync0CycleTime = 0;