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

logic might be there

Browse Source
This commit is contained in:
Hakan Bastedt
2024-02-01 20:30:02 +01:00
parent 934f3cb028
commit d1eb1d9a40
4 changed files with 182 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Firmware/.vscode/settings.json vendored
View File

@@ -10,7 +10,8 @@
"vector": "cpp",
"system_error": "cpp",
"numeric": "cpp",
"ostream": "cpp"
"ostream": "cpp",
"cmath": "cpp"
},
"C_Cpp.errorSquiggles": "disabled",
"cmake.configureOnOpen": false

53
Firmware/include/StepGen2.h Executable file
View File

@@ -0,0 +1,53 @@
#ifndef STEPGEN
#define STEPGEN
#include <HardwareTimer.h>
class StepGen2
{
private:
volatile uint8_t timerIsRunning;
volatile int32_t timerStepPosition;
volatile int32_t timerStepDirection;
volatile int32_t timerStepPositionAtEnd;
volatile int32_t timerNewEndStepPosition;
volatile uint32_t timerNewCycleTime;
volatile double_t actualPosition;
volatile double_t requestedPosition;
volatile double_t oldPosition;
volatile int32_t oldStepPosition;
volatile uint8_t enabled;
HardwareTimer *MyTim;
int16_t stepsPerMM;
uint8_t dirPin;
PinName stepPin;
uint32_t timerChan;
const uint32_t maxFreq = 100000;
volatile uint32_t prevFreq1 = 0;
volatile uint32_t prevFreq2 = 0;
uint32_t err = 0;
public:
static uint32_t sync0CycleTime;
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));
void handleStepper(void);
void timerCB();
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; }
};
#endif

114
Firmware/src/StepGen2.cpp Executable file
View File

@@ -0,0 +1,114 @@
#include <Arduino.h>
#include <stdio.h>
#include "StepGen2.h"
StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void))
{
timerIsRunning = 0;
timerStepPosition = 0;
timerStepDirection = 0;
timerStepPositionAtEnd = 0;
timerNewEndStepPosition = 0;
actualPosition = 0;
requestedPosition = 0;
oldPosition = 0;
oldStepPosition = 0;
stepsPerMM = 0;
enabled = 0;
dirPin = _dirPin;
stepPin = _stepPin;
timerChan = _timerChannel;
MyTim = new HardwareTimer(Timer);
MyTim->attachInterrupt(irq);
pinMode(dirPin, OUTPUT);
}
void StepGen2::handleStepper(void)
{
if (!enabled)
return;
lcncCycleTime = StepGen2::sync0CycleTime;
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
//
oldPos(reqPos()); // Save the numeric position for next step
oldStepPos(stepPosStop); // also the step we are at
//
if (Tstart > lcncCycleTime) // Not enough movement to make a step
return; //
if (/* 1.0 / Tstep */ kTRAJ > 200000) //
{ // Too high frequency, deal with this later.
err = 1; //
return; //
} //
int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in
//
switch (abs(stepPosStart - oldStepPos())) //
{ //
case 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; //
break; //
case 1: //
// Let it slide through and deal with it after the case switch
break; //
default: //
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()
{
#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;
}
}
if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
{
timerIsRunning = 0;
MyTim->pause();
}
#endif
}
uint32_t StepGen2::sync0CycleTime = 0;

21
Firmware/src/main.cpp
View File

@@ -17,23 +17,22 @@ void indexPulseEncoderCB1(void)
{
Encoder1.indexPulse();
}
#if 0
#include "StepGen.h"
void timerCallbackStep1(void);
StepGen Step1(TIM1, 4, PA_11, PA12, timerCallbackStep1);
void timerCallbackStep1(void)
{
Step1.timerCB();
}
void timerCallbackStep2(void);
StepGen Step2(TIM3, 4, PC_9, PC10, timerCallbackStep2);
void timerCallbackStep2(void)
{
Step2.timerCB();
}
#endif
#include "StepGen2.h"
CircularBuffer<uint32_t, 200> Tim;
volatile uint64_t nowTime = 0, thenTime = 0;
@@ -41,19 +40,22 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
{
Encoder1.setLatch(Obj.IndexLatchEnable);
Encoder1.setScale(Obj.EncPosScale);
#if 0
Step1.reqPos(Obj.StepGenIn1.CommandedPosition);
Step1.setScale(Obj.StepGenIn1.StepsPerMM);
Step1.enable(Obj.Enable1);
Step2.reqPos(Obj.StepGenIn2.CommandedPosition);
Step2.setScale(Obj.StepGenIn2.StepsPerMM);
Step2.enable(Obj.Enable1);
#endif
}
void handleStepper(void)
{
#if 0
Step1.handleStepper();
Step2.handleStepper();
#endif
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -62,10 +64,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
Obj.EncPos = Encoder1.currentPos();
Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
Obj.IndexByte = Encoder1.getIndexState();
#if 0
Obj.StepGenOut1.ActualPosition = Step1.actPos();
Obj.StepGenOut2.ActualPosition = Step2.actPos();
#endif
uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
Tim.push(dTim);
uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
@@ -179,6 +181,9 @@ uint16_t dc_checker(void)
{
// Indicate we run DC
ESCvar.dcsync = 1;
StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
#if 0
StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
#endif
StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
return 0;
}