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

Class StepGen2 done after Stepgen.odb

Browse Source
This commit is contained in:
Hakan Bastedt
2024-02-08 21:28:48 +01:00
parent 2fb5252d37
commit fe3de876fa
5 changed files with 83 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Firmware/doc/.~lock.Stepgen.odp#
View File

@@ -1 +0,0 @@
,HAKAN-PC/Hakan,Hakan-PC,05.02.2024 20:27,file:///C:/Users/Hakan/AppData/Roaming/LibreOffice/4;

BIN
Firmware/doc/Stepgen.odp
View File

Binary file not shown.

43
Firmware/include/StepGen2.h
View File

@@ -7,49 +7,40 @@ class StepGen2
{
private:
volatile double_t actualPosition;
volatile double_t requestedPosition;
volatile double_t oldPosition;
volatile int32_t oldStepPosition;
volatile uint8_t enabled;
volatile int32_t nSteps;
volatile uint32_t timerPulseSteps;
volatile float Tstart;
volatile float Tstop;
volatile float Tstep;
HardwareTimer *MyTim;
HardwareTimer *MyTim2; // 10,11,13,14
int16_t stepsPerMM;
const float maxAllowedFrequency = 100000; // 100 kHz for now
HardwareTimer *pulseTimer;
uint32_t pulseTimerChan;
HardwareTimer *startTimer; // 10,11,13,14
uint8_t dirPin;
PinName stepPin;
uint32_t timerChan;
const uint32_t maxFreq = 100000;
volatile uint32_t prevFreq1 = 0;
volatile uint32_t prevFreq2 = 0;
const float Tjitter = 50.0; // Time unit is microseconds
uint32_t err = 0;
public:
static uint32_t sync0CycleTime;
volatile double_t commandedPosition; // End position when this cycle is completed
volatile int32_t commandedStepPosition; // End step position when this cycle is completed
volatile double_t initialPosition; // From previous cycle
volatile int32_t initialStepPosition; // From previous cycle
int16_t stepsPerMM; // This many steps per mm
volatile uint8_t enabled; // Enabled step generator
volatile float frequency;
static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time
volatile uint32_t lcncCycleTime; // Linuxcnc nominal cycle time (1 ms often)
StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
uint32_t handleStepper(void);
void timerCB();
void timer2CB();
void enable(uint8_t yes);
void reqPos(double_t pos) { requestedPosition = pos; };
double reqPos() { return requestedPosition; };
void oldPos(double_t pos) { oldPosition = pos; };
double oldPos() { return oldPosition; };
void oldStepPos(int32_t pos) { oldStepPosition = pos; }
int32_t oldStepPos() { return oldStepPosition; }
void actPos(double_t pos) { actualPosition = pos; };
double actPos() { return actualPosition; };
void setScale(int16_t spm) { stepsPerMM = spm; }
int16_t getScale() { return stepsPerMM; }
uint32_t updatePosAndReturn(int32_t stepPosStop, uint32_t i);
void startTimerCB();
void pulseTimerCB();
uint32_t updatePos(uint32_t i);
};
#endif

151
Firmware/src/StepGen2.cpp
View File

@@ -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;
}

13
Firmware/src/main.cpp
View File

@@ -33,11 +33,11 @@ void timerCallbackStep2(void)
}
#endif
#include "StepGen2.h"
void timerCallbackStep(void);
void timerCallbackStepStart(void);
StepGen2 Step(TIM1, 4, PA_11, PA12, timerCallbackStep, TIM10, timerCallbackStepStart);
void timerCallbackStep(void) { Step.timerCB(); }
void timerCallbackStepStart(void) { Step.timer2CB(); }
void pulseTimerCallback(void);
void startTimerCallback(void);
StepGen2 Step(TIM1, 4, PA_11, PA12, pulseTimerCallback, TIM10, startTimerCallback);
void pulseTimerCallback(void) { Step.pulseTimerCB(); }
void startTimerCallback(void) { Step.startTimerCB(); }
CircularBuffer<uint32_t, 200> Tim;
volatile uint64_t nowTime = 0, thenTime = 0;
@@ -57,6 +57,9 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
void handleStepper(void)
{
Step.enabled = true;
Step.commandedPosition = Obj.StepGenIn1.CommandedPosition;
#if 0
Step1.handleStepper();
Step2.handleStepper();