Class StepGen2 done after Stepgen.odb
This commit is contained in:
Hakan Bastedt
@@ -5,117 +5,82 @@
|
||||
StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
|
||||
{
|
||||
actualPosition = 0;
|
||||
requestedPosition = 0;
|
||||
oldPosition = 0;
|
||||
oldStepPosition = 0;
|
||||
commandedPosition = 0;
|
||||
commandedStepPosition = 0;
|
||||
initialPosition = 0;
|
||||
initialStepPosition = 0;
|
||||
stepsPerMM = 0;
|
||||
enabled = 0;
|
||||
|
||||
dirPin = _dirPin;
|
||||
stepPin = _stepPin;
|
||||
timerChan = _timerChannel;
|
||||
MyTim = new HardwareTimer(Timer);
|
||||
MyTim->attachInterrupt(irq);
|
||||
pulseTimerChan = _timerChannel;
|
||||
pulseTimer = new HardwareTimer(Timer);
|
||||
pulseTimer->attachInterrupt(irq);
|
||||
pinMode(dirPin, OUTPUT);
|
||||
MyTim2 = new HardwareTimer(Timer2);
|
||||
MyTim2->attachInterrupt(irq2);
|
||||
startTimer = new HardwareTimer(Timer2);
|
||||
startTimer->attachInterrupt(irq2);
|
||||
}
|
||||
|
||||
uint32_t StepGen2::handleStepper(void)
|
||||
{
|
||||
if (!enabled)
|
||||
return 1;
|
||||
return updatePos(0);
|
||||
lcncCycleTime = StepGen2::sync0CycleTime;
|
||||
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
|
||||
if (initialStepPosition == commandedStepPosition) // No movement
|
||||
return updatePos(1);
|
||||
|
||||
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 kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime; // Slope
|
||||
float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime; // Intercept
|
||||
int32_t stepPosStart = floor(y0TRAJ); // First step position, integer value of first point position
|
||||
int32_t stepPosStop = floor(y1TRAJ); // End step position
|
||||
//
|
||||
float Tstart = (stepPosStart - mTRAJ) / kTRAJ; // First step at this time
|
||||
float Tstop = (stepPosStop - mTRAJ) / kTRAJ; // And the last step
|
||||
float Tstep = fabs(1.0 / kTRAJ); // Time between steps
|
||||
float stepFrequency = fabs(kTRAJ); // 1/Tstep - which is kTRAJ
|
||||
//
|
||||
if (Tstart > lcncCycleTime) // Not enough movement to make a step
|
||||
return updatePosAndReturn(stepPosStop, 2); //
|
||||
if (/* 1.0 / Tstep */ kTRAJ > 200000) //
|
||||
{ // Too high frequency, deal with this later.
|
||||
return updatePosAndReturn(stepPosStop, 3); //
|
||||
} //
|
||||
int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in
|
||||
//
|
||||
if (abs(stepPosStart - oldStepPos()) == 0) // StepPosStart and oldStepPos() are often the same, but don't redo the step
|
||||
{ //
|
||||
stepPosStart += dir; // New first step
|
||||
Tstart += Tstep; //
|
||||
if (Tstart > lcncCycleTime) // Not enough movement to make a step
|
||||
return updatePosAndReturn(stepPosStop, 4); //
|
||||
} //
|
||||
if (abs(stepPosStart - oldStepPos()) > 1) // Shouldn't happen
|
||||
{ //
|
||||
return updatePosAndReturn(stepPosStop, 5); //
|
||||
} //
|
||||
// Now the old point and the start point should be separate.
|
||||
if (Tstart > lcncCycleTime) // Not enough movement to make a step
|
||||
return updatePosAndReturn(stepPosStop, 6); // Check this again
|
||||
// Tstart, Tstep and Tstop defines the coming pwm-sequence.
|
||||
//
|
||||
MyTim2->setOverflow(Tstart + Tjitter, MICROSEC_FORMAT); // All handled by irqs
|
||||
MyTim2->resume();
|
||||
return updatePosAndReturn(stepPosStop, 0);
|
||||
}
|
||||
void StepGen2::timer2CB()
|
||||
{
|
||||
MyTim2->pause(); // Once is enough.
|
||||
MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
|
||||
MyTim->setOverflow(floor(1e6 / Tstep), HERTZ_FORMAT); // 100000 microseconds = 100 milliseconds
|
||||
MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50%
|
||||
nSteps = round((Tstop - Tstart) / Tstep + 1);
|
||||
if (nSteps > 0)
|
||||
MyTim->resume();
|
||||
}
|
||||
void StepGen2::timerCB()
|
||||
{
|
||||
#if 0
|
||||
timerStepPosition += timerStepDirection; // The step that was just completed
|
||||
if (timerNewEndStepPosition != 0) // Are we going to reload?
|
||||
{
|
||||
// Input for reload is timerNewEndStepPosition
|
||||
// The timer has current position and from this
|
||||
// can set new frequency and new endtarget for steps
|
||||
MyTim->pause(); // We are not at stop, let's stop it. Note stepPin is floating
|
||||
int32_t steps = timerNewEndStepPosition - timerStepPosition;
|
||||
if (steps != 0)
|
||||
{
|
||||
uint8_t sgn = steps > 0 ? HIGH : LOW;
|
||||
digitalWrite(dirPin, sgn);
|
||||
float_t freqf = abs(steps) / float(pwmCycleTime * 1.0e-6);
|
||||
uint32_t freq = uint32_t(freqf);
|
||||
timerStepDirection = steps > 0 ? 1 : -1;
|
||||
timerStepPositionAtEnd = timerNewEndStepPosition;
|
||||
timerNewEndStepPosition = 0; // Set to zero to not reload next time
|
||||
MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
|
||||
MyTim->setOverflow(freq, HERTZ_FORMAT);
|
||||
MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
|
||||
MyTim->resume();
|
||||
timerIsRunning = 1;
|
||||
}
|
||||
float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step
|
||||
/ (float)lcncCycleTime; // from here on
|
||||
if (approximateFrequency > maxAllowedFrequency) // Stay on this position
|
||||
return 1;
|
||||
|
||||
float kTRAJ = (commandedPosition - initialPosition) / float(lcncCycleTime); // Straight line equation
|
||||
float mTRAJ = initialPosition; // position = kTRAJ x time + mTRAJ
|
||||
// Operating on incoming positions (not steps)
|
||||
if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01) // Very flat slope
|
||||
{ //
|
||||
Tstart = 0.5 * lcncCycleTime; // Just take a step in the middle of the cycle
|
||||
frequency = 10000; // At some suitable frequency
|
||||
nSteps = kTRAJ > 0 ? 1 : -1; // Take only one step
|
||||
}
|
||||
if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
|
||||
else // Regular step train, up or down
|
||||
{
|
||||
timerIsRunning = 0;
|
||||
MyTim->pause();
|
||||
if (kTRAJ > 0)
|
||||
Tstart = (ceil(initialPosition * stepsPerMM) - mTRAJ) / kTRAJ;
|
||||
else
|
||||
Tstart = (floor(initialPosition * stepsPerMM) - mTRAJ) / kTRAJ;
|
||||
frequency = kTRAJ * stepsPerMM;
|
||||
nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction.
|
||||
}
|
||||
#endif
|
||||
updatePos(5);
|
||||
|
||||
startTimer->setOverflow(Tstart + Tjitter, MICROSEC_FORMAT); // All handled by irqs
|
||||
startTimer->resume();
|
||||
return 1;
|
||||
}
|
||||
void StepGen2::startTimerCB()
|
||||
{
|
||||
startTimer->pause(); // Once is enough.
|
||||
digitalWrite(dirPin, nSteps > 0 ? 1 : -1);
|
||||
timerPulseSteps = abs(nSteps);
|
||||
pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
|
||||
pulseTimer->setOverflow(uint32_t(frequency), HERTZ_FORMAT);
|
||||
pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT); // 50%
|
||||
pulseTimer->resume();
|
||||
}
|
||||
void StepGen2::pulseTimerCB()
|
||||
{
|
||||
--timerPulseSteps;
|
||||
if (timerPulseSteps == 0)
|
||||
pulseTimer->pause();
|
||||
}
|
||||
|
||||
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}
|
||||
uint32_t StepGen2::updatePos(uint32_t i)
|
||||
{ //
|
||||
initialPosition = commandedPosition; // Save the numeric position for next step
|
||||
initialStepPosition = commandedStepPosition; // also the step we are at}
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user