125 lines
5.5 KiB
C++
Executable File
125 lines
5.5 KiB
C++
Executable File
#include <Arduino.h>
|
|
#include <stdio.h>
|
|
#include "StepGen2.h"
|
|
#include "extend32to64.h"
|
|
|
|
extern "C"
|
|
{
|
|
#include "esc.h"
|
|
}
|
|
extern extend32to64 longTime;
|
|
|
|
StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
|
|
{
|
|
actualPosition = 0;
|
|
commandedPosition = 0;
|
|
commandedStepPosition = 0;
|
|
initialPosition = 0;
|
|
initialStepPosition = 0;
|
|
stepsPerMM = 0;
|
|
enabled = 0;
|
|
|
|
dirPin = _dirPin;
|
|
dirPinName = digitalPinToPinName(dirPin);
|
|
stepPin = _stepPin;
|
|
pulseTimerChan = _timerChannel;
|
|
pulseTimer = new HardwareTimer(Timer);
|
|
pulseTimer->attachInterrupt(pulseTimerChan, irq); // Capture/compare innterrupt
|
|
pinMode(dirPin, OUTPUT);
|
|
startTimer = new HardwareTimer(Timer2);
|
|
startTimer->attachInterrupt(irq2);
|
|
}
|
|
|
|
uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
|
|
{
|
|
frequency = 0;
|
|
nSteps = 0;
|
|
dbg = 0;
|
|
if (!enabled) // Just .... don't
|
|
return updatePos(0);
|
|
|
|
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
|
|
nSteps = commandedStepPosition - initialStepPosition;
|
|
if (nSteps == 0) // No movement
|
|
{
|
|
return updatePos(1);
|
|
}
|
|
lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
|
|
|
|
if (abs(nSteps) < 0) // 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);
|
|
|
|
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.
|
|
|
|
if (nSteps == 0) // Can do this much earlier, but want some calculated data for debugging
|
|
return updatePos(1);
|
|
|
|
timerFrequency = uint32_t(ceil(frequency));
|
|
startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
|
|
startTimer->refresh();
|
|
startTimer->resume();
|
|
return 1;
|
|
}
|
|
|
|
void StepGen2::startTimerCB()
|
|
{
|
|
startTimer->pause(); // Once is enough.
|
|
digitalWriteFast(dirPinName, nSteps < 0 ? HIGH : LOW); // nSteps negative => decrease, HIGH
|
|
// There will be a short break here for t2 usecs, in the future.
|
|
|
|
timerEndPosition += nSteps;
|
|
pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
|
|
pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
|
|
// pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
|
|
pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
|
|
pulseTimer->refresh();
|
|
pulseTimer->resume();
|
|
}
|
|
|
|
void StepGen2::pulseTimerCB()
|
|
{
|
|
int16_t dir = digitalReadFast(dirPinName); //
|
|
if (dir == HIGH) // The step just taken
|
|
timerPosition--;
|
|
else
|
|
timerPosition++;
|
|
int32_t diffPosition = timerEndPosition - timerPosition; // Same "polarity" as nSteps
|
|
if (diffPosition == 0)
|
|
pulseTimer->pause();
|
|
else
|
|
{
|
|
if (diffPosition < 0 && dir == LOW) // Change direction. Should not end up here, but alas
|
|
digitalWriteFast(dirPinName, HIGH); // Normal is to be HIGH when decreasing
|
|
if (diffPosition > 0 && dir == HIGH) // Change direction. Should not end up here, but alas
|
|
digitalWriteFast(dirPinName, LOW); // Normal is to be LOW when increasing
|
|
// Normally nothing is needed
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
uint32_t StepGen2::sync0CycleTime = 0;
|