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

2024-03-26 22:59:41 +01:00
parent 88bd1ead78
commit ea5a39d300
3 changed files with 72 additions and 36 deletions
--- a/Firmware/include/StepGen3.h
+++ b/Firmware/include/StepGen3.h
@@ -79,18 +79,19 @@ public:

    stepgen_t *stepgen_array = 0;

-    int num_chan = 0;  // number of step generators configured */
-    long periodns;     // makepulses function period in nanosec */
-    long old_periodns; // used to detect changes in periodns */
-    double periodfp;   // makepulses function period in seconds */
-    double freqscale;  // conv. factor from Hz to addval counts */
-    double accelscale; // conv. Hz/sec to addval cnts/period */
-    long old_dtns;     // update_freq funct period in nsec */
-    double dt;         // update_freq period in seconds */
-    double recip_dt;   // recprocal of period, avoids divides */
+    int num_chan = 0;          // number of step generators configured */
+    long periodns;             // makepulses function period in nanosec */
+    long old_periodns;         // used to detect changes in periodns */
+    double periodfp;           // makepulses function period in seconds */
+    double freqscale;          // conv. factor from Hz to addval counts */
+    double accelscale;         // conv. Hz/sec to addval cnts/period */
+    long old_dtns;             // update_freq funct period in nsec */
+    double dt;                 // update_freq period in seconds */
+    double recip_dt;           // recprocal of period, avoids divides */
+    volatile uint64_t cnt = 0; // Debug counter

    StepGen3(void);
-    void updateStepGen(double *pos_cmd);
+    void updateStepGen(double pos_cmd1, double pos_cmd2);
    int rtapi_app_main();
    int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode);
    void make_pulses(void *arg, long period);
@@ -143,11 +144,11 @@ private:
 };

 // For the example
-#define BASE_PERIOD 12000 // 12 is max
+#define BASE_PERIOD 20000 // 12 is max
 #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_Z_STEPGEN_MAXACCEL 520.0
+#define JOINT_Z_STEPGEN_MAXACCEL (1.2 * 520.0)
 #define JOINT_Z_SCALE -80

 #endif
--- a/Firmware/src/StepGen3.cpp
+++ b/Firmware/src/StepGen3.cpp
@@ -349,6 +349,7 @@ void StepGen3::make_pulses(void *arg, long period)
    periodns = period;
    /* point to stepgen data structures */
    stepgen = (stepgen_t *)arg;
+    cnt++;

    for (n = 0; n < num_chan; n++)
    {
@@ -1101,13 +1102,14 @@ StepGen3::StepGen3(void)
    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++)
    {
        stepgen_t *step;
        step = &(stepgen_array[i]);
-        step->pos_cmd = pos_cmd[i];
        update_pos(step, SERVO_PERIOD);
        update_freq(step, SERVO_PERIOD);
    }
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -10,6 +10,8 @@ _Objects Obj;
 HardwareSerial Serial1(PA10, PA9);

 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"
 void indexPulseEncoderCB1(void);
@@ -38,24 +40,53 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
   // 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;
-uint64_t reallyNowTime = 0, reallyThenTime = 0;
-uint64_t timeDiff; // Timediff in nanoseconds
+uint64_t reallyNowTime = 0, reallyThenTime = 0; // Times in microseconds
+uint64_t timeDiff; // Timediff in microseconds
+int32_t delayT;
+
 void handleStepper(void)
 {
-#if 1
-   double pos[MAX_CHAN];
-   pos[0] = Obj.CommandedPosition1;
-   pos[1] = Obj.CommandedPosition2;
+   uint32_t t = micros();
+   reallyNowTime = longTime.extendTime(t);
+   timeDiff = reallyNowTime - reallyThenTime; // Time-diff in microseconds
+   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)
   {
      Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1;
      Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
-      Step->updateStepGen(pos);
+
      Obj.ActualPosition1 = Step->stepgen_array[0].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
@@ -65,8 +96,8 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
   // Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
   // Obj.IndexByte = Encoder1.getIndexState();
   float_t ap2 = Obj.ActualPosition2;
-#if 0
-   uint64_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
+#if 1
+   uint64_t dTim = irqTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
   Tim.push(dTim);
   uint64_t max_Tim = 0, min_Tim = UINT64_MAX;
   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;
 #endif
   Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
-   Obj.D1 = 0;
-   Obj.D2 = 0;
-   Obj.D3 = abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
-   Obj.D4 = 0;
+   Obj.D1 = Step->cnt % INT16_MAX; //nLoops;
+   Obj.D2 = Step->stepgen_array[1].freq;
+   Obj.D3 = 100*Obj.CommandedPosition2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
+   Obj.D4 = 100*Obj.ActualPosition2;
 }

 void ESC_interrupt_enable(uint32_t mask);
@@ -123,7 +154,7 @@ void basePeriodCB(void)
      {
         step = &(Step->stepgen_array[i]);
         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);
   Step = new StepGen3;

-   HardwareTimer *MyTim = new HardwareTimer(TIM1); // The base period timer
-   MyTim->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT);
-   MyTim->attachInterrupt(basePeriodCB);
-   MyTim->resume();
+   myTim = new HardwareTimer(TIM1); // The base period timer
+   myTim->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT);
+   myTim->attachInterrupt(basePeriodCB);
+   myTim->resume();
+
+   syncTimer = new HardwareTimer(TIM2); // The base period timer
+   syncTimer->attachInterrupt(syncWithLCNC);
 }

-double pos = 0;
 void loop(void)
 {
   uint64_t dTime;
@@ -172,7 +205,7 @@ void loop(void)
 void sync0Handler(void)
 {
   ALEventIRQ = ESC_ALeventread();
-   // if (ALEventIRQ & ESCREG_ALEVENT_SM2)
+   //if (ALEventIRQ & ESCREG_ALEVENT_SM2)
   {
      serveIRQ = 1;
      irqTime = longTime.extendTime(micros());