/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* <h2><center>© Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "string.h"
#include "math.h"
#include "inttypes.h"
#define ADDR_ERR_1 0x00
#define ADDR_ERR_2 0x01
#define ADDR_ERR_3 0x02
#define ADDR_ERR_4 0x03
#define ADDR_ERR_5 0x04
#define ADDR_ERR_6 0x05
#define ADDR_ERR_7 0x06
#define ADDR_ERR_8 0x07
#define ADDR_ERR_9 0x08
#define ADDR_ERR_10 0x09
#define ADDR_RPS_SET_H 0x0A
#define ADDR_RPS_SET_L 0x0B
#define ADDR_TRQ_OVER 0x0C
#define ADDR_ROT_RST 0x0D
#define ADDR_STP_ROT_H 0x0E
#define ADDR_STP_ROT_L 0x0F
#define ADDR_ROT_SUM_H 0x10
#define ADDR_ROT_SUM_L 0x11
#define ADDR_RPS_RAW_H 0x12
#define ADDR_RPS_RAW_L 0x13
#define ADDR_RPS_AVE_H 0x14
#define ADDR_RPS_AVE_L 0x15
#define ADDR_RPS_MAX_H 0x16
#define ADDR_RPS_MAX_L 0x17
#define ADDR_RPS_MIN_H 0x18
#define ADDR_RPS_MIN_L 0x19
#define ADDR_DTY_RAW_H 0x1A
#define ADDR_DTY_RAW_L 0x1B
#define ADDR_DTY_AVE_H 0x1C
#define ADDR_DTY_AVE_L 0x1D
#define ADDR_DTY_MAX_H 0x1E
#define ADDR_DTY_MAX_L 0x1F
#define ADDR_DTY_MIN_H 0x20
#define ADDR_DTY_MIN_L 0x21
#define ADDR_DRV_MEM 0x22
#define ADDR_AVE_TIM 0x23
#define ADDR_PID_KP_H 0x24
#define ADDR_PID_KP_L 0x25
#define ADDR_PID_KI_H 0x26
#define ADDR_PID_KI_L 0x27
#define ADDR_PID_KD_H 0x28
#define ADDR_PID_KD_L 0x29
#define ADDR_DT1_REQ 0x30
#define ADDR_DT1_DST_H 0x31
#define ADDR_DT1_DST_L 0x32
#define ADDR_DT2_REQ 0x33
#define ADDR_DT2_DST_H 0x34
#define ADDR_DT2_DST_L 0x35
/* 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 ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
I2C_HandleTypeDef hi2c1;
RTC_HandleTypeDef hrtc;
SPI_HandleTypeDef hspi1;
DMA_HandleTypeDef hdma_spi1_rx;
DMA_HandleTypeDef hdma_spi1_tx;
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim6;
TIM_HandleTypeDef htim7;
TIM_HandleTypeDef htim17;
UART_HandleTypeDef huart2;
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_DMA_Init(void);
static void MX_SPI1_Init(void);
static void MX_I2C1_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM6_Init(void);
static void MX_TIM7_Init(void);
static void MX_TIM2_Init(void);
static void MX_RTC_Init(void);
static void MX_ADC1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM17_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int _write(int file, char *ptr, int len){
HAL_UART_Transmit(&huart2,(uint8_t *)ptr,len,10);
return len;
}
uint8_t param_arry[200] = {0};
enum {
SPI_STATE_READY,SPI_STATE_WAIT_COMMAND,SPI_STATE_GET_COMMAND,SPI_STATE_WAIT_READ,SPI_STATE_WAIT_WRITE,SPI_STATE_COMMAND_ERR
};
enum{
DIST1_READY,DIST1_INIT_START,DIST1_INIT_END,DIST1_MES_START,DIST1_TRG_WAIT,DIST1_ECH_WAIT,DIST1_MES_END
};
enum{
DIST2_READY,DIST2_INIT_START,DIST2_INIT_END,DIST2_MES_START,DIST2_TRG_WAIT,DIST2_ECH_WAIT,DIST2_MES_END
};
uint16_t dist1_st = 0, dist1_ed = 0;
uint16_t dist2_st = 0, dist2_ed = 0;
uint8_t dist1_state = DIST1_READY;
uint8_t dist2_state = DIST2_READY;
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
setbuf(stdout, NULL);
param_arry[ADDR_AVE_TIM] = 50;
param_arry[ADDR_PID_KP_H] = ((uint16_t)(600)) >> 8;
param_arry[ADDR_PID_KP_L] = ((uint16_t)(600)) & 0x00FF;
param_arry[ADDR_PID_KI_H] = ((uint16_t)(2)) >> 8;
param_arry[ADDR_PID_KI_L] = ((uint16_t)(2)) & 0x00FF;
param_arry[ADDR_PID_KD_H] = ((uint16_t)(300)) >> 8;
param_arry[ADDR_PID_KD_L] = ((uint16_t)(300)) & 0x00FF;
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_USART2_UART_Init();
MX_DMA_Init();
MX_SPI1_Init();
MX_I2C1_Init();
MX_TIM1_Init();
MX_TIM6_Init();
MX_TIM7_Init();
MX_TIM2_Init();
MX_RTC_Init();
MX_ADC1_Init();
MX_TIM3_Init();
MX_TIM17_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, 0);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, 0);
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1, 0);
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2, 0);
HAL_TIM_Encoder_Start( &htim3, TIM_CHANNEL_1);
HAL_TIM_Encoder_Start( &htim3, TIM_CHANNEL_2);
HAL_TIM_Base_Start_IT(&htim6);
HAL_TIM_Base_Start_IT(&htim7);
HAL_TIM_Base_Start_IT(&htim17);
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
uint8_t spiState = SPI_STATE_READY;
while (1)
{
/*spi_start*/
static uint8_t command_rxbuf[2] = {};
static uint8_t spi_txbuf[128] = {0};
static uint8_t spi_rxbuf[128] = {0};
if (spiState == SPI_STATE_READY) {
HAL_SPI_Receive_DMA(&hspi1, command_rxbuf,3);
spiState = SPI_STATE_WAIT_COMMAND;
}
if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_COMMAND) {
spiState = SPI_STATE_GET_COMMAND;
}
if (spiState == SPI_STATE_GET_COMMAND && command_rxbuf[0] == 0x10 && command_rxbuf[2] >=1) {
spiState = SPI_STATE_WAIT_READ;
for (int var = 0; var < command_rxbuf[2]; ++var) {
spi_txbuf[var] = param_arry[command_rxbuf[1] + var];
}
HAL_SPI_Transmit_DMA(&hspi1, spi_txbuf, command_rxbuf[2]);
}
else if (spiState == SPI_STATE_GET_COMMAND && command_rxbuf[0] == 0x20 && command_rxbuf[2] >=1) {
spiState = SPI_STATE_WAIT_WRITE;
HAL_SPI_Receive_DMA(&hspi1, spi_rxbuf, command_rxbuf[2]);
}
else if (spiState == SPI_STATE_GET_COMMAND) {
spiState = SPI_STATE_COMMAND_ERR;
printf("command_err\r\n");
}
if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_READ) {
spiState = SPI_STATE_READY;
}
if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_WRITE) {
for (int var = 0; var < command_rxbuf[2]; ++var) {
param_arry[command_rxbuf[1] + var] = spi_rxbuf[var];
}
spiState = SPI_STATE_READY;
}
/*spi_end*/
/*dist_mes*/
uint8_t dist_trg_pin_data = HAL_GPIO_ReadPin(DIST_TRG_GPIO_Port,DIST_TRG_Pin);
uint8_t dist1_ech_pin_data = HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port,DIST1_ECH_Pin);
uint16_t dist1_dist = (int16_t)(((param_arry[ADDR_DT1_DST_H] << 8) & 0xFF00) | (param_arry[ADDR_DT1_DST_H] & 0x00FF));
uint16_t dist1_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
if (param_arry[ADDR_DT1_REQ] != 0 && dist1_state == DIST1_READY && dist_trg_pin_data == GPIO_PIN_RESET && dist1_ech_pin_data == GPIO_PIN_RESET
&& dist2_state == DIST2_READY) {
dist1_state = DIST1_INIT_START;
param_arry[ADDR_DT1_DST_H] = 0;
param_arry[ADDR_DT1_DST_L] = 0;
}else if (dist1_state == DIST1_INIT_START) {
dist1_state = DIST1_INIT_END;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_SET);
__HAL_TIM_SET_COUNTER(&htim17,0);
dist1_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
}else if (dist1_state == DIST1_INIT_END && dist_trg_pin_data == GPIO_PIN_SET) {
dist1_state = DIST1_TRG_WAIT;
}else if (dist1_state == DIST1_TRG_WAIT && dist1_cycle_time > 2) {
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
dist1_state = DIST1_ECH_WAIT;
}else if (dist1_state == DIST1_MES_END) {
dist1_state = DIST1_READY;
param_arry[ADDR_DT1_REQ] = 0;
}else if (dist1_state >= DIST1_INIT_END && dist1_cycle_time > 3000) {
dist1_dist = -1;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
param_arry[ADDR_DT1_DST_H] = (dist1_dist) >> 8;
param_arry[ADDR_DT1_DST_L] = (dist1_dist) & 0x00FF;
param_arry[ADDR_DT1_REQ] = 0;
dist1_state = DIST1_READY;
}
/*dist2_mes*/
uint8_t dist2_ech_pin_data = HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port,DIST2_ECH_Pin);
uint16_t dist2_dist = (int16_t)(((param_arry[ADDR_DT2_DST_H] << 8) & 0xFF00) | (param_arry[ADDR_DT2_DST_H] & 0x00FF));
uint16_t dist2_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
if (param_arry[ADDR_DT2_REQ] != 0 && dist2_state == DIST2_READY && dist1_state == DIST1_READY
&& dist_trg_pin_data == GPIO_PIN_RESET && dist2_ech_pin_data == GPIO_PIN_RESET){
dist2_state = DIST2_INIT_START;
param_arry[ADDR_DT2_DST_H] = 0;
param_arry[ADDR_DT2_DST_L] = 0;
}else if (dist2_state == DIST2_INIT_START) {
dist2_state = DIST2_INIT_END;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_SET);
__HAL_TIM_SET_COUNTER(&htim17,0);
dist2_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
}else if (dist2_state == DIST2_INIT_END && dist_trg_pin_data == GPIO_PIN_SET) {
dist2_state = DIST2_TRG_WAIT;
}else if (dist2_state == DIST2_TRG_WAIT && dist2_cycle_time > 2) {
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
dist2_state = DIST2_ECH_WAIT;
}else if (dist2_state == DIST2_MES_END) {
dist2_state = DIST2_READY;
param_arry[ADDR_DT2_REQ] = 0;
}else if (dist2_state >= DIST2_INIT_END && dist2_cycle_time > 3000) {
dist2_dist = -1;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
param_arry[ADDR_DT2_DST_H] = (dist2_dist) >> 8;
param_arry[ADDR_DT2_DST_L] = (dist2_dist) & 0x00FF;
param_arry[ADDR_DT2_REQ] = 0;
dist2_state = DIST2_READY;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_LSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.LSIState = RCC_LSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
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_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_RTC
|RCC_PERIPHCLK_TIM1|RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12PLLCLK_DIV1;
PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI;
PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
PeriphClkInit.Tim1ClockSelection = RCC_TIM1CLK_PLLCLK;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief ADC1 Initialization Function
* @param None
* @retval None
*/
static void MX_ADC1_Init(void)
{
/* USER CODE BEGIN ADC1_Init 0 */
/* USER CODE END ADC1_Init 0 */
ADC_MultiModeTypeDef multimode = {0};
ADC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN ADC1_Init 1 */
/* USER CODE END ADC1_Init 1 */
/** Common config
*/
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.DMAContinuousRequests = DISABLE;
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc1.Init.LowPowerAutoWait = DISABLE;
hadc1.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
Error_Handler();
}
/** Configure the ADC multi-mode
*/
multimode.Mode = ADC_MODE_INDEPENDENT;
if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
{
Error_Handler();
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_4;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC1_Init 2 */
/* USER CODE END ADC1_Init 2 */
}
/**
* @brief I2C1 Initialization Function
* @param None
* @retval None
*/
static void MX_I2C1_Init(void)
{
/* USER CODE BEGIN I2C1_Init 0 */
/* USER CODE END I2C1_Init 0 */
/* USER CODE BEGIN I2C1_Init 1 */
/* USER CODE END I2C1_Init 1 */
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x2000090E;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/** Configure Analogue filter
*/
if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
{
Error_Handler();
}
/** Configure Digital filter
*/
if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C1_Init 2 */
/* USER CODE END I2C1_Init 2 */
}
/**
* @brief RTC Initialization Function
* @param None
* @retval None
*/
static void MX_RTC_Init(void)
{
/* USER CODE BEGIN RTC_Init 0 */
/* USER CODE END RTC_Init 0 */
/* USER CODE BEGIN RTC_Init 1 */
/* USER CODE END RTC_Init 1 */
/** Initialize RTC Only
*/
hrtc.Instance = RTC;
hrtc.Init.HourFormat = RTC_HOURFORMAT_24;
hrtc.Init.AsynchPrediv = 127;
hrtc.Init.SynchPrediv = 255;
hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;
hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;
hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;
if (HAL_RTC_Init(&hrtc) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN RTC_Init 2 */
/* USER CODE END RTC_Init 2 */
}
/**
* @brief SPI1 Initialization Function
* @param None
* @retval None
*/
static void MX_SPI1_Init(void)
{
/* USER CODE BEGIN SPI1_Init 0 */
/* USER CODE END SPI1_Init 0 */
/* USER CODE BEGIN SPI1_Init 1 */
/* USER CODE END SPI1_Init 1 */
/* SPI1 parameter configuration*/
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_SLAVE;
hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_HARD_INPUT;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi1.Init.CRCPolynomial = 7;
hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
hspi1.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
if (HAL_SPI_Init(&hspi1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN SPI1_Init 2 */
/* USER CODE END SPI1_Init 2 */
}
/**
* @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 = 0;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 2559;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
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.MasterOutputTrigger2 = TIM_TRGO2_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
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();
}
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != 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.BreakFilter = 0;
sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
sBreakDeadTimeConfig.Break2Filter = 0;
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 TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2;
htim2.Init.Prescaler = 0;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 4294967295;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/**
* @brief TIM3 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM3_Init(void)
{
/* USER CODE BEGIN TIM3_Init 0 */
/* USER CODE END TIM3_Init 0 */
TIM_Encoder_InitTypeDef sConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM3_Init 1 */
/* USER CODE END TIM3_Init 1 */
htim3.Instance = TIM3;
htim3.Init.Prescaler = 0;
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
htim3.Init.Period = 65535;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sConfig.IC1Filter = 0;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 0;
if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM3_Init 2 */
/* USER CODE END TIM3_Init 2 */
}
/**
* @brief TIM6 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM6_Init(void)
{
/* USER CODE BEGIN TIM6_Init 0 */
/* USER CODE END TIM6_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM6_Init 1 */
/* USER CODE END TIM6_Init 1 */
htim6.Instance = TIM6;
htim6.Init.Prescaler = 64;
htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
htim6.Init.Period = 9999;
htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM6_Init 2 */
/* USER CODE END TIM6_Init 2 */
}
/**
* @brief TIM7 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM7_Init(void)
{
/* USER CODE BEGIN TIM7_Init 0 */
/* USER CODE END TIM7_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM7_Init 1 */
/* USER CODE END TIM7_Init 1 */
htim7.Instance = TIM7;
htim7.Init.Prescaler = 64000;
htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
htim7.Init.Period = 100;
htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM7_Init 2 */
/* USER CODE END TIM7_Init 2 */
}
/**
* @brief TIM17 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM17_Init(void)
{
/* USER CODE BEGIN TIM17_Init 0 */
/* USER CODE END TIM17_Init 0 */
/* USER CODE BEGIN TIM17_Init 1 */
/* USER CODE END TIM17_Init 1 */
htim17.Instance = TIM17;
htim17.Init.Prescaler = 639;
htim17.Init.CounterMode = TIM_COUNTERMODE_UP;
htim17.Init.Period = 65535;
htim17.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim17.Init.RepetitionCounter = 0;
htim17.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim17) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM17_Init 2 */
/* USER CODE END TIM17_Init 2 */
}
/**
* @brief USART2 Initialization Function
* @param None
* @retval None
*/
static void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 38400;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
/**
* Enable DMA controller clock
*/
static void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Channel2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
/* DMA1_Channel3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : TEST_Pin */
GPIO_InitStruct.Pin = TEST_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(TEST_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : DIST2_ECH_Pin DIST1_ECH_Pin */
GPIO_InitStruct.Pin = DIST2_ECH_Pin|DIST1_ECH_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pin : DIST_TRG_Pin */
GPIO_InitStruct.Pin = DIST_TRG_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(DIST_TRG_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : LED_Pin */
GPIO_InitStruct.Pin = LED_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
}
/* USER CODE BEGIN 4 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_TXRX_COMP;
//printf("spi_txrx_comp\r\n");
}
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_RX_COMP;
//printf("spi_rx_comp\r\n");
}
void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_TX_COMP;
//printf("spi_tx_comp\r\n");
}
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_ERR;
printf("spi_err\r\n");
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
if (GPIO_Pin == DIST1_ECH_Pin && HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port, DIST1_ECH_Pin) == GPIO_PIN_SET && dist1_state == DIST1_ECH_WAIT) {
dist1_state = DIST1_MES_START;
dist1_st = __HAL_TIM_GET_COUNTER(&htim17);
//printf("%d",dist1_st);
}
if (GPIO_Pin == DIST1_ECH_Pin && HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port, DIST1_ECH_Pin) == GPIO_PIN_RESET && dist1_state == DIST1_MES_START) {
dist1_ed = __HAL_TIM_GET_COUNTER(&htim17);
int16_t dist1_dist = ((float)(dist1_ed - dist1_st) * 3.4f) / 2.0f;
param_arry[ADDR_DT1_DST_H] = (dist1_dist) >> 8;
param_arry[ADDR_DT1_DST_L] = (dist1_dist) & 0x00FF;
dist1_state = DIST1_MES_END;
}
if (GPIO_Pin == DIST2_ECH_Pin && HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port, DIST2_ECH_Pin) == GPIO_PIN_SET && dist2_state == DIST2_ECH_WAIT) {
dist2_state = DIST2_MES_START;
dist2_st = __HAL_TIM_GET_COUNTER(&htim17);
//printf("%d",dist1_st);
}
if (GPIO_Pin == DIST2_ECH_Pin && HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port, DIST2_ECH_Pin) == GPIO_PIN_RESET && dist2_state == DIST2_MES_START) {
dist2_ed = __HAL_TIM_GET_COUNTER(&htim17);
int16_t dist2_dist = ((float)(dist2_ed - dist2_st) * 3.4f) / 2.0f;
param_arry[ADDR_DT2_DST_H] = (dist2_dist) >> 8;
param_arry[ADDR_DT2_DST_L] = (dist2_dist) & 0x00FF;
dist2_state = DIST2_MES_END;
}
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
//HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
if (htim == &htim6) {
HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_SET);
const float dt = 10.0f;
const float rot_pls = 52;
const float pwm_period = 2559.0f;
int16_t cnt_raw = 0;
static float rps_arr[50] = {}, duty_arr[50] = {};
static float rps_max=0, rps_min=0, duty_max=0, duty_min=0;
static float rps_raw = 0,rps_ave = 0;
static float duty_raw = 0,duty = 0,duty_ave = 0;
float rps_sum = 0, duty_sum=0;
static int64_t cnt_sum = 0;
static int16_t rot_sum = 0;
static uint8_t ave_count = 0;
static uint16_t trq_over_cont = 0;
static float P,I,D,preP=0;
float ave_point = param_arry[ADDR_AVE_TIM];
float set_rps = (float)(int16_t)(((param_arry[ADDR_RPS_SET_H] << 8) & 0xFF00) | (param_arry[ADDR_RPS_SET_L] & 0x00FF));
int16_t stop_rot = (int16_t)(((param_arry[ADDR_STP_ROT_H] << 8) & 0xFF00) | (param_arry[ADDR_STP_ROT_L] & 0x00FF));
float kp = (float)(int16_t)(((param_arry[ADDR_PID_KP_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KP_L] & 0x00FF)) * 0.01f;
float ki = (float)(int16_t)(((param_arry[ADDR_PID_KI_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KI_L] & 0x00FF)) * 0.01f;
float kd = (float)(int16_t)(((param_arry[ADDR_PID_KD_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KD_L] & 0x00FF)) * 0.01f;
cnt_raw = __HAL_TIM_GET_COUNTER(&htim3);
__HAL_TIM_SET_COUNTER(&htim3,0);
if (ave_count > ave_point) {ave_count = 0;}
if (param_arry[ADDR_ROT_RST] != 0x00) {
cnt_sum = 0;
param_arry[ADDR_ROT_RST] = 0x00;
}
cnt_sum += cnt_raw;
rot_sum = cnt_sum / rot_pls;
rps_raw = ((float)cnt_raw / rot_pls) * (1000.0f/dt);
rps_arr[ave_count] = rps_raw;
//PID
P = set_rps - rps_raw;
I += (P + preP) / 2.0f * dt;
D = (P - preP) / dt;
preP = P;
if (fabs(rps_ave) < 0.001 && set_rps == 0) {
P = 0;
I = 0;
D = 0;
}
duty_raw = kp * P + ki *I + kd * D;
if (duty_raw >= 100) {
duty_raw = 100;
}else if (duty <= -100) {
duty_raw = -100;
}
duty = duty_raw;
duty_arr[ave_count] = duty;
for (int var = 0; var < ave_point; ++var) {duty_sum += duty_arr[var];}
duty_ave = duty_sum / ave_point;
for (int var = 0; var < ave_point; ++var) {rps_sum += rps_arr[var];}
rps_ave = rps_sum / ave_point;
if (rps_max < rps_ave) {rps_max = rps_ave;}
if (rps_min > rps_ave) {rps_min = rps_ave;}
if (duty_max < duty) {duty_max = duty;}
if (duty < duty_min) {duty_min = duty;}
if (param_arry[ADDR_DRV_MEM] != 0) {
duty_max = duty;
duty_min = duty;
rps_max = rps_ave;
rps_min = rps_ave;
param_arry[ADDR_DRV_MEM] = 0x00;
}
param_arry[ADDR_ROT_SUM_H] = (rot_sum >> 8);
param_arry[ADDR_ROT_SUM_L] = (rot_sum >> 0);
param_arry[ADDR_RPS_RAW_H] = ((int16_t)(rps_raw * 100.0f)) >> 8;
param_arry[ADDR_RPS_RAW_L] = ((int16_t)(rps_raw * 100.0f)) >> 0;
param_arry[ADDR_RPS_AVE_H] = ((int16_t)(rps_ave * 100.0f)) >> 8;
param_arry[ADDR_RPS_AVE_L] = ((int16_t)(rps_ave * 100.0f)) >> 0;
param_arry[ADDR_RPS_MAX_H] = ((int16_t)(rps_max * 100.0f)) >> 8;
param_arry[ADDR_RPS_MAX_L] = ((int16_t)(rps_max * 100.0f)) >> 0;
param_arry[ADDR_RPS_MIN_H] = ((int16_t)(rps_min * 100.0f)) >> 8;
param_arry[ADDR_RPS_MIN_L] = ((int16_t)(rps_min * 100.0f)) >> 0;
param_arry[ADDR_DTY_AVE_H] = ((int16_t)(duty_ave * 100.0f)) >> 8;
param_arry[ADDR_DTY_AVE_L] = ((int16_t)(duty_ave * 100.0f)) >> 0;
param_arry[ADDR_DTY_MAX_H] = ((int16_t)(duty_max * 100.0f)) >> 8;
param_arry[ADDR_DTY_MAX_L] = ((int16_t)(duty_max * 100.0f)) >> 0;
param_arry[ADDR_DTY_MIN_H] = ((int16_t)(duty_min * 100.0f)) >> 8;
param_arry[ADDR_DTY_MIN_L] = ((int16_t)(duty_min * 100.0f)) >> 0;
ave_count += 1;
/*PWM*/
int pwm_count = (1-(fabs(duty)/100)) * pwm_period;
if (param_arry[ADDR_TRQ_OVER] != 0) {
param_arry[ADDR_RPS_SET_H] = 0x00;
param_arry[ADDR_RPS_SET_L] = 0x00;
}
if ((stop_rot > 0 && rot_sum >= stop_rot) || (stop_rot < 0 && rot_sum <= stop_rot)) {
param_arry[ADDR_RPS_SET_H] = 0x00;
param_arry[ADDR_RPS_SET_L] = 0x00;
}
if (duty >= 0) {
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, (int)pwm_period);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, pwm_count);
}else {
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, pwm_count);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, (int)pwm_period);
}
if (fabs(duty) >= 99 && fabs(rps_raw) < 0.5f) {
trq_over_cont += 1;
if (trq_over_cont > 1000/dt) {
param_arry[ADDR_TRQ_OVER] = 0x01;
param_arry[ADDR_RPS_SET_H] = 0x00;
param_arry[ADDR_RPS_SET_L] = 0x00;
}
}else{
trq_over_cont = 0;
}
HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_RESET);
//printf("%5.2f_%5.2f_%5.2f\r\n",P,I,D);
}
if (htim == &htim7){
HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
//printf("%5.2f_%5.2f_%5.2f_%5.2f\r\n",g_rps_raw,g_rps_ave,g_rps_max,g_rps_min);
//int16_t rps_ave = (int16_t)(((param_arry[ADDR_RPS_AVE_H] << 8) & 0xFF00) | (param_arry[ADDR_RPS_AVE_L] & 0x00FF));
}
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
