From eced221b357b31ede1884690e823bab141c2e572 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Sun, 23 Feb 2025 20:40:11 +0100 Subject: [PATCH] Added SDO for basePeriod, PDOs for scale, enable, maxAccel. All got default values in ESI --- .../Firmware/include/StepGen3.h | 8 +- ...EaserCAT_3000_test_plasma_torch_reader.xml | 2087 ----------------- .../Firmware/lib/soes-esi/ecat_options.h | 4 +- .../Firmware/lib/soes-esi/eeprom.bin | Bin 2048 -> 2048 bytes .../Firmware/lib/soes-esi/eeprom.hex | 14 +- .../Firmware/lib/soes-esi/esi.json | 225 +- .../Firmware/lib/soes-esi/objectlist.c | 252 +- .../Firmware/lib/soes-esi/utypes.h | 23 +- .../Firmware/src/StepGen3.cpp | 18 +- .../Firmware/src/main.cpp | 62 +- 10 files changed, 393 insertions(+), 2300 deletions(-) delete mode 100755 Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/MetalMusings_EaserCAT_3000_test_plasma_torch_reader.xml diff --git a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/include/StepGen3.h b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/include/StepGen3.h index fb0cd99..898bad9 100755 --- a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/include/StepGen3.h +++ b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/include/StepGen3.h @@ -92,7 +92,12 @@ public: volatile uint64_t cnt = 0; // Debug counter #undef double StepGen3(void); - void updateStepGen(double pos_cmd1, double pos_cmd2, double pos_cmd3, double pos_cmd4, uint32_t servoPeriod); + + void updateStepGen(double pos_cmd1, double pos_cmd2, double pos_cmd3, double pos_cmd4, + float pos_scale1, float pos_scale2, float pos_scale3, float pos_scale4, + float max_acc1, float max_acc2, float max_acc3, float max_acc4, + uint8_t enable1, uint8_t enable2, uint8_t enable3, uint8_t enable4, + uint32_t servoPeriod); void makeAllPulses(void); int rtapi_app_main(); int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode); @@ -130,6 +135,7 @@ private: {2, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 0}; }; +// Initial values. Please update base_period using SDO, the others using PDOs // For the example 20000 ns => 50 kHz // It may be ok with over 100 kHz, it depends on the overall load. You just need to test. #define BASE_PERIOD 20000 diff --git a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/MetalMusings_EaserCAT_3000_test_plasma_torch_reader.xml b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/MetalMusings_EaserCAT_3000_test_plasma_torch_reader.xml deleted file mode 100755 index 68246b8..0000000 --- a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/MetalMusings_EaserCAT_3000_test_plasma_torch_reader.xml +++ /dev/null @@ -1,2087 +0,0 @@ - - - - 2730 - MetalMusings - - - - - MachineControl - Plasma torch reader - - - - - THCAD Reader ax58100 - MetalMusings EaserCAT 3000 test plasma torch reader - MachineControl - - 5002 - 0 - - - - DT1018 - 144 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - Vendor ID - UDINT - 32 - 16 - - ro - - - - 2 - Product Code - UDINT - 32 - 48 - - ro - - - - 3 - Revision Number - UDINT - 32 - 80 - - ro - - - - 4 - Serial Number - UDINT - 32 - 112 - - ro - - - - - DT1600 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - Scale - UDINT - 32 - 16 - - ro - - - - - DT1601 - 144 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - OUT1 - UDINT - 32 - 16 - - ro - - - - 2 - OUT2 - UDINT - 32 - 48 - - ro - - - - 3 - OUT3 - UDINT - 32 - 80 - - ro - - - - 4 - OUT4 - UDINT - 32 - 112 - - ro - - - - - DT1602 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - CommandedPosition1 - UDINT - 32 - 16 - - ro - - - - - DT1603 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - CommandedPosition2 - UDINT - 32 - 16 - - ro - - - - - DT1604 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - CommandedPosition3 - UDINT - 32 - 16 - - ro - - - - - DT1605 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - CommandedPosition4 - UDINT - 32 - 16 - - ro - - - - - DT1606 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - StepsPerMM1 - UDINT - 32 - 16 - - ro - - - - - DT1607 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - StepsPerMM2 - UDINT - 32 - 16 - - ro - - - - - DT1608 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - StepsPerMM3 - UDINT - 32 - 16 - - ro - - - - - DT1609 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - StepsPerMM4 - UDINT - 32 - 16 - - ro - - - - - DT1A00 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - Velocity - UDINT - 32 - 16 - - ro - - - - - DT1A01 - 272 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - IN1 - UDINT - 32 - 16 - - ro - - - - 2 - IN2 - UDINT - 32 - 48 - - ro - - - - 3 - IN3 - UDINT - 32 - 80 - - ro - - - - 4 - IN4 - UDINT - 32 - 112 - - ro - - - - 5 - IN5 - UDINT - 32 - 144 - - ro - - - - 6 - IN6 - UDINT - 32 - 176 - - ro - - - - 7 - IN7 - UDINT - 32 - 208 - - ro - - - - 8 - IN8 - UDINT - 32 - 240 - - ro - - - - - DT1A02 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - Frequency - UDINT - 32 - 16 - - ro - - - - - DT1A03 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - ActualPosition1 - UDINT - 32 - 16 - - ro - - - - - DT1A04 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - ActualPosition2 - UDINT - 32 - 16 - - ro - - - - - DT1A05 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - ActualPosition3 - UDINT - 32 - 16 - - ro - - - - - DT1A06 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - 1 - ActualPosition4 - UDINT - 32 - 16 - - ro - - - - - DT1C00ARR - USINT - 32 - - 1 - 4 - - - - DT1C00 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - Elements - DT1C00ARR - 32 - 16 - - ro - - - - - DT1C12ARR - UINT - 160 - - 1 - 10 - - - - DT1C12 - 176 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - Elements - DT1C12ARR - 160 - 16 - - ro - - - - - DT1C13ARR - UINT - 112 - - 1 - 7 - - - - DT1C13 - 128 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - Elements - DT1C13ARR - 112 - 16 - - ro - - - - - DT6001ARR - USINT - 64 - - 1 - 8 - - - - DT6001 - 80 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - Elements - DT6001ARR - 64 - 16 - - ro - T - - - - - DT7001ARR - USINT - 32 - - 1 - 4 - - - - DT7001 - 48 - - 0 - Max SubIndex - USINT - 8 - 0 - - ro - - - - Elements - DT7001ARR - 32 - 16 - - ro - R - - - - - UDINT - 32 - - - STRING(51) - 408 - - - STRING(5) - 40 - - - USINT - 8 - - - UINT - 16 - - - REAL - 32 - - - DINT - 32 - - - - - #x1000 - Device Type - UDINT - 32 - - 5001 - - - ro - m - - - - #x1008 - Device Name - STRING(51) - 408 - - MetalMusings EaserCAT 3000 test plasma torch reader - - - ro - - - - #x1009 - Hardware Version - STRING(5) - 40 - - 0.0.1 - - - ro - o - - - - #x100A - Software Version - STRING(5) - 40 - - 0.0.1 - - - ro - - - - #x1018 - Identity Object - DT1018 - 144 - - - Max SubIndex - - 4 - - - - Vendor ID - - 2730 - - - - Product Code - - 13294766 - - - - Revision Number - - 1 - - - - Serial Number - - 1 - - - - - ro - - - - #x1600 - Scale - DT1600 - 48 - - - Max SubIndex - - 1 - - - - Scale - - #x70000020 - - - - - ro - - - - #x1601 - Outputs - DT1601 - 144 - - - Max SubIndex - - 4 - - - - OUT1 - - #x70010108 - - - - OUT2 - - #x70010208 - - - - OUT3 - - #x70010308 - - - - OUT4 - - #x70010408 - - - - - ro - - - - #x1602 - CommandedPosition1 - DT1602 - 48 - - - Max SubIndex - - 1 - - - - CommandedPosition1 - - #x70020020 - - - - - ro - - - - #x1603 - CommandedPosition2 - DT1603 - 48 - - - Max SubIndex - - 1 - - - - CommandedPosition2 - - #x70030020 - - - - - ro - - - - #x1604 - CommandedPosition3 - DT1604 - 48 - - - Max SubIndex - - 1 - - - - CommandedPosition3 - - #x70040020 - - - - - ro - - - - #x1605 - CommandedPosition4 - DT1605 - 48 - - - Max SubIndex - - 1 - - - - CommandedPosition4 - - #x70050020 - - - - - ro - - - - #x1606 - StepsPerMM1 - DT1606 - 48 - - - Max SubIndex - - 1 - - - - StepsPerMM1 - - #x70060020 - - - - - ro - - - - #x1607 - StepsPerMM2 - DT1607 - 48 - - - Max SubIndex - - 1 - - - - StepsPerMM2 - - #x70070020 - - - - - ro - - - - #x1608 - StepsPerMM3 - DT1608 - 48 - - - Max SubIndex - - 1 - - - - StepsPerMM3 - - #x70080020 - - - - - ro - - - - #x1609 - StepsPerMM4 - DT1609 - 48 - - - Max SubIndex - - 1 - - - - StepsPerMM4 - - #x70090020 - - - - - ro - - - - #x1A00 - Velocity - DT1A00 - 48 - - - Max SubIndex - - 1 - - - - Velocity - - #x60000020 - - - - - ro - - - - #x1A01 - Inputs - DT1A01 - 272 - - - Max SubIndex - - 8 - - - - IN1 - - #x60010108 - - - - IN2 - - #x60010208 - - - - IN3 - - #x60010308 - - - - IN4 - - #x60010408 - - - - IN5 - - #x60010508 - - - - IN6 - - #x60010608 - - - - IN7 - - #x60010708 - - - - IN8 - - #x60010808 - - - - - ro - - - - #x1A02 - Frequency - DT1A02 - 48 - - - Max SubIndex - - 1 - - - - Frequency - - #x60020020 - - - - - ro - - - - #x1A03 - ActualPosition1 - DT1A03 - 48 - - - Max SubIndex - - 1 - - - - ActualPosition1 - - #x60030020 - - - - - ro - - - - #x1A04 - ActualPosition2 - DT1A04 - 48 - - - Max SubIndex - - 1 - - - - ActualPosition2 - - #x60040020 - - - - - ro - - - - #x1A05 - ActualPosition3 - DT1A05 - 48 - - - Max SubIndex - - 1 - - - - ActualPosition3 - - #x60050020 - - - - - ro - - - - #x1A06 - ActualPosition4 - DT1A06 - 48 - - - Max SubIndex - - 1 - - - - ActualPosition4 - - #x60060020 - - - - - ro - - - - #x1C00 - Sync Manager Communication Type - DT1C00 - 48 - - - Max SubIndex - - 4 - - - - Communications Type SM0 - - 1 - - - - Communications Type SM1 - - 2 - - - - Communications Type SM2 - - 3 - - - - Communications Type SM3 - - 4 - - - - - ro - - - - #x1C12 - Sync Manager 2 PDO Assignment - DT1C12 - 176 - - - Max SubIndex - - 10 - - - - PDO Mapping - - #x1600 - - - - PDO Mapping - - #x1601 - - - - PDO Mapping - - #x1602 - - - - PDO Mapping - - #x1603 - - - - PDO Mapping - - #x1604 - - - - PDO Mapping - - #x1605 - - - - PDO Mapping - - #x1606 - - - - PDO Mapping - - #x1607 - - - - PDO Mapping - - #x1608 - - - - PDO Mapping - - #x1609 - - - - - ro - - - - #x1C13 - Sync Manager 3 PDO Assignment - DT1C13 - 128 - - - Max SubIndex - - 7 - - - - PDO Mapping - - #x1A00 - - - - PDO Mapping - - #x1A01 - - - - PDO Mapping - - #x1A02 - - - - PDO Mapping - - #x1A03 - - - - PDO Mapping - - #x1A04 - - - - PDO Mapping - - #x1A05 - - - - PDO Mapping - - #x1A06 - - - - - ro - - - - #x6000 - Velocity - REAL - 32 - - 0 - - - ro - T - - - - #x6001 - Inputs - DT6001 - 80 - - - Max SubIndex - - 8 - - - - IN1 - - 0 - - - - IN2 - - 0 - - - - IN3 - - 0 - - - - IN4 - - 0 - - - - IN5 - - 0 - - - - IN6 - - 0 - - - - IN7 - - 0 - - - - IN8 - - 0 - - - - - ro - - - - #x6002 - Frequency - REAL - 32 - - 0 - - - ro - T - - - - #x6003 - ActualPosition1 - REAL - 32 - - 0 - - - ro - T - - - - #x6004 - ActualPosition2 - REAL - 32 - - 0 - - - ro - T - - - - #x6005 - ActualPosition3 - REAL - 32 - - 0 - - - ro - T - - - - #x6006 - ActualPosition4 - REAL - 32 - - 0 - - - ro - T - - - - #x7000 - Scale - REAL - 32 - - 0 - - - ro - R - - - - #x7001 - Outputs - DT7001 - 48 - - - Max SubIndex - - 4 - - - - OUT1 - - 0 - - - - OUT2 - - 0 - - - - OUT3 - - 0 - - - - OUT4 - - 0 - - - - - ro - - - - #x7002 - CommandedPosition1 - REAL - 32 - - 0 - - - ro - R - - - - #x7003 - CommandedPosition2 - REAL - 32 - - 0 - - - ro - R - - - - #x7004 - CommandedPosition3 - REAL - 32 - - 0 - - - ro - R - - - - #x7005 - CommandedPosition4 - REAL - 32 - - 0 - - - ro - R - - - - #x7006 - StepsPerMM1 - DINT - 32 - - 0 - - - ro - R - - - - #x7007 - StepsPerMM2 - DINT - 32 - - 0 - - - ro - R - - - - #x7008 - StepsPerMM3 - DINT - 32 - - 0 - - - ro - R - - - - #x7009 - StepsPerMM4 - DINT - 32 - - 0 - - - ro - R - - - - - - Outputs - Inputs - MBoxState - MBoxOut - MBoxIn - Outputs - Inputs - - #x1600 - Scale - - #x7000 - #x0 - 32 - Scale - REAL - - - - #x1601 - Outputs - - #x7001 - #x1 - 8 - OUT1 - USINT - - - #x7001 - #x2 - 8 - OUT2 - USINT - - - #x7001 - #x3 - 8 - OUT3 - USINT - - - #x7001 - #x4 - 8 - OUT4 - USINT - - - - #x1602 - CommandedPosition1 - - #x7002 - #x0 - 32 - CommandedPosition1 - REAL - - - - #x1603 - CommandedPosition2 - - #x7003 - #x0 - 32 - CommandedPosition2 - REAL - - - - #x1604 - CommandedPosition3 - - #x7004 - #x0 - 32 - CommandedPosition3 - REAL - - - - #x1605 - CommandedPosition4 - - #x7005 - #x0 - 32 - CommandedPosition4 - REAL - - - - #x1606 - StepsPerMM1 - - #x7006 - #x0 - 32 - StepsPerMM1 - DINT - - - - #x1607 - StepsPerMM2 - - #x7007 - #x0 - 32 - StepsPerMM2 - DINT - - - - #x1608 - StepsPerMM3 - - #x7008 - #x0 - 32 - StepsPerMM3 - DINT - - - - #x1609 - StepsPerMM4 - - #x7009 - #x0 - 32 - StepsPerMM4 - DINT - - - - #x1A00 - Velocity - - #x6000 - #x0 - 32 - Velocity - REAL - - - - #x1A01 - Inputs - - #x6001 - #x1 - 8 - IN1 - USINT - - - #x6001 - #x2 - 8 - IN2 - USINT - - - #x6001 - #x3 - 8 - IN3 - USINT - - - #x6001 - #x4 - 8 - IN4 - USINT - - - #x6001 - #x5 - 8 - IN5 - USINT - - - #x6001 - #x6 - 8 - IN6 - USINT - - - #x6001 - #x7 - 8 - IN7 - USINT - - - #x6001 - #x8 - 8 - IN8 - USINT - - - - #x1A02 - Frequency - - #x6002 - #x0 - 32 - Frequency - REAL - - - - #x1A03 - ActualPosition1 - - #x6003 - #x0 - 32 - ActualPosition1 - REAL - - - - #x1A04 - ActualPosition2 - - #x6004 - #x0 - 32 - ActualPosition2 - REAL - - - - #x1A05 - ActualPosition3 - - #x6005 - #x0 - 32 - ActualPosition3 - REAL - - - - #x1A06 - ActualPosition4 - - #x6006 - #x0 - 32 - ActualPosition4 - REAL - - - - - - - - SM-Synchron - SM-Synchron - #x000 - - - DC - DC-Synchron - #x300 - - - - 2048 - 050603446400000000001A000000 - - - - - \ No newline at end of file diff --git a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/ecat_options.h b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/ecat_options.h index 75887fa..2c142f3 100755 --- a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/ecat_options.h +++ b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/ecat_options.h @@ -33,8 +33,8 @@ #define SM3_smc 0x20 #define SM3_act 1 -#define MAX_MAPPINGS_SM2 13 -#define MAX_MAPPINGS_SM3 14 +#define MAX_MAPPINGS_SM2 18 +#define MAX_MAPPINGS_SM3 7 #define MAX_RXPDO_SIZE 512 #define MAX_TXPDO_SIZE 512 diff --git a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/eeprom.bin b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/lib/soes-esi/eeprom.bin index 926f832298060168f620c02da1fa2b8c47f490b8..e5685d6301013924c63d17d1e8d8b3f6ab7a8c99 100755 GIT binary patch delta 90 zcmZn=Xb_NPWn*?pVE_RsAogRpaB<%S1_K5LCLoV-qNB(}!$3881{Q8-KWByH{JfH) l{2V^t#N>?3yi{ihm(A1H-PtKpYT`zzjZb9QCkHSp0RWA@7nuM6 delta 125 zcmZn=Xb_NPWn*?pVE_RsAogQe#l^s|?#?L&Mj(%IqNB(}!$1iG1{RSJ4`)Xgg`m{L zl++@H#0pajLj!|}6U9u7eN#&kb9_sSGxO4m6stepgen_array[0].pos_scale = -Obj.StepsPerMM1; // Scale perhaps changed - Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2; - Step->stepgen_array[2].pos_scale = -Obj.StepsPerMM3; - Step->stepgen_array[3].pos_scale = -Obj.StepsPerMM4; - posCmd1 = Obj.CommandedPosition1; // The position update - posCmd2 = Obj.CommandedPosition2; + posScale1 = Obj.StepsPerMM1; // Scale perhaps changed + posScale2 = Obj.StepsPerMM2; + posScale3 = Obj.StepsPerMM3; + posScale4 = Obj.StepsPerMM4; + posCmd1 = Obj.CommandedPosition1; // The position update, etc + posCmd2 = Obj.CommandedPosition2; // Is recognised by the base Time loop posCmd3 = Obj.CommandedPosition3; posCmd4 = Obj.CommandedPosition4; + maxAcc1 = Obj.MaxAcceleration1; + maxAcc2 = Obj.MaxAcceleration2; + maxAcc3 = Obj.MaxAcceleration3; + maxAcc4 = Obj.MaxAcceleration4; + enable1 = Obj.Enable1; + enable2 = Obj.Enable2; + enable3 = Obj.Enable3; + enable4 = Obj.Enable4; + if (Obj.BasePeriod != 0) // Use default value from setup() if not set by SDO. + basePeriod = Obj.BasePeriod; } void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation @@ -97,8 +114,11 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation scale = Obj.Scale; Obj.Velocity = scale * sin(ESCvar.Time * 1e-8 * 6.28); // Test - for (int i = 0; i < min(sizeof(Obj.Inputs), sizeof(INPUTS)); i++) - Obj.Inputs[i] = digitalRead(INPUTS[i]) == HIGH ? 1 : 0; + for (int i = 0; i < 8; i++) + if (digitalRead(INPUTS[i]) == HIGH) + bitSet(Obj.Input8, i); + else + bitClear(Obj.Input8, i); #if 0 Obj.IndexStatus = Encoder1.indexHappened(); Obj.EncPos = Encoder1.currentPos(); @@ -157,12 +177,12 @@ void setup(void) Serial1.begin(115200); delay(1000); // To make terminal window ready - for (int i = 0; i < min(sizeof(Obj.Outputs), sizeof(OUTPUTS)); i++) + for (int i = 0; i < 4; i++) { pinMode(OUTPUTS[i], OUTPUT); digitalWrite(OUTPUTS[i], LOW); } - for (int i = 0; i < min(sizeof(Obj.Inputs), sizeof(INPUTS)); i++) + for (int i = 0; i < 8; i++) pinMode(INPUTS[i], INPUT); pinMode(DAC1_pin, OUTPUT); analogWrite(DAC1_pin, 0); @@ -177,6 +197,8 @@ void setup(void) pinMode(PE5, OUTPUT); // Step 4 pinMode(PE4, OUTPUT); // Dir 4 + basePeriod = BASE_PERIOD; // Random-ish number, but it should work + baseTimer = new HardwareTimer(TIM11); // The base period timer baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); // Or the line above, This one is uncalibrated baseTimer->attachInterrupt(basePeriodCB); @@ -319,10 +341,14 @@ void Rollover_IT_callback(void) void updateStepperGenerators(void) { baseTimer->pause(); - Step->updateStepGen(posCmd1, posCmd2, posCmd3, posCmd4, sync0CycleTime); // Update positions - Step->makeAllPulses(); // Make first step right here - basePeriodCnt = sync0CycleTime / BASE_PERIOD; // - baseTimer->refresh(); // + Step->updateStepGen(posCmd1, posCmd2, posCmd3, posCmd4, + posScale1, posScale2, posScale3, posScale4, + maxAcc1, maxAcc2, maxAcc3, maxAcc4, + enable1, enable2, enable3, enable4, + sync0CycleTime); // Update positions + Step->makeAllPulses(); // Make first step right here + basePeriodCnt = sync0CycleTime / basePeriod; // + baseTimer->refresh(); // baseTimer->resume(); // Make the other steps in baseTimer's ISR }