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

Reshuffle file structure

Browse Source
This commit is contained in:
Hakan Bastedt
2023-12-31 09:17:42 +01:00
parent 1851f4168e
commit 485901120c
125 changed files with 45 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

270
Firmware/src/Stepper.cpp Executable file
View File

@@ -0,0 +1,270 @@
#include <Arduino.h>
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "Stepper.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim1;
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM1_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if (htim->Instance == TIM1)
{
//__HAL_RCC_GPIOE_CLK_ENABLE();
/**TIM1 GPIO Configuration
PE9 ------> TIM1_CH1
PA8 ------< TIM1_CH1
*/
GPIO_InitStruct.Pin = GPIO_PIN_11; // 9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
// HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}
}
/**
* @brief The application entry point.
* @retval int
*/
void StepperSetup(void)
{
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
// HAL_Init();
// SystemClock_Config();
/* Initialize all configured peripherals */
// MX_GPIO_Init();
MX_TIM1_Init();
// htim1.Instance->ARR = 1;
// htim1.Instance->CCR1 = 1;
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
TIM_TypeDef *TIMM = TIM1;
#define CLOCK_FREQ (168000000-2000000)
// Best range on timer clock frequency/pulse length/65355 +1 => best resolution in the pulse range
TIM1->PSC = CLOCK_FREQ / 1000 / (1 << 16);
/* Infinite loop */
#if 0
while (1)
{
makePulses(1200, 15);
HAL_Delay(1000);
}
#endif
}
void makePulses(uint32_t totalLength /* µsec */, uint32_t nPulses)
{
uint64_t TickFreq = CLOCK_FREQ / (TIM1->PSC+1); // 56 MHz at PSC=2
uint64_t TicksTotal = TickFreq * totalLength / 1000000; // Total number of ticks during this time, ca 56000
uint32_t TicksPerPulse = TicksTotal / nPulses;
TIM1->ARR = TicksPerPulse - 1;
TIM1->CCR1 = TicksPerPulse / 2;
TIM1->RCR = nPulses - 1;
TIM1->EGR = TIM_EGR_UG;
TIM1->CR1 |= TIM_CR1_OPM;
TIM1->CR1 |= TIM_CR1_CEN;
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLM = 8;
RCC_OscInitStruct.PLL.PLLN = 168;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief TIM1 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM1_Init(void)
{
/* USER CODE BEGIN TIM1_Init 0 */
/* USER CODE END TIM1_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
/* USER CODE BEGIN TIM1_Init 1 */
/* USER CODE END TIM1_Init 1 */
htim1.Instance = TIM1;
htim1.Init.Prescaler = 70;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 65535;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 10;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM2;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
sBreakDeadTimeConfig.DeadTime = 0;
sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM1_Init 2 */
/* USER CODE END TIM1_Init 2 */
HAL_TIM_MspPostInit(&htim1);
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOE_CLK_ENABLE();
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */

352
Firmware/src/Stm32F4_Encoder.cpp Executable file
View File

@@ -0,0 +1,352 @@
#include <Stm32F4_Encoder.h>
/*
Stm32F4_Encoder.cpp
Created on: Nov 20, 2020
Author: GoktugH.
*/
Encoder::Encoder()
{
int unit;
}
void Encoder::eattach(int enco)
{
}
void Encoder::attachh(int encoNumber)
{
eattach(encoNumber);
}
void Encoder::SetCount(enum EncTimer enc, int64_t Counter)
{
if (enc == Tim2)
TIM2->CNT = Counter;
else if (enc == Tim3)
TIM3->CNT = Counter;
else if (enc == Tim4)
TIM4->CNT = Counter;
else if (enc == Tim8)
TIM8->CNT = Counter;
}
uint16_t Encoder::GetCount(enum EncTimer enc)
{
if (enc == Tim2)
c = (TIM2->CNT);
else if (enc == Tim3)
c = (TIM3->CNT);
else if (enc == Tim4)
c = (TIM4->CNT);
else if (enc == Tim8)
c = (TIM8->CNT);
return c;
}
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 rcc_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);
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);
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);
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;
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;
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;
}

230
Firmware/src/main.cpp Executable file
View File

@@ -0,0 +1,230 @@
#include <Arduino.h>
#include <stdio.h>
extern "C"
{
#include "ecat_slv.h"
#include "utypes.h"
};
#include <CircularBuffer.h>
#define RINGBUFFERLEN 101
CircularBuffer<double_t, RINGBUFFERLEN> Pos;
CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
#include <Stm32F4_Encoder.h>
int64_t PreviousEncoderCounterValue = 0;
int64_t unwrap_encoder(uint16_t in, int64_t *prev);
Encoder EncoderInit;
Encoder *encP = &EncoderInit;
#include "Stepper.h"
#define INDEX_PIN PA2
HardwareSerial Serial1(PA10, PA9);
_Objects Obj;
void indexPulse(void);
double PosScaleRes = 1.0;
uint32_t CurPosScale = 1;
uint8_t OldLatchCEnable = 0;
volatile uint8_t indexPulseFired = 0;
uint32_t nFires = 0;
volatile uint8_t pleaseZeroTheCounter = 0;
uint32_t sync0CycleTime = 0;
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
{
if (Obj.IndexLatchEnable && !OldLatchCEnable) // Should only happen first time IndexCEnable is set
{
pleaseZeroTheCounter = 1;
}
OldLatchCEnable = Obj.IndexLatchEnable;
if (CurPosScale != Obj.EncPosScale && Obj.EncPosScale != 0)
{
CurPosScale = Obj.EncPosScale;
PosScaleRes = 1.0 / double(CurPosScale);
}
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{
Obj.IndexStatus = 0;
if (indexPulseFired)
{
Obj.IndexStatus = 1;
indexPulseFired = 0;
nFires++;
PreviousEncoderCounterValue = 0;
}
Obj.DiffT = sync0CycleTime;
int64_t pos = unwrap_encoder(TIM2->CNT, &PreviousEncoderCounterValue);
double CurPos = pos * PosScaleRes;
Obj.EncPos = CurPos;
double diffT = 0;
double diffPos = 0;
TDelta.push(ESCvar.Time); // Running average over the length of the circular buffer
Pos.push(CurPos);
if (Pos.size() >= 2)
{
diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
diffPos = fabs(Pos.last() - Pos.first());
}
Obj.EncFrequency = diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
Obj.IndexByte = digitalRead(INDEX_PIN);
if (Obj.IndexByte)
Serial1.printf("IS 1\n");
}
void ESC_interrupt_enable(uint32_t mask);
void ESC_interrupt_disable(uint32_t mask);
uint16_t dc_checker(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 = NULL, // StepGen,
.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);
rcc_config();
StepperSetup();
// Set starting count value
EncoderInit.SetCount(Tim2, 0);
// EncoderInit.SetCount(Tim3, 0);
// EncoderInit.SetCount(Tim4, 0);
// EncoderInit.SetCount(Tim8, 0);
ecat_slv_init(&config);
attachInterrupt(digitalPinToInterrupt(INDEX_PIN), indexPulse, RISING); // Always when Index triggered
}
void loop(void)
{
ESCvar.PrevTime = ESCvar.Time;
ecat_slv();
}
#define ONE_PERIOD 65536
#define HALF_PERIOD 32768
int64_t unwrap_encoder(uint16_t in, int64_t *prev)
{
int64_t c64 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int64_t dif = (c64 - *prev); // 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 < -HALF_PERIOD)
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int64_t unwrapped = *prev + mod_dif;
*prev = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}
void indexPulse(void)
{
if (pleaseZeroTheCounter)
{
TIM2->CNT = 0;
indexPulseFired = 1;
Pos.clear();
TDelta.clear();
pleaseZeroTheCounter = 0;
}
}
volatile int32_t actualPosition = 0;
volatile int32_t requestedPosition;
volatile uint32_t pulsesToGo = 0;
volatile byte forwardDirection = 0; // 1 if going forward
#define STEPPER_DIR PA12
//#define STEPPER_STEP PA11 // Set in StepperSetup
void sync0Handler(void)
{
// Update the actual position
actualPosition += pulsesToGo;
Obj.StepgenOut1.ActualPosition = actualPosition;
// Get new end position
requestedPosition = Obj.StepGenIn1.CommandedPosition;
// Get the diff and the direction
pulsesToGo = requestedPosition - actualPosition;
forwardDirection = pulsesToGo > 0 ? 1 : 0;
// Set direction pin
digitalWrite(STEPPER_DIR, forwardDirection); // I think one should really wait a bit when changed
// Make the pulses using hardware timer
makePulses(sync0CycleTime/1000, pulsesToGo);
}
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));
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);
}
}
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
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();
return 0;
}