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

wip now the 50 kHz timer is only 40 kHz. How come?

Browse Source
This commit is contained in:
Hakan Bastedt
2024-03-26 22:59:41 +01:00
parent 88bd1ead78
commit ea5a39d300
3 changed files with 72 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
Firmware/include/StepGen3.h
View File

@@ -79,18 +79,19 @@ public:
stepgen_t *stepgen_array = 0; stepgen_t *stepgen_array = 0;
int num_chan = 0; // number of step generators configured */ int num_chan = 0; // number of step generators configured */
long periodns; // makepulses function period in nanosec */ long periodns; // makepulses function period in nanosec */
long old_periodns; // used to detect changes in periodns */ long old_periodns; // used to detect changes in periodns */
double periodfp; // makepulses function period in seconds */ double periodfp; // makepulses function period in seconds */
double freqscale; // conv. factor from Hz to addval counts */ double freqscale; // conv. factor from Hz to addval counts */
double accelscale; // conv. Hz/sec to addval cnts/period */ double accelscale; // conv. Hz/sec to addval cnts/period */
long old_dtns; // update_freq funct period in nsec */ long old_dtns; // update_freq funct period in nsec */
double dt; // update_freq period in seconds */ double dt; // update_freq period in seconds */
double recip_dt; // recprocal of period, avoids divides */ double recip_dt; // recprocal of period, avoids divides */
volatile uint64_t cnt = 0; // Debug counter
StepGen3(void); StepGen3(void);
void updateStepGen(double *pos_cmd); void updateStepGen(double pos_cmd1, double pos_cmd2);
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);
void make_pulses(void *arg, long period); void make_pulses(void *arg, long period);
@@ -143,11 +144,11 @@ private:
}; };
// For the example // For the example
#define BASE_PERIOD 12000 // 12 is max #define BASE_PERIOD 20000 // 12 is max
#define SERVO_PERIOD 1000000 #define SERVO_PERIOD 1000000
#define JOINT_X_STEPGEN_MAXACCEL 520.0 #define JOINT_X_STEPGEN_MAXACCEL (1.2 * 520.0)
#define JOINT_X_SCALE -200 #define JOINT_X_SCALE -200
#define JOINT_Z_STEPGEN_MAXACCEL 520.0 #define JOINT_Z_STEPGEN_MAXACCEL (1.2 * 520.0)
#define JOINT_Z_SCALE -80 #define JOINT_Z_SCALE -80
#endif #endif

6
Firmware/src/StepGen3.cpp
View File

@@ -349,6 +349,7 @@ void StepGen3::make_pulses(void *arg, long period)
periodns = period; periodns = period;
/* point to stepgen data structures */ /* point to stepgen data structures */
stepgen = (stepgen_t *)arg; stepgen = (stepgen_t *)arg;
cnt++;
for (n = 0; n < num_chan; n++) for (n = 0; n < num_chan; n++)
{ {
@@ -1101,13 +1102,14 @@ 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_cmd) void StepGen3::updateStepGen(double pos_cmd1, double pos_cmd2)
{ {
stepgen_array[0].pos_cmd = pos_cmd1;
stepgen_array[1].pos_cmd = pos_cmd2;
for (int i = 0; i < num_chan; i++) for (int i = 0; i < num_chan; i++)
{ {
stepgen_t *step; stepgen_t *step;
step = &(stepgen_array[i]); step = &(stepgen_array[i]);
step->pos_cmd = pos_cmd[i];
update_pos(step, SERVO_PERIOD); update_pos(step, SERVO_PERIOD);
update_freq(step, SERVO_PERIOD); update_freq(step, SERVO_PERIOD);
} }

75
Firmware/src/main.cpp
View File

@@ -10,6 +10,8 @@ _Objects Obj;
HardwareSerial Serial1(PA10, PA9); HardwareSerial Serial1(PA10, PA9);
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
HardwareTimer *myTim; // The base period timer
HardwareTimer *syncTimer; // The timer that syncs "with linuxcnc cycle"
#include "MyEncoder.h" #include "MyEncoder.h"
void indexPulseEncoderCB1(void); void indexPulseEncoderCB1(void);
@@ -38,24 +40,53 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
// Step2.enable(1); // Step2.enable(1);
} }
volatile double pos_cmd1, pos_cmd2;
volatile uint64_t syncTime=0, oldSyncTime = 0;
void syncWithLCNC()
{
if (Step)
Step->updateStepGen(pos_cmd1, pos_cmd2);
syncTimer->pause();
syncTime = longTime.extendTime(micros());
}
uint16_t nLoops; uint16_t nLoops;
uint64_t reallyNowTime = 0, reallyThenTime = 0; uint64_t reallyNowTime = 0, reallyThenTime = 0; // Times in microseconds
uint64_t timeDiff; // Timediff in nanoseconds uint64_t timeDiff; // Timediff in microseconds
int32_t delayT;
void handleStepper(void) void handleStepper(void)
{ {
#if 1 uint32_t t = micros();
double pos[MAX_CHAN]; reallyNowTime = longTime.extendTime(t);
pos[0] = Obj.CommandedPosition1; timeDiff = reallyNowTime - reallyThenTime; // Time-diff in microseconds
pos[1] = Obj.CommandedPosition2; nLoops = round(double(timeDiff) / 1000.0);
reallyThenTime = reallyNowTime;
pos_cmd1 = Obj.CommandedPosition1;
pos_cmd2 = Obj.CommandedPosition2;
// Obj.ActualPosition1 = Obj.CommandedPosition1;
// Obj.ActualPosition2 = Obj.CommandedPosition2;
if (Step) if (Step)
{ {
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;
Step->updateStepGen(pos);
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;
} }
#endif uint32_t diffT = longTime.extendTime(micros()) - irqTime;
delayT = 700 - diffT;
if (delayT > 0 && delayT < 900)
{
syncTimer->setOverflow(delayT, MICROSEC_FORMAT);
syncTimer->refresh();
syncTimer->resume();
}
else
{
syncWithLCNC();
}
} }
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -65,8 +96,8 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
// Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); // Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
// Obj.IndexByte = Encoder1.getIndexState(); // Obj.IndexByte = Encoder1.getIndexState();
float_t ap2 = Obj.ActualPosition2; float_t ap2 = Obj.ActualPosition2;
#if 0 #if 1
uint64_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds uint64_t dTim = irqTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
Tim.push(dTim); Tim.push(dTim);
uint64_t max_Tim = 0, min_Tim = UINT64_MAX; uint64_t max_Tim = 0, min_Tim = UINT64_MAX;
for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++) for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++)
@@ -80,10 +111,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
thenTime = irqTime; thenTime = irqTime;
#endif #endif
Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
Obj.D1 = 0; Obj.D1 = Step->cnt % INT16_MAX; //nLoops;
Obj.D2 = 0; Obj.D2 = Step->stepgen_array[1].freq;
Obj.D3 = abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos(); Obj.D3 = 100*Obj.CommandedPosition2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
Obj.D4 = 0; Obj.D4 = 100*Obj.ActualPosition2;
} }
void ESC_interrupt_enable(uint32_t mask); void ESC_interrupt_enable(uint32_t mask);
@@ -123,7 +154,7 @@ void basePeriodCB(void)
{ {
step = &(Step->stepgen_array[i]); step = &(Step->stepgen_array[i]);
digitalWrite(Step->dirPin[i], step->phase[DIR_PIN] ? LOW : HIGH); digitalWrite(Step->dirPin[i], step->phase[DIR_PIN] ? LOW : HIGH);
digitalWrite(Step->stepPin[i], step->phase[STEP_PIN] ? LOW : HIGH); digitalWrite(Step->stepPin[i], step->phase[STEP_PIN] ? HIGH : LOW);
} }
} }
} }
@@ -142,13 +173,15 @@ void setup(void)
pinMode(PC10, OUTPUT); pinMode(PC10, OUTPUT);
Step = new StepGen3; Step = new StepGen3;
HardwareTimer *MyTim = new HardwareTimer(TIM1); // The base period timer myTim = new HardwareTimer(TIM1); // The base period timer
MyTim->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); myTim->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT);
MyTim->attachInterrupt(basePeriodCB); myTim->attachInterrupt(basePeriodCB);
MyTim->resume(); myTim->resume();
syncTimer = new HardwareTimer(TIM2); // The base period timer
syncTimer->attachInterrupt(syncWithLCNC);
} }
double pos = 0;
void loop(void) void loop(void)
{ {
uint64_t dTime; uint64_t dTime;
@@ -172,7 +205,7 @@ void loop(void)
void sync0Handler(void) void sync0Handler(void)
{ {
ALEventIRQ = ESC_ALeventread(); ALEventIRQ = ESC_ALeventread();
// if (ALEventIRQ & ESCREG_ALEVENT_SM2) //if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{ {
serveIRQ = 1; serveIRQ = 1;
irqTime = longTime.extendTime(micros()); irqTime = longTime.extendTime(micros());