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

Tried to extrapolate when missing position data. Didn't work as expected but I'll keep it.

Browse Source
This commit is contained in:
Hakan Bastedt
2024-04-12 17:56:26 +02:00
parent b1cc8f1717
commit a4ca1f641a
3 changed files with 39 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Firmware/include/StepGen3.h
View File

@@ -92,7 +92,7 @@ public:
volatile uint64_t cnt = 0; // Debug counter volatile uint64_t cnt = 0; // Debug counter
#undef double #undef double
StepGen3(void); StepGen3(void);
void updateStepGen(double pos_cmd1, double pos_cmd2); void updateStepGen(double pos_cmd1, double pos_cmd2, uint32_t servoPeriod);
void makeAllPulses(void); void makeAllPulses(void);
int rtapi_app_main(); int rtapi_app_main();
int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode); int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode);
@@ -147,7 +147,7 @@ private:
// For the example // For the example
#define BASE_PERIOD 20000 #define BASE_PERIOD 20000
#define SERVO_PERIOD 2000000 //#define SERVO_PERIOD 2000000
#define JOINT_X_STEPGEN_MAXACCEL 0.0 #define JOINT_X_STEPGEN_MAXACCEL 0.0
#define JOINT_Z_STEPGEN_MAXACCEL 0.0 #define JOINT_Z_STEPGEN_MAXACCEL 0.0
#define JOINT_X_SCALE -200 #define JOINT_X_SCALE -200

6
Firmware/src/StepGen3.cpp
View File

@@ -1102,7 +1102,7 @@ StepGen3::StepGen3(void)
stepgen_array[0].enable = stepgen_array[1].enable = 1; stepgen_array[0].enable = stepgen_array[1].enable = 1;
} }
void StepGen3::updateStepGen(double pos_cmd1, double pos_cmd2) void StepGen3::updateStepGen(double pos_cmd1, double pos_cmd2, uint32_t servoPeriod)
{ {
stepgen_array[0].pos_cmd = pos_cmd1; stepgen_array[0].pos_cmd = pos_cmd1;
stepgen_array[1].pos_cmd = pos_cmd2; stepgen_array[1].pos_cmd = pos_cmd2;
@@ -1110,8 +1110,8 @@ void StepGen3::updateStepGen(double pos_cmd1, double pos_cmd2)
{ {
stepgen_t *step; stepgen_t *step;
step = &(stepgen_array[i]); step = &(stepgen_array[i]);
update_pos(step, SERVO_PERIOD); update_pos(step, servoPeriod); // servoPeriod is in nanosecs
update_freq(step, SERVO_PERIOD); update_freq(step, servoPeriod);
} }
} }

49
Firmware/src/main.cpp
View File

@@ -12,7 +12,7 @@ HardwareSerial Serial1(PA10, PA9);
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
HardwareTimer *baseTimer; // The base period timer HardwareTimer *baseTimer; // The base period timer
HardwareTimer *syncTimer; // The timer that syncs "with linuxcnc cycle" HardwareTimer *syncTimer; // The timer that syncs "with linuxcnc cycle"
uint16_t sync0CycleTime; // usecs uint32_t sync0CycleTime; // nanosecs, often 1000000 ( 1 ms )
#include "MyEncoder.h" #include "MyEncoder.h"
volatile uint16_t encCnt = 0; volatile uint16_t encCnt = 0;
@@ -47,18 +47,21 @@ volatile uint16_t basePeriodCnt;
volatile uint64_t makePulsesCnt = 0, prevMakePulsesCnt = 0; volatile uint64_t makePulsesCnt = 0, prevMakePulsesCnt = 0;
volatile uint16_t deltaMakePulsesCnt; volatile uint16_t deltaMakePulsesCnt;
volatile double pos_cmd1, pos_cmd2; double posCmd1, posCmd2;
double oldPosCmd1, oldPosCmd2;
double deltaPosCmd1, deltaPosCmd2;
void syncWithLCNC() void syncWithLCNC()
{ {
syncTimer->pause(); syncTimer->pause();
baseTimer->pause(); baseTimer->pause();
deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt; deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt;
prevMakePulsesCnt = makePulsesCnt; prevMakePulsesCnt = makePulsesCnt;
Step->updateStepGen(pos_cmd1, pos_cmd2); // Update positions Step->updateStepGen(posCmd1, posCmd2, sync0CycleTime); // Update positions
Step->makeAllPulses(); // Make first step right here Step->makeAllPulses(); // Make first step right here
basePeriodCnt = SERVO_PERIOD / BASE_PERIOD; // basePeriodCnt = sync0CycleTime / BASE_PERIOD; //
baseTimer->refresh(); // baseTimer->refresh(); //
baseTimer->resume(); // Make the other steps in ISR baseTimer->resume(); // Make the other steps in ISR
} }
void basePeriodCB(void) void basePeriodCB(void)
@@ -74,6 +77,7 @@ uint16_t thisCycleTime; // In usecs
int16_t maxIrqServeTime = 0; int16_t maxIrqServeTime = 0;
uint64_t oldIrqTime = 0; uint64_t oldIrqTime = 0;
uint16_t nLoops; uint16_t nLoops;
uint16_t failedSM2s = 0;
void handleStepper(void) void handleStepper(void)
{ {
@@ -100,18 +104,33 @@ void handleStepper(void)
if (maxIrqServeTime < maxInBuffer) if (maxIrqServeTime < maxInBuffer)
maxIrqServeTime = maxInBuffer; maxIrqServeTime = maxInBuffer;
} }
if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{ // The normal case, position update every cycle
posCmd1 = Obj.CommandedPosition1; // The position update
posCmd2 = Obj.CommandedPosition2;
deltaPosCmd1 = posCmd1 - oldPosCmd1; // Needed for extrapolation in the other case
deltaPosCmd2 = posCmd2 - oldPosCmd2;
failedSM2s = 0;
}
else
{ // Not normal, we didn't get a position update. Extrapolate from previous updates
if (failedSM2s++ < 10) // Do max 10 such extrapolations, should be plenty
{ //
posCmd1 += deltaPosCmd1; // Continue with the same speed
posCmd2 += deltaPosCmd2;
}
}
oldPosCmd1 = posCmd1;
oldPosCmd2 = posCmd2;
pos_cmd1 = Obj.CommandedPosition1; // Obj.ActualPosition1 = Obj.CommandedPosition1;
pos_cmd2 = Obj.CommandedPosition2; // Obj.ActualPosition2 = Obj.CommandedPosition2;
Obj.ActualPosition1 = Obj.CommandedPosition1;
Obj.ActualPosition2 = Obj.CommandedPosition2;
Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1; Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1;
Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2; Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
// Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb; Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
// Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb; Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
delayT = maxIrqServeTime - diffT; // Add 10 as some safety margin delayT = maxIrqServeTime - diffT; // Add 10 as some safety margin
if (delayT > 0 && delayT < 900) if (delayT > 0 && delayT < 900)
@@ -270,7 +289,7 @@ uint16_t dc_checker(void)
{ {
// Indicate we run DC // Indicate we run DC
ESCvar.dcsync = 1; ESCvar.dcsync = 1;
sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs sync0CycleTime = ESC_SYNC0cycletime(); // nanosecs
return 0; return 0;
} }