diff --git a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/main.cpp b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/main.cpp index 14b3c7c..89712ea 100755 --- a/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/main.cpp +++ b/Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/main.cpp @@ -1,8 +1,7 @@ // EaserCAT 3000 #include #include -extern "C" -{ +extern "C" { #include "ecat_slv.h" #include "utypes.h" }; @@ -29,8 +28,8 @@ const byte DAC1_pin = PA4; //////// Stepper generators #include "StepGen3.h" StepGen3 *Step = 0; -HardwareTimer *baseTimer; // The base period timer -uint32_t sync0CycleTime; // nanosecs, often 1000000 ( 1 ms ) +HardwareTimer *baseTimer; // The base period timer +uint32_t sync0CycleTime; // nanosecs, often 1000000 ( 1 ms ) volatile double posCmd1, posCmd2, posCmd3, posCmd4; volatile float posScale1, posScale2, posScale3, posScale4; volatile float maxAcc1, maxAcc2, maxAcc3, maxAcc4; @@ -47,16 +46,17 @@ void basePeriodCB(void); volatile uint16_t encCnt = 0; void indexPulseEncoderCB1(void); MyEncoder Encoder1(TIM2, PA3, indexPulseEncoderCB1); -void indexPulseEncoderCB1(void) -{ - encCnt++; - Encoder1.indexPulse(); +void indexPulseEncoderCB1(void) { + encCnt++; + Encoder1.indexPulse(); } ///////// Frequency counter for Torch height #include "HardwareTimer.h" -// NOTE This mod in the beginning (line 33) of HardwareTimer.cpp for 32-bit precision -////// //#define MAX_RELOAD ((1 << 16) - 1) // Currently even 32b timers are used as 16b to have generic behavior +// NOTE This mod in the beginning (line 33) of HardwareTimer.cpp for 32-bit +// precision +////// //#define MAX_RELOAD ((1 << 16) - 1) // Currently even 32b timers are +///used as 16b to have generic behavior ////// #define MAX_RELOAD 0xFFFFFFFF ////// HardwareTimer.cpp is part of the Stm32duino code @@ -72,76 +72,81 @@ void Rollover_IT_callback(void); const byte SYNC0 = PC3; const byte SYNC1 = PC1; const byte SINT = PC0; -volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt -volatile byte serveIRQ = 0; // Flag indicating we got a SYNCx pulse and should act on that -volatile uint32_t globalIRQ = 0; // Testing -extern "C" uint32_t ESC_SYNC0cycletime(void); // A SOES function we need -void globalInt(void); // ISR for INT line +volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt +volatile byte serveIRQ = + 0; // Flag indicating we got a SYNCx pulse and should act on that +volatile uint32_t globalIRQ = 0; // Testing +extern "C" uint32_t ESC_SYNC0cycletime(void); // A SOES function we need +void globalInt(void); // ISR for INT line ////// EtherCAT routines called regularly, to read data, do stuff and write data -void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation +void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation { - for (int i = 0; i < 4; i++) - digitalWrite(OUTPUTS[i], bitcheck(Obj.Output4, i) ? HIGH : LOW); - analogWrite(DAC1_pin, Obj.Voltage); - Encoder1.setLatch(Obj.EncoderOut.IndexLatchEnable); - Encoder1.setScale(Obj.EncoderOut.Scale); + for (int i = 0; i < 4; i++) + digitalWrite(OUTPUTS[i], bitcheck(Obj.Output4, i) ? HIGH : LOW); + analogWrite(DAC1_pin, Obj.Voltage); + Encoder1.setLatch(Obj.EncoderOut.IndexLatchEnable); + Encoder1.setScale(Obj.EncoderOut.Scale); - 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. - newBasePeriod = Obj.BasePeriod; + 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. + newBasePeriod = Obj.BasePeriod; } -void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation +void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation { float scale = 1; - if (Obj.EncoderOut.Scale != 0.0) - scale = Obj.EncoderOut.Scale; + if (Obj.EncoderOut.Scale != 0.0) scale = Obj.EncoderOut.Scale; - for (int i = 0; i < 8; i++) - if (digitalRead(INPUTS[i]) == HIGH) - bitSet(Obj.Input8, i); - else - bitClear(Obj.Input8, i); + for (int i = 0; i < 8; i++) + if (digitalRead(INPUTS[i]) == HIGH) + bitSet(Obj.Input8, i); + else + bitClear(Obj.Input8, i); - Obj.EncoderIn.IndexStatus = Encoder1.indexHappened(); - Obj.EncoderIn.Position = Encoder1.currentPos(); - Obj.EncoderIn.IndexByte = Encoder1.getIndexState(); - Obj.EncoderIn.Velocity = Obj.EncoderOut.Scale * Encoder1.frequency(longTime.extendTime(micros())); + Obj.EncoderIn.IndexStatus = Encoder1.indexHappened(); + Obj.EncoderIn.Position = Encoder1.currentPos(); + Obj.EncoderIn.IndexByte = Encoder1.getIndexState(); + Obj.EncoderIn.Velocity = + Obj.EncoderOut.Scale * Encoder1.frequency(longTime.extendTime(micros())); - Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb; - Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb; - Obj.ActualPosition3 = Step->stepgen_array[2].pos_fb; - Obj.ActualPosition4 = Step->stepgen_array[3].pos_fb; + Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb; + Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb; + Obj.ActualPosition3 = Step->stepgen_array[2].pos_fb; + Obj.ActualPosition4 = Step->stepgen_array[3].pos_fb; } -void handleStepper(void) // Called every cycle, updates stepper generator with new positions, - // restarts stepper generator and reads out current posution +void handleStepper( + void) // Called every cycle, updates stepper generator with new positions, + // restarts stepper generator and reads out current posution { - static int warned = 0; - if (!warned && sync0CycleTime == 0) // This is kludge to be used during testing to activate stepper during free run - // Stepper generators normally run only during synchronized conditions. But to do testing. - { - sync0CycleTime = 1000000; // 1e6 ns = 1e3 us = 1ms - Serial1.println("Warn sync0Cycletime"); - warned = 1; - } - updateStepperGenerators(); + static int warned = 0; + if (!warned && + sync0CycleTime == + 0) // This is kludge to be used during testing to activate stepper + // during free run Stepper generators normally run only during + // synchronized conditions. But to do testing. + { + sync0CycleTime = 1000000; // 1e6 ns = 1e3 us = 1ms + Serial1.println("Warn sync0Cycletime"); + warned = 1; + } + updateStepperGenerators(); } void ESC_interrupt_enable(uint32_t mask); @@ -149,198 +154,186 @@ void ESC_interrupt_disable(uint32_t mask); uint16_t dc_checker(void); void sync0Handler(void); -static esc_cfg_t config = - { - .user_arg = NULL, - .use_interrupt = 0, - .watchdog_cnt = 150, - .set_defaults_hook = NULL, - .pre_state_change_hook = NULL, - .post_state_change_hook = NULL, - .application_hook = handleStepper, - .safeoutput_override = NULL, - .pre_object_download_hook = NULL, - .post_object_download_hook = NULL, - .rxpdo_override = NULL, - .txpdo_override = NULL, - .esc_hw_interrupt_enable = ESC_interrupt_enable, - .esc_hw_interrupt_disable = ESC_interrupt_disable, - .esc_hw_eep_handler = NULL, - .esc_check_dc_handler = dc_checker, +static esc_cfg_t config = { + .user_arg = NULL, + .use_interrupt = 0, + .watchdog_cnt = 150, + .set_defaults_hook = NULL, + .pre_state_change_hook = NULL, + .post_state_change_hook = NULL, + .application_hook = handleStepper, + .safeoutput_override = NULL, + .pre_object_download_hook = NULL, + .post_object_download_hook = NULL, + .rxpdo_override = NULL, + .txpdo_override = NULL, + .esc_hw_interrupt_enable = ESC_interrupt_enable, + .esc_hw_interrupt_disable = ESC_interrupt_disable, + .esc_hw_eep_handler = NULL, + .esc_check_dc_handler = dc_checker, }; -void setup(void) -{ - Serial1.begin(115200); - delay(1000); // To make terminal window ready +void setup(void) { + Serial1.begin(115200); + delay(1000); // To make terminal window ready - for (int i = 0; i < 4; i++) - { - pinMode(OUTPUTS[i], OUTPUT); - digitalWrite(OUTPUTS[i], LOW); - } - for (int i = 0; i < 8; i++) - pinMode(INPUTS[i], INPUT); - pinMode(DAC1_pin, OUTPUT); - analogWrite(DAC1_pin, 0); + for (int i = 0; i < 4; i++) { + pinMode(OUTPUTS[i], OUTPUT); + digitalWrite(OUTPUTS[i], LOW); + } + for (int i = 0; i < 8; i++) pinMode(INPUTS[i], INPUT); + pinMode(DAC1_pin, OUTPUT); + analogWrite(DAC1_pin, 0); - Step = new StepGen3; // More settings in StepGen3.cpp and Stepgen3.h - pinMode(PA11, OUTPUT); // Step 1 - pinMode(PA12, OUTPUT); // Dir 1 - pinMode(PC9, OUTPUT); // Step 2 - pinMode(PC8, OUTPUT); // Dir 2 - pinMode(PD12, OUTPUT); // Step 3 - pinMode(PD11, OUTPUT); // Dir 3 - pinMode(PE5, OUTPUT); // Step 4 - pinMode(PE4, OUTPUT); // Dir 4 + Step = new StepGen3; // More settings in StepGen3.cpp and Stepgen3.h + pinMode(PA11, OUTPUT); // Step 1 + pinMode(PA12, OUTPUT); // Dir 1 + pinMode(PC9, OUTPUT); // Step 2 + pinMode(PC8, OUTPUT); // Dir 2 + pinMode(PD12, OUTPUT); // Step 3 + pinMode(PD11, OUTPUT); // Dir 3 + pinMode(PE5, OUTPUT); // Step 4 + pinMode(PE4, OUTPUT); // Dir 4 - basePeriod = newBasePeriod = BASE_PERIOD; // Random-ish number, but it should work. Change through sdos + basePeriod = newBasePeriod = BASE_PERIOD; // Random-ish number, but it should + // work. Change through sdos - 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); + 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); - encoder_config(); // Needed by encoder, possibly breaks some timers. + encoder_config(); // Needed by encoder, possibly breaks some timers. - ecat_slv_init(&config); - attachInterrupt(digitalPinToInterrupt(PC0), globalInt, RISING); // For testing, should go into Enable_interrupt later on + ecat_slv_init(&config); + attachInterrupt( + digitalPinToInterrupt(PC0), globalInt, + RISING); // For testing, should go into Enable_interrupt later on } -void loop(void) -{ - uint64_t dTime; - if (serveIRQ) - { - DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | - DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); - serveIRQ = 0; - ESCvar.PrevTime = ESCvar.Time; - ecat_slv_poll(); - } - dTime = longTime.extendTime(micros()) - irqTime; - if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt - ecat_slv(); +void loop(void) { + uint64_t dTime; + if (serveIRQ) { + DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | + DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); + serveIRQ = 0; + ESCvar.PrevTime = ESCvar.Time; + ecat_slv_poll(); + } + dTime = longTime.extendTime(micros()) - irqTime; + if (dTime > + 5000) // Don't run ecat_slv_poll when expecting to serve interrupt + ecat_slv(); } -void sync0Handler(void) -{ - ALEventIRQ = ESC_ALeventread(); - // if (ALEventIRQ & ESCREG_ALEVENT_SM2) - { - irqTime = longTime.extendTime(micros()); - serveIRQ = 1; - } +void sync0Handler(void) { + ALEventIRQ = ESC_ALeventread(); + // if (ALEventIRQ & ESCREG_ALEVENT_SM2) + { + irqTime = longTime.extendTime(micros()); + serveIRQ = 1; + } } // Enable SM2 interrupts -void ESC_interrupt_enable(uint32_t mask) -{ - // Enable interrupt for SYNC0 or SM2 or SM3 - uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; - if (mask & user_int_mask) - { - ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); - ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3)); - attachInterrupt(digitalPinToInterrupt(SYNC0), sync0Handler, RISING); +void ESC_interrupt_enable(uint32_t mask) { + // Enable interrupt for SYNC0 or SM2 or SM3 + uint32_t user_int_mask = + ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; + if (mask & user_int_mask) { + ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); + ESC_ALeventmaskwrite(ESC_ALeventmaskread() & + ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3)); + attachInterrupt(digitalPinToInterrupt(SYNC0), sync0Handler, RISING); - // Set LAN9252 interrupt pin driver as push-pull active high - uint32_t bits = 0x00000111; - ESC_write(0x54, &bits, 4); + // Set LAN9252 interrupt pin driver as push-pull active high + uint32_t bits = 0x00000111; + ESC_write(0x54, &bits, 4); - // Enable LAN9252 interrupt - bits = 0x00000001; - ESC_write(0x5c, &bits, 4); - } + // Enable LAN9252 interrupt + bits = 0x00000001; + ESC_write(0x5c, &bits, 4); + } } // Disable SM2 interrupts -void ESC_interrupt_disable(uint32_t mask) -{ - // Enable interrupt for SYNC0 or SM2 or SM3 - uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; +void ESC_interrupt_disable(uint32_t mask) { + // Enable interrupt for SYNC0 or SM2 or SM3 + uint32_t user_int_mask = + ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; - if (mask & user_int_mask) - { - // Disable interrupt from SYNC0 etc - ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask)); - detachInterrupt(digitalPinToInterrupt(SYNC0)); - // Disable LAN9252 interrupt - uint32_t bits = 0x00000000; - ESC_write(0x5c, &bits, 4); - } + if (mask & user_int_mask) { + // Disable interrupt from SYNC0 etc + ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask)); + detachInterrupt(digitalPinToInterrupt(SYNC0)); + // Disable LAN9252 interrupt + uint32_t bits = 0x00000000; + ESC_write(0x5c, &bits, 4); + } } // Setup of DC -uint16_t dc_checker(void) -{ - // Indicate we run DC - ESCvar.dcsync = 1; - sync0CycleTime = ESC_SYNC0cycletime(); // nanosecs - return 0; +uint16_t dc_checker(void) { + // Indicate we run DC + ESCvar.dcsync = 1; + sync0CycleTime = ESC_SYNC0cycletime(); // nanosecs + return 0; } // Test/debug routine for the INT line -void globalInt(void) -{ - globalIRQ++; -} +void globalInt(void) { globalIRQ++; } ////// Frequency counter (torch height) callback routines -void InputCapture_IT_callback(void) -{ - CurrentCapture = FrequencyTimer->getCaptureCompare(channel); +void InputCapture_IT_callback(void) { + CurrentCapture = FrequencyTimer->getCaptureCompare(channel); - /* frequency computation */ - if (CurrentCapture > LastCapture) - { - FrequencyMeasured = input_freq / (CurrentCapture - LastCapture); - } - else if (CurrentCapture <= LastCapture) - { - /* 0xFFFFFFFF is max overflow value */ - FrequencyMeasured = input_freq / (0xFFFFFFFF + CurrentCapture - LastCapture); - } - LastCapture = CurrentCapture; - rolloverCompareCount = 0; + /* frequency computation */ + if (CurrentCapture > LastCapture) { + FrequencyMeasured = input_freq / (CurrentCapture - LastCapture); + } else if (CurrentCapture <= LastCapture) { + /* 0xFFFFFFFF is max overflow value */ + FrequencyMeasured = + input_freq / (0xFFFFFFFF + CurrentCapture - LastCapture); + } + LastCapture = CurrentCapture; + rolloverCompareCount = 0; } /* In case of timer rollover, frequency is to low to be measured set value to 0 - To reduce minimum frequency, it is possible to increase prescaler. But this is at a cost of precision. */ -void Rollover_IT_callback(void) -{ - rolloverCompareCount++; + To reduce minimum frequency, it is possible to increase prescaler. But this + is at a cost of precision. */ +void Rollover_IT_callback(void) { + rolloverCompareCount++; - if (rolloverCompareCount > 1) - { - FrequencyMeasured = 0; - } + if (rolloverCompareCount > 1) { + FrequencyMeasured = 0; + } } ///// Stepper generator callback routines -void updateStepperGenerators(void) -{ - baseTimer->pause(); - 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 - if (newBasePeriod != basePeriod) // Changed via sdos - { - basePeriod = newBasePeriod; - baseTimer->setOverflow(basePeriod / 1000, MICROSEC_FORMAT); // update timer frequency - } - basePeriodCnt = sync0CycleTime / basePeriod; // - baseTimer->refresh(); // - baseTimer->resume(); - // Make the other steps in baseTimer's ISR +void updateStepperGenerators(void) { + baseTimer->pause(); + 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 + if (newBasePeriod != basePeriod) // Changed via sdos + { + basePeriod = newBasePeriod; + baseTimer->setOverflow(basePeriod / 1000, + MICROSEC_FORMAT); // update timer frequency + } + basePeriodCnt = sync0CycleTime / basePeriod; // + baseTimer->refresh(); // + baseTimer->resume(); + // Make the other steps in baseTimer's ISR } -void basePeriodCB(void) -{ - if (--basePeriodCnt > 0) // Stop - Step->makeAllPulses(); // Make steps and pulses here - else - baseTimer->pause(); +void basePeriodCB(void) { + if (--basePeriodCnt > 0) // Stop + Step->makeAllPulses(); // Make steps and pulses here + else + baseTimer->pause(); }