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

Ny structure. Start of the "main" branch

Browse Source
This commit is contained in:
Hakan Bastedt
2024-11-20 11:18:13 +01:00
parent 31b896871d
commit 1918604586
415 changed files with 202039 additions and 21080 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/MyEncoder.cpp Executable file
View File

@@ -0,0 +1,99 @@
#include "MyENcoder.h"
MyEncoder::MyEncoder(TIM_TypeDef *_tim_base, uint8_t _indexPin, void irq(void))
{
tim_base = _tim_base;
indexPin = _indexPin;
attachInterrupt(digitalPinToInterrupt(indexPin), irq, RISING); // When Index triggered
EncoderInit.SetCount(0);
}
#define ONE_PERIOD 65536
#define HALF_PERIOD 32768
int64_t MyEncoder::unwrapEncoder(uint16_t in)
{
int32_t c32 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int32_t dif = (c32 - previousEncoderCounterValue); // core concept: prev + (current - prev) = current
// wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
int32_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
if (dif < -HALF_PERIOD)
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int64_t unwrapped = previousEncoderCounterValue + mod_dif;
previousEncoderCounterValue = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}
void MyEncoder::indexPulse(void)
{
if (pleaseZeroTheCounter)
{
tim_base->CNT = 0;
indexPulseFired = 1;
Pos.clear();
TDelta.clear();
pleaseZeroTheCounter = 0;
}
}
uint8_t MyEncoder::indexHappened()
{
if (indexPulseFired)
{
indexPulseFired = 0;
previousEncoderCounterValue = 0;
return 1;
}
return 0;
}
double MyEncoder::currentPos()
{
curPos = unwrapEncoder(tim_base->CNT) * PosScaleRes;
return curPos;
}
double MyEncoder::frequency(uint64_t time)
{
double diffT = 0;
double diffPos = 0;
double frequency;
TDelta.push(time); // Running average over the length of the circular buffer
Pos.push(curPos);
if (Pos.size() == RINGBUFFERLEN)
{
diffT = 1.0e-6 * (TDelta.last() - TDelta.first()); // Time is in microseconds
diffPos = fabs(Pos.last() - Pos.first());
frequency = diffPos / diffT;
oldFrequency = frequency;
return frequency; // Revolutions per second
}
else
return oldFrequency;
}
uint8_t MyEncoder::getIndexState()
{
return digitalRead(indexPin);
}
void MyEncoder::setScale(double scale)
{
if (CurPosScale != scale && scale != 0)
{
CurPosScale = scale;
PosScaleRes = 1.0 / double(scale);
}
}
void MyEncoder::setLatch(uint8_t latchEnable)
{
if (latchEnable && !oldLatchCEnable) // Should only happen first time IndexCEnable is set
{
pleaseZeroTheCounter = 1;
}
oldLatchCEnable = latchEnable;
}

1217
Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/StepGen3.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

391
Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/Stm32F4_Encoder.cpp Executable file
View File

@@ -0,0 +1,391 @@
#include <Stm32F4_Encoder.h>
/*
Stm32F4_Encoder.cpp
Created on: Nov 20, 2020
Author: GoktugH.
*/
// TIM2, TIM3, TIM4, TIM8
Encoder::Encoder()
{
int unit;
}
// void Encoder::SetCount(enum EncTimer enc, int64_t Counter)
void Encoder::SetCount(int64_t Counter)
{
tim_base->CNT = Counter;
}
// uint16_t Encoder::GetCount(enum EncTimer enc)
uint16_t Encoder::GetCount()
{
return tim_base->CNT;
}
void GpioConfigPortA(GPIO_TypeDef *GPIOx)
{
uint32_t pinpos = 0x00, pos = 0x00, currentpin = 0x00;
/* ------------------------- Configure the port pins ---------------- */
/*-- GPIO Mode Configuration --*/
for (pinpos = 0x00; pinpos < 0x10; pinpos++)
{
pos = ((uint32_t)0x01) << pinpos;
/* Get the port pins position */
currentpin = (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_6 | GPIO_Pin_7) & pos;
if (currentpin == pos)
{
GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2));
GPIOx->MODER |= (((uint32_t)GPIO_Mode_AF) << (pinpos * 2));
if ((GPIO_Mode_AF == GPIO_Mode_OUT) || (GPIO_Mode_AF == GPIO_Mode_AF))
{
/* Check Speed mode parameters */
/* Speed mode configuration */
GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2));
GPIOx->OSPEEDR |= ((uint32_t)(GPIO_Speed_50MHz) << (pinpos * 2));
/* Check Output mode parameters */
/* Output mode configuration*/
GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos));
GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_OType_PP) << ((uint16_t)pinpos));
}
/* Pull-up Pull down resistor configuration*/
GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2));
GPIOx->PUPDR |= (((uint32_t)GPIO_PuPd_NOPULL) << (pinpos * 2));
}
}
}
void GpioConfigPortC(GPIO_TypeDef *GPIOx)
{
uint32_t pinpos = 0x00, pos = 0x00, currentpin = 0x00;
/* ------------------------- Configure the port pins ---------------- */
/*-- GPIO Mode Configuration --*/
for (pinpos = 0x00; pinpos < 0x10; pinpos++)
{
pos = ((uint32_t)0x01) << pinpos;
/* Get the port pins position */
currentpin = (GPIO_Pin_6 | GPIO_Pin_7) & pos;
if (currentpin == pos)
{
GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2));
GPIOx->MODER |= (((uint32_t)GPIO_Mode_AF) << (pinpos * 2));
if ((GPIO_Mode_AF == GPIO_Mode_OUT) || (GPIO_Mode_AF == GPIO_Mode_AF))
{
/* Check Speed mode parameters */
/* Speed mode configuration */
GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2));
GPIOx->OSPEEDR |= ((uint32_t)(GPIO_Speed_50MHz) << (pinpos * 2));
/* Check Output mode parameters */
/* Output mode configuration*/
GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos));
GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_OType_PP) << ((uint16_t)pinpos));
}
/* Pull-up Pull down resistor configuration*/
GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2));
GPIOx->PUPDR |= (((uint32_t)GPIO_PuPd_NOPULL) << (pinpos * 2));
}
}
}
void GpioConfigPortD(GPIO_TypeDef *GPIOx)
{
uint32_t pinpos = 0x00, pos = 0x00, currentpin = 0x00;
/* ------------------------- Configure the port pins ---------------- */
/*-- GPIO Mode Configuration --*/
for (pinpos = 0x00; pinpos < 0x10; pinpos++)
{
pos = ((uint32_t)0x01) << pinpos;
/* Get the port pins position */
currentpin = (GPIO_Pin_12 | GPIO_Pin_13) & pos;
if (currentpin == pos)
{
GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2));
GPIOx->MODER |= (((uint32_t)GPIO_Mode_AF) << (pinpos * 2));
if ((GPIO_Mode_AF == GPIO_Mode_OUT) || (GPIO_Mode_AF == GPIO_Mode_AF))
{
/* Check Speed mode parameters */
/* Speed mode configuration */
GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2));
GPIOx->OSPEEDR |= ((uint32_t)(GPIO_Speed_50MHz) << (pinpos * 2));
/* Check Output mode parameters */
/* Output mode configuration*/
GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos));
GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_OType_PP) << ((uint16_t)pinpos));
}
/* Pull-up Pull down resistor configuration*/
GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2));
GPIOx->PUPDR |= (((uint32_t)GPIO_PuPd_NOPULL) << (pinpos * 2));
}
}
}
void TIM_EncoderInterConfig(TIM_TypeDef *TIMx, uint16_t TIM_EncoderMode, uint16_t TIM_IC1Polarity, uint16_t TIM_IC2Polarity)
{
uint16_t tmpsmcr = 0;
uint16_t tmpccmr1 = 0;
uint16_t tmpccer = 0;
/* Get the TIMx SMCR register value */
tmpsmcr = TIMx->SMCR;
/* Get the TIMx CCMR1 register value */
tmpccmr1 = TIMx->CCMR1;
/* Get the TIMx CCER register value */
tmpccer = TIMx->CCER;
/* Set the encoder Mode */
tmpsmcr &= (uint16_t)~TIM_SMCR_SMS;
tmpsmcr |= TIM_EncoderMode;
/* Select the Capture Compare 1 and the Capture Compare 2 as input */
tmpccmr1 &= ((uint16_t)~TIM_CCMR1_CC1S) & ((uint16_t)~TIM_CCMR1_CC2S);
tmpccmr1 |= TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0;
/* Set the TI1 and the TI2 Polarities */
tmpccer &= ((uint16_t)~TIM_CCER_CC1P) & ((uint16_t)~TIM_CCER_CC2P);
tmpccer |= (uint16_t)(TIM_IC1Polarity | (uint16_t)(TIM_IC2Polarity << (uint16_t)4));
/* Write to TIMx SMCR */
TIMx->SMCR = tmpsmcr;
/* Write to TIMx CCMR1 */
TIMx->CCMR1 = tmpccmr1;
/* Write to TIMx CCER */
TIMx->CCER = tmpccer;
}
void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef *TIM_TimeBaseInitStruct)
{
/* Set the default configuration */
TIM_TimeBaseInitStruct->TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseInitStruct->TIM_Prescaler = 0x0000;
TIM_TimeBaseInitStruct->TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStruct->TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStruct->TIM_RepetitionCounter = 0x0000;
}
void TIM_TimeBaseInit(TIM_TypeDef *TIMx, TIM_TimeBaseInitTypeDef *TIM_TimeBaseInitStruct)
{
uint16_t tmpcr1 = 0;
tmpcr1 = TIMx->CR1;
if ((TIMx == TIM1) || (TIMx == TIM8) ||
(TIMx == TIM2) || (TIMx == TIM3) ||
(TIMx == TIM4) || (TIMx == TIM5))
{
/* Select the Counter Mode */
tmpcr1 &= (uint16_t)(~(TIM_CR1_DIR | TIM_CR1_CMS));
tmpcr1 |= (uint32_t)TIM_TimeBaseInitStruct->TIM_CounterMode;
}
if ((TIMx != TIM6) && (TIMx != TIM7))
{
/* Set the clock division */
tmpcr1 &= (uint16_t)(~TIM_CR1_CKD);
tmpcr1 |= (uint32_t)TIM_TimeBaseInitStruct->TIM_ClockDivision;
}
TIMx->CR1 = tmpcr1;
/* Set the Autoreload value */
TIMx->ARR = TIM_TimeBaseInitStruct->TIM_Period;
/* Set the Prescaler value */
TIMx->PSC = TIM_TimeBaseInitStruct->TIM_Prescaler;
if ((TIMx == TIM1) || (TIMx == TIM8))
{
/* Set the Repetition Counter value */
TIMx->RCR = TIM_TimeBaseInitStruct->TIM_RepetitionCounter;
}
/* Generate an update event to reload the Prescaler
and the repetition counter(only for TIM1 and TIM8) value immediatly */
TIMx->EGR = TIM_PSCReloadMode_Immediate;
}
TIM_TimeBaseInitTypeDef TIMER_InitStructure;
TIM_TimeBaseInitTypeDef TIMER_InitStructureE;
TIM_TimeBaseInitTypeDef TIMER_InitStructureEE;
TIM_TimeBaseInitTypeDef TIMER_InitStructureEEG;
void TIM_Cmd(TIM_TypeDef *TIMx, FunctionalState NewState)
{
if (NewState != DISABLE)
{
/* Enable the TIM Counter */
TIMx->CR1 |= TIM_CR1_CEN;
}
else
{
/* Disable the TIM Counter */
TIMx->CR1 &= (uint16_t)~TIM_CR1_CEN;
}
}
void GPIO_PinAF(GPIO_TypeDef *GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF)
{
uint32_t temp = 0x00;
uint32_t temp_2 = 0x00;
temp = ((uint32_t)(GPIO_AF) << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4));
GPIOx->AFR[GPIO_PinSource >> 0x03] &= ~((uint32_t)0xF << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4));
temp_2 = GPIOx->AFR[GPIO_PinSource >> 0x03] | temp;
GPIOx->AFR[GPIO_PinSource >> 0x03] = temp_2;
}
void encoder_config()
{
RCC->AHB1ENR |= 0x1; // GPIOA
RCC->AHB1ENR |= 0x4; // GPIOC
RCC->AHB1ENR |= 0x8; // GPIOD
RCC->AHB1ENR |= 0x10; // GPIOE
RCC->APB1ENR |= 0x20000000; // ENABLE DAC
// RCC->APB2ENR |= 0x00000002; // APB2 TIM8
RCC->APB1ENR |= 0x00000004; // APB1 TIM4
RCC->APB1ENR |= 0x00000001; // APB1 TIM2
// RCC->APB1ENR |= 0x00000002; // APB1 TIM3
GpioConfigPortA(GPIOA);
// GpioConfigPortC(GPIOC);
GpioConfigPortD(GPIOD);
#if 0 // Skipping since TIM8 is step generator and TIM3, chan4 is smae as TIM8, chan4
GPIO_PinAF(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);
GPIO_PinAF(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3);
GPIO_PinAF(GPIOC, GPIO_PinSource6, GPIO_AF_TIM8);
GPIO_PinAF(GPIOC, GPIO_PinSource7, GPIO_AF_TIM8);
#endif
GPIO_PinAF(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);
GPIO_PinAF(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);
GPIO_PinAF(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
GPIO_PinAF(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
#if 0 // Skipping since I use TIM8 as stepper generator
TIM_EncoderInterConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIMER_InitStructure.TIM_Period = 65535;
TIMER_InitStructure.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
TIM_TimeBaseInit(TIM8, &TIMER_InitStructure);
TIM_TimeBaseStructInit(&TIMER_InitStructure);
TIM_Cmd(TIM8, ENABLE);
TIM8->CNT = 0;
#endif
TIM_EncoderInterConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIMER_InitStructureE.TIM_Period = 65535;
TIMER_InitStructureE.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
TIM_TimeBaseInit(TIM4, &TIMER_InitStructureE);
TIM_TimeBaseStructInit(&TIMER_InitStructureE);
TIM_Cmd(TIM4, ENABLE);
TIM4->CNT = 0;
TIM_EncoderInterConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIMER_InitStructureEE.TIM_Period = 65535;
TIMER_InitStructureEE.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
TIM_TimeBaseInit(TIM2, &TIMER_InitStructureEE);
TIM_TimeBaseStructInit(&TIMER_InitStructureEE);
TIM_Cmd(TIM2, ENABLE);
TIM2->CNT = 0;
#if 0
TIM_EncoderInterConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIMER_InitStructureEEG.TIM_Period = 65535;
TIMER_InitStructureEEG.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
TIM_TimeBaseInit(TIM3, &TIMER_InitStructureEEG);
TIM_TimeBaseStructInit(&TIMER_InitStructureEEG);
TIM_Cmd(TIM3, ENABLE);
TIM3->CNT = 0;
#endif
}
void encoder2_config()
{
#if 0
#include "mbed.h"
#include "stm32f4xx.h"
#include "stm32f4xx_hal_tim_ex.h"
TIM_HandleTypeDef timer;
TIM_Encoder_InitTypeDef encoder;
// direction to PA_9 -- step pulse to PA_8
int main()
{
GPIO_InitTypeDef GPIO_InitStruct;
__TIM1_CLK_ENABLE();
__GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
timer.Instance = TIM1;
timer.Init.Period = 0xffff;
timer.Init.Prescaler = 0;
timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
timer.Init.CounterMode = TIM_COUNTERMODE_UP;
encoder.EncoderMode = TIM_ENCODERMODE_TI12;
encoder.IC1Filter = 0x0f;
encoder.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
encoder.IC1Prescaler = TIM_ICPSC_DIV4;
encoder.IC1Selection = TIM_ICSELECTION_DIRECTTI;
encoder.IC2Filter = 0x0f;
encoder.IC2Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
encoder.IC2Prescaler = TIM_ICPSC_DIV4;
encoder.IC2Selection = TIM_ICSELECTION_DIRECTTI;
HAL_TIM_Encoder_Init(&timer, &encoder);
HAL_TIM_Encoder_Start(&timer,TIM_CHANNEL_1);
TIM1->EGR = 1; // Generate an update event
TIM1->CR1 = 1; // Enable the counter
while (1) {
int16_t count1;
count1=TIM1->CNT;
printf("%d\r\n", count1);
wait(1.0);
};
}
#endif
}

18
Cards/EaserCAT-3000-Digital-Stepper-Analog-Encoder-Frequency/Firmware/src/extend32to64.cpp Executable file
View File

@@ -0,0 +1,18 @@
#include "extend32to64.h"
// Extend from 32-bit to 64-bit precision
int64_t extend32to64::extendTime(uint32_t in)
{
int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current
// wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
if (dif < int64_t(-HALF_PERIOD))
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int64_t unwrapped = previousTimeValue + mod_dif;
previousTimeValue = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}

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

@@ -0,0 +1,334 @@
#include <Arduino.h>
#include <stdio.h>
extern "C"
{
#include "ecat_slv.h"
#include "utypes.h"
};
_Objects Obj;
#include "extend32to64.h"
extend32to64 longTime;
volatile uint64_t irqTime = 0;
HardwareSerial Serial1(PA10, PA9);
////// Digital IO
const byte INPUTS[8] = {PE8, PE9, PE10, PE11, PE12, PE13, PE14, PE15};
const byte OUTPUTS[4] = {PC5, PB0, PB1, PE7};
const byte THCAD_PIN = PA2; // CAN BE PA0, should be PA0
// PA2 is connected to Timer 2, a 32-bit timer
///// Analog out
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 )
volatile double posCmd1, posCmd2, posCmd3, posCmd4;
volatile uint16_t basePeriodCnt;
volatile uint16_t deltaMakePulsesCnt;
volatile uint64_t makePulsesCnt = 0;
void updateStepperGenerators(void);
void basePeriodCB(void);
///////// Spindle Encoder
#include "MyEncoder.h"
volatile uint16_t encCnt = 0;
void indexPulseEncoderCB1(void);
MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1);
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
////// #define MAX_RELOAD 0xFFFFFFFF
////// HardwareTimer.cpp is part of the Stm32duino code <add where to find that>
uint32_t channel;
volatile uint32_t FrequencyMeasured, LastCapture = 0, CurrentCapture;
uint32_t input_freq = 0;
volatile uint32_t rolloverCompareCount = 0;
HardwareTimer *FrequencyTimer;
void InputCapture_IT_callback(void);
void Rollover_IT_callback(void);
////// EtherCAT
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
////// EtherCAT routines called regularly, to read data, do stuff and write data
void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation
{
for (int i = 0; i < min(sizeof(Obj.Outputs), sizeof(OUTPUTS)); i++)
digitalWrite(OUTPUTS[i], Obj.Outputs[i] == 1 ? HIGH : LOW);
// analogWrite(DAC1_pin, Obj.Voltage);
// Encoder1.setLatch(Obj.IndexLatchEnable);
// Encoder1.setScale(2000);
Step->stepgen_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;
posCmd3 = Obj.CommandedPosition3;
posCmd4 = Obj.CommandedPosition4;
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{
Obj.Velocity = Obj.Scale * FrequencyMeasured;
float scale = 1;
if (Obj.Scale != 0.0)
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;
#if 0
Obj.IndexStatus = Encoder1.indexHappened();
Obj.EncPos = Encoder1.currentPos();
Obj.EncFrequency = Encoder1.frequency(longTime.extendTime(micros()));
Obj.IndexByte = Encoder1.getIndexState();
Obj.Velocity = Obj.Scale * FrequencyMeasured;
#endif
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
{
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);
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,
};
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++)
{
pinMode(OUTPUTS[i], OUTPUT);
digitalWrite(OUTPUTS[i], LOW);
}
for (int i = 0; i < min(sizeof(Obj.Inputs), sizeof(INPUTS)); 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
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);
// Automatically retrieve TIM instance and channel associated to pin
// This is used to be compatible with all STM32 series automatically.
TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(digitalPinToPinName(THCAD_PIN), PinMap_PWM);
channel = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(THCAD_PIN), PinMap_PWM));
// FrequencyTimer = new HardwareTimer(Instance); // TIM2
FrequencyTimer = new HardwareTimer(TIM5); // TIM5
uint32_t PrescalerFactor = 1;
FrequencyTimer->setMode(channel, TIMER_INPUT_CAPTURE_RISING, THCAD_PIN);
FrequencyTimer->setPrescaleFactor(PrescalerFactor);
FrequencyTimer->setOverflow(0xFFFFFFF0); // Max Period value to have the largest possible time to detect rising edge and avoid timer rollover
FrequencyTimer->attachInterrupt(channel, InputCapture_IT_callback);
FrequencyTimer->attachInterrupt(Rollover_IT_callback);
FrequencyTimer->resume();
// Compute this scale factor only once
input_freq = FrequencyTimer->getTimerClkFreq() / FrequencyTimer->getPrescaleFactor();
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 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);
// 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);
}
}
// 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;
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;
}
// Test/debug routine for the INT line
void globalInt(void)
{
globalIRQ++;
}
////// Frequency counter (torch height) callback routines
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;
}
/* 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++;
if (rolloverCompareCount > 1)
{
FrequencyMeasured = 0;
}
}
///// Stepper generator callback routines
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(); //
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();
}