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-2000/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;
}

148
Cards/EaserCAT-2000/Firmware/src/StepGen.cpp Executable file
View File

@@ -0,0 +1,148 @@
#include <Arduino.h>
#include <stdio.h>
#include "StepGen.h"
StepGen::StepGen(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void))
{
timerIsRunning = 0;
timerStepPosition = 0;
timerStepDirection = 0;
timerStepPositionAtEnd = 0;
timerNewEndStepPosition = 0;
actualPosition = 0;
requestedPosition = 0;
stepsPerMM = 0;
enabled = 0;
dirPin = _dirPin;
stepPin = _stepPin;
timerChan = _timerChannel;
MyTim = new HardwareTimer(Timer);
MyTim->attachInterrupt(irq);
pinMode(dirPin, OUTPUT);
}
void StepGen::reqPos(double_t pos)
{
requestedPosition = pos;
}
double StepGen::reqPos()
{
return requestedPosition;
}
void StepGen::actPos(double pos)
{
actualPosition = pos;
}
double StepGen::actPos()
{
return actualPosition;
}
void StepGen::enable(uint8_t yes)
{
enabled = yes;
}
void StepGen::handleStepper(void)
{
if (!enabled)
return;
pwmCycleTime = StepGen::sync0CycleTime;
actPos(timerStepPosition / double(stepsPerMM));
double diffPosition = reqPos() - actPos();
#if 1
// Wild "tone" kludge. map() function
#define SPEED_MIN 0.00005
#define SPEED_MAX 0.0005
#define FACT_LOW 1.0
#define FACT_HIGH 20.0
if (abs(diffPosition) < SPEED_MIN) // 60 mm/min = 0.001 mm/ms
{
pwmCycleTime = FACT_LOW * StepGen::sync0CycleTime;
}
else if (abs(diffPosition) > SPEED_MAX) // 60 mm/min = 0.001 mm/ms
{
pwmCycleTime = FACT_HIGH * StepGen::sync0CycleTime;
}
else
{
pwmCycleTime = (FACT_LOW + (FACT_HIGH - FACT_LOW) * (abs(diffPosition) - SPEED_MIN) / (SPEED_MAX - SPEED_MIN)) * StepGen::sync0CycleTime;
}
#endif
uint64_t fre = (abs(diffPosition) * stepsPerMM * 1000000) / pwmCycleTime; // Frequency needed
if (fre > maxFreq) // Only do maxFre
{
double maxDist = (maxFreq * pwmCycleTime) / (stepsPerMM * 1000000.0) * (diffPosition > 0 ? 1 : -1);
reqPos(actPos() + maxDist);
}
int32_t pulsesAtEndOfCycle = stepsPerMM * reqPos();
// Will be picked up by the timer_CB and the timer is reloaded, if it runs.
timerNewEndStepPosition = pulsesAtEndOfCycle;
if (!timerIsRunning) // Timer isn't running. Start it here
{
int32_t steps = pulsesAtEndOfCycle - timerStepPosition; // Pulses to go + or -
if (steps != 0)
{
if (steps > 0)
{
digitalWrite(dirPin, HIGH);
timerStepDirection = 1;
}
else
{
digitalWrite(dirPin, LOW);
timerStepDirection = -1;
}
timerStepPositionAtEnd = pulsesAtEndOfCycle; // Current Position
float_t freqf = abs(steps) / (pwmCycleTime*1.0e-6);
uint32_t freq = uint32_t(freqf);
MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
MyTim->setOverflow(freq, HERTZ_FORMAT);
MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
timerIsRunning = 1;
MyTim->resume();
}
}
}
void StepGen::timerCB()
{
timerStepPosition += timerStepDirection; // The step that was just completed
if (timerNewEndStepPosition != 0) // Are we going to reload?
{
// Input for reload is timerNewEndStepPosition
// The timer has current position and from this
// can set new frequency and new endtarget for steps
MyTim->pause(); // We are not at stop, let's stop it. Note stepPin is floating
int32_t steps = timerNewEndStepPosition - timerStepPosition;
if (steps != 0)
{
uint8_t sgn = steps > 0 ? HIGH : LOW;
digitalWrite(dirPin, sgn);
float_t freqf = abs(steps) / float(pwmCycleTime*1.0e-6);
uint32_t freq = uint32_t(freqf);
timerStepDirection = steps > 0 ? 1 : -1;
timerStepPositionAtEnd = timerNewEndStepPosition;
timerNewEndStepPosition = 0; // Set to zero to not reload next time
MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
MyTim->setOverflow(freq, HERTZ_FORMAT);
MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
MyTim->resume();
timerIsRunning = 1;
}
}
if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
{
timerIsRunning = 0;
MyTim->pause();
}
}
void StepGen::setScale(int16_t spm)
{
stepsPerMM = spm;
}
uint32_t StepGen::sync0CycleTime = 0;

124
Cards/EaserCAT-2000/Firmware/src/StepGen2.cpp Executable file
View File

@@ -0,0 +1,124 @@
#include <Arduino.h>
#include <stdio.h>
#include "StepGen2.h"
#include "extend32to64.h"
extern "C"
{
#include "esc.h"
}
extern extend32to64 longTime;
StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
{
actualPosition = 0;
commandedPosition = 0;
commandedStepPosition = 0;
initialPosition = 0;
initialStepPosition = 0;
stepsPerMM = 0;
enabled = 0;
dirPin = _dirPin;
dirPinName = digitalPinToPinName(dirPin);
stepPin = _stepPin;
pulseTimerChan = _timerChannel;
pulseTimer = new HardwareTimer(Timer);
pulseTimer->attachInterrupt(pulseTimerChan, irq); // Capture/compare innterrupt
pinMode(dirPin, OUTPUT);
startTimer = new HardwareTimer(Timer2);
startTimer->attachInterrupt(irq2);
}
uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
{
frequency = 0;
nSteps = 0;
dbg = 0;
if (!enabled) // Just .... don't
return updatePos(0);
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
nSteps = commandedStepPosition - initialStepPosition;
if (nSteps == 0) // No movement
{
return updatePos(1);
}
lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
if (abs(nSteps) < 0) // Some small number
{ //
frequency = (abs(nSteps) + 1) / lcncCycleTime; // Distribute steps inside available time
Tpulses = abs(nSteps) / frequency; //
Tstartf = (lcncCycleTime - Tpulses) / 2.0; //
} //
else // Regular step train, up or down
{ //
float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation. position = kTRAJ x time + mTRAJ
float mTRAJ = initialPosition; // Operating on incoming positions (not steps)
if (kTRAJ > 0) //
Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing upwards
else //
Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing downwards
frequency = fabs(kTRAJ * stepsPerMM); //
Tpulses = abs(nSteps) / frequency;
}
updatePos(5);
uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs)
dbg = timeSinceISR; //
Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq.
if (nSteps == 0) // Can do this much earlier, but want some calculated data for debugging
return updatePos(1);
timerFrequency = uint32_t(ceil(frequency));
startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
startTimer->refresh();
startTimer->resume();
return 1;
}
void StepGen2::startTimerCB()
{
startTimer->pause(); // Once is enough.
digitalWriteFast(dirPinName, nSteps < 0 ? HIGH : LOW); // nSteps negative => decrease, HIGH
// There will be a short break here for t2 usecs, in the future.
timerEndPosition += nSteps;
pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
// pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
pulseTimer->refresh();
pulseTimer->resume();
}
void StepGen2::pulseTimerCB()
{
int16_t dir = digitalReadFast(dirPinName); //
if (dir == HIGH) // The step just taken
timerPosition--;
else
timerPosition++;
int32_t diffPosition = timerEndPosition - timerPosition; // Same "polarity" as nSteps
if (diffPosition == 0)
pulseTimer->pause();
else
{
if (diffPosition < 0 && dir == LOW) // Change direction. Should not end up here, but alas
digitalWriteFast(dirPinName, HIGH); // Normal is to be HIGH when decreasing
if (diffPosition > 0 && dir == HIGH) // Change direction. Should not end up here, but alas
digitalWriteFast(dirPinName, LOW); // Normal is to be LOW when increasing
// Normally nothing is needed
}
}
uint32_t StepGen2::updatePos(uint32_t i)
{ //
initialPosition = commandedPosition; // Save the numeric position for next step
initialStepPosition = commandedStepPosition; // also the step we are at}
return i;
}
uint32_t StepGen2::sync0CycleTime = 0;

1195
Cards/EaserCAT-2000/Firmware/src/StepGen3.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

391
Cards/EaserCAT-2000/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-2000/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
}

355
Cards/EaserCAT-2000/Firmware/src/main.cpp Executable file
View File

@@ -0,0 +1,355 @@
#include <Arduino.h>
#include <stdio.h>
extern "C"
{
#include "ecat_slv.h"
#include "utypes.h"
};
_Objects Obj;
HardwareSerial Serial1(PA10, PA9);
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
HardwareTimer *baseTimer; // The base period timer
HardwareTimer *syncTimer; // The timer that syncs "with linuxcnc cycle"
uint32_t sync0CycleTime; // nanosecs, often 1000000 ( 1 ms )
#include "MyEncoder.h"
volatile uint16_t encCnt = 0;
void indexPulseEncoderCB1(void);
MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1);
void indexPulseEncoderCB1(void)
{
encCnt++;
Encoder1.indexPulse();
}
// #include <RunningAverage.h>
// RunningAverage irqServeDelays(1000); // To get the max delay of the irq serve time over the last second
CircularBuffer<uint16_t, 1000> irqServeDelays;
#include "StepGen3.h"
StepGen3 *Step = 0;
#include "extend32to64.h"
volatile uint64_t irqTime = 0, irqCnt = 0;
extend32to64 longTime;
void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs);
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
{
Encoder1.setLatch(Obj.IndexLatchEnable);
Encoder1.setScale(2000);
}
volatile uint16_t basePeriodCnt;
volatile uint64_t makePulsesCnt = 0, prevMakePulsesCnt = 0;
volatile uint16_t deltaMakePulsesCnt;
volatile double posCmd1, posCmd2;
double oldPosCmd1, oldPosCmd2;
double deltaPosCmd1, deltaPosCmd2;
void syncWithLCNC()
{
syncTimer->pause();
baseTimer->pause();
deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt;
prevMakePulsesCnt = makePulsesCnt;
Step->updateStepGen(posCmd1, posCmd2, sync0CycleTime); // Update positions
Step->makeAllPulses(); // Make first step right here
basePeriodCnt = sync0CycleTime / BASE_PERIOD; //
baseTimer->refresh(); //
baseTimer->resume(); // Make the other steps in ISR
}
void basePeriodCB(void)
{
if (--basePeriodCnt > 0) // Stop
Step->makeAllPulses();
else
baseTimer->pause();
}
int32_t delayT;
uint16_t thisCycleTime; // In usecs
int16_t maxIrqServeTime = 0;
uint64_t oldIrqTime = 0;
uint16_t nLoops;
uint16_t failedSM2s = 0;
uint16_t totalFailedSM2s = 0;
uint16_t nLoopsAboveNorm = 0;
void handleStepper(void)
{
if (oldIrqTime != 0) // See if there is a delay in data, normally it *should* be nLoops=1, but sometimes it is more
{
thisCycleTime = irqTime - oldIrqTime;
nLoops = round(float(thisCycleTime) / float(sync0CycleTime / 1000));
nLoopsAboveNorm += nLoops - 1;
}
oldIrqTime = irqTime;
uint32_t diffT = longTime.extendTime(micros()) - irqTime; // Time from interrupt was received by isr
irqServeDelays.push(diffT);
if (irqServeDelays.isFull()) // Do max calcs, just waiting a second
{
uint16_t maxInBuffer = 0;
using index_t = decltype(irqServeDelays)::index_t;
for (index_t i = 0; i < irqServeDelays.size(); i++)
{
if (maxInBuffer < irqServeDelays[i])
maxInBuffer = irqServeDelays[i];
}
if (maxIrqServeTime > maxInBuffer) // Reduce by one, slowly eating up excess time
maxIrqServeTime--;
if (maxIrqServeTime < maxInBuffer)
maxIrqServeTime = maxInBuffer;
}
if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{ // The normal case, position update every cycle
posCmd1 = Obj.CommandedPosition1; // The position update
posCmd2 = Obj.CommandedPosition2;
deltaPosCmd1 = posCmd1 - oldPosCmd1; // Needed for extrapolation in the other case
deltaPosCmd2 = posCmd2 - oldPosCmd2;
failedSM2s = 0;
}
else
{ // Not normal, we didn't get a position update. Extrapolate from previous updates
if (failedSM2s++ < 100) // Do max 10 such extrapolations, should be plenty
{ //
posCmd1 += deltaPosCmd1; // Continue with the same speed
posCmd2 += deltaPosCmd2;
}
totalFailedSM2s++;
}
oldPosCmd1 = posCmd1;
oldPosCmd2 = posCmd2;
// Obj.ActualPosition1 = Obj.CommandedPosition1;
// Obj.ActualPosition2 = Obj.CommandedPosition2;
Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1;
Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
delayT = maxIrqServeTime - diffT; // Add 10 as some safety margin
if (delayT > 0 && delayT < 900)
{
syncTimer->setOverflow(delayT, MICROSEC_FORMAT); // Work in flawed units, its ok
syncTimer->refresh();
syncTimer->resume();
}
else
{
syncWithLCNC();
}
}
float_t oldCommandedPosition = 0;
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{
Obj.IndexStatus = Encoder1.indexHappened();
Obj.EncPos = Encoder1.currentPos();
Obj.EncFrequency = Encoder1.frequency(longTime.extendTime(micros()));
Obj.IndexByte = Encoder1.getIndexState();
Obj.DiffT = nLoops;
Obj.D1 = 1000 * deltaPosCmd2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
Obj.D2 = totalFailedSM2s; // Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX; // Step->stepgen_array[1].freq;
Obj.D3 = 1000 * Obj.CommandedPosition2; // Step->stepgen_array[1].freq;
Obj.D4 = 1000 * posCmd2; // Step->stepgen_array[1].rawcount % UINT16_MAX;
oldCommandedPosition = Obj.CommandedPosition2;
}
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 = 1,
.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 measureCrystalFrequency(void);
volatile byte serveIRQ = 0;
void setup(void)
{
Serial1.begin(115200);
#if 0
measureCrystalFrequency(); // Calibrate crystal frequency
#endif
Step = new StepGen3;
encoder_config(); // Needed by encoder, probably breaks some timers.
ecat_slv_init(&config);
pinMode(PA11, OUTPUT); // Step X
pinMode(PA12, OUTPUT); // Dir X
pinMode(PC9, OUTPUT); // Step Z
pinMode(PC10, OUTPUT); // Dir Z
baseTimer = new HardwareTimer(TIM11); // The base period timer
setFrequencyAdjustedMicrosSeconds(baseTimer, BASE_PERIOD / 1000);
// baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); // Or the line above, This one is uncalibrated
baseTimer->attachInterrupt(basePeriodCB);
syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer
syncTimer->attachInterrupt(syncWithLCNC);
}
void loop(void)
{
uint64_t dTime;
if (serveIRQ)
{
DIG_process(ALEventIRQ, 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_poll();
}
void sync0Handler(void)
{
ALEventIRQ = ESC_ALeventread();
// if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{
irqTime = longTime.extendTime(micros());
serveIRQ = 1;
}
irqCnt++; // debug output
}
// 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;
uint32_t user_int_mask = ESCREG_ALEVENT_SM2; // Only SM2
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(PC3), 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;
uint32_t user_int_mask = ESCREG_ALEVENT_SM2;
if (mask & user_int_mask)
{
// Disable interrupt from SYNC0
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask));
detachInterrupt(digitalPinToInterrupt(PC3));
// Disable LAN9252 interrupt
uint32_t bits = 0x00000000;
ESC_write(0x5c, &bits, 4);
}
}
extern "C" uint32_t ESC_SYNC0cycletime(void);
// Setup of DC
uint16_t dc_checker(void)
{
// Indicate we run DC
ESCvar.dcsync = 1;
sync0CycleTime = ESC_SYNC0cycletime(); // nanosecs
return 0;
}
//
// Code to calibrate the crystal.
//
#include <HardwareTimer.h>
HardwareTimer *timer;
volatile uint32_t cnt;
void CB(void)
{
if (cnt-- == 0)
{
timer->pause();
}
}
void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs)
{
const uint16_t calibrated1000 = 1042; // <- This is the factor to adjust to make 1 sec = 1 sec
uint32_t period_cyc = (usecs * (timer->getTimerClkFreq() / 1000)) / calibrated1000; // Avoid overflow during math
uint32_t Prescalerfactor = (period_cyc / 0x10000) + 1;
uint32_t PeriodTicks = period_cyc / Prescalerfactor;
timer->setPrescaleFactor(Prescalerfactor);
timer->setOverflow(PeriodTicks, TICK_FORMAT);
// Serial1.printf("Period_cyc=%u Prescalefactor =%u ticks = %u\n", period_cyc, Prescalerfactor, PeriodTicks);
}
void measureCrystalFrequency(void)
{
timer = new HardwareTimer(TIM1);
Serial1.begin(115200);
delay(3000);
Serial1.printf("Clock freq = %u\n", timer->getTimerClkFreq());
setFrequencyAdjustedMicrosSeconds(timer, 1000);
timer->refresh();
timer->attachInterrupt(CB);
cnt = 10000;
Serial1.printf("\n");
uint32_t startT = micros();
timer->resume();
while (cnt != 0)
;
uint32_t endT = micros();
uint32_t diffT = endT - startT;
Serial1.printf("\n");
Serial1.printf("diff = %u\n", diffT);
Serial1.printf("\n");
delay(10000);
Serial1.printf("\n");
exit(0);
}