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

Indentation

Browse Source
This commit is contained in:
Hakan Bastedt
2025-10-21 21:53:51 +02:00
parent cd78577e4d
commit cf4bece469
1 changed files with 214 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files

435
Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/main.cpp
View File

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