commit e9dc2d693850bed78c498ade1942fd6f83b5ffc2 Author: surface Date: Fri Oct 17 20:02:09 2025 +0200 init diff --git a/Core/Inc/main.h b/Core/Inc/main.h new file mode 100644 index 0000000..931cb0f --- /dev/null +++ b/Core/Inc/main.h @@ -0,0 +1,71 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 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 */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __MAIN_H +#define __MAIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f4xx_hal.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ + +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); + +/* Exported functions prototypes ---------------------------------------------*/ +void Error_Handler(void); + +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +/* Private defines -----------------------------------------------------------*/ + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +#ifdef __cplusplus +} +#endif + +#endif /* __MAIN_H */ diff --git a/Core/Src/main.c b/Core/Src/main.c new file mode 100644 index 0000000..c6334c7 --- /dev/null +++ b/Core/Src/main.c @@ -0,0 +1,903 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 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 "main.h" +#include "cmsis_os.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 ---------------------------------------------------------*/ +I2C_HandleTypeDef hi2c1; + +TIM_HandleTypeDef htim3; +TIM_HandleTypeDef htim4; +TIM_HandleTypeDef htim5; + +UART_HandleTypeDef huart6; + +/* Definitions for defaultTask */ +osThreadId_t defaultTaskHandle; +const osThreadAttr_t defaultTask_attributes = { + .name = "defaultTask", + .stack_size = 128 * 4, + .priority = (osPriority_t) osPriorityNormal, +}; +/* Definitions for balance */ +osThreadId_t balanceHandle; +const osThreadAttr_t balance_attributes = { + .name = "balance", + .stack_size = 128 * 4, + .priority = (osPriority_t) osPriorityLow, +}; +/* Definitions for motor */ +osThreadId_t motorHandle; +const osThreadAttr_t motor_attributes = { + .name = "motor", + .stack_size = 128 * 4, + .priority = (osPriority_t) osPriorityLow, +}; +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +void SystemClock_Config(void); +static void MX_GPIO_Init(void); +static void MX_I2C1_Init(void); +static void MX_TIM3_Init(void); +static void MX_TIM4_Init(void); +static void MX_TIM5_Init(void); +static void MX_USART6_UART_Init(void); +void StartDefaultTask(void *argument); +void start_balance(void *argument); +void start_motor(void *argument); + +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ +#define TIM4_ARR_VALUE 1024 +uint32_t TIM4_CurrentCount; +uint32_t TIM4_PreviousCount = 0; +#define TS 0.01 +#define TSmillis TS*1000 +#define M_PI 3.1415 +float encoder_TIM4_speed_rad() { + int32_t TIM4_DiffCount; + TIM4_CurrentCount = __HAL_TIM_GET_COUNTER(&htim4); + /* evaluate increment of TIM3 counter from previous count */ + if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim4)) { + /* check for counter underflow */ + if (TIM4_CurrentCount <= TIM4_PreviousCount) + TIM4_DiffCount = TIM4_CurrentCount - TIM4_PreviousCount; + else + TIM4_DiffCount = -((TIM4_ARR_VALUE + 1) - TIM4_CurrentCount) + - TIM4_PreviousCount; + } else { + /* check for counter overflow */ + if (TIM4_CurrentCount >= TIM4_PreviousCount) + TIM4_DiffCount = TIM4_CurrentCount - TIM4_PreviousCount; + else + TIM4_DiffCount = ((TIM4_ARR_VALUE + 1) - TIM4_PreviousCount) + + TIM4_CurrentCount; + } + TIM4_PreviousCount = TIM4_CurrentCount; + + return (float) TIM4_DiffCount / (TIM4_ARR_VALUE * TS) * 2 * M_PI; +} +#define TIM1_ARR_VALUE 1024 +uint32_t TIM1_CurrentCount; +uint32_t TIM1_PreviousCount = 0; +float encoder_TIM1_speed_rad() { + int32_t TIM1_DiffCount; + TIM1_CurrentCount = __HAL_TIM_GET_COUNTER(&htim3); + /* evaluate increment of TIM3 counter from previous count */ + if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim3)) { + /* check for counter underflow */ + if (TIM1_CurrentCount <= TIM1_PreviousCount) + TIM1_DiffCount = TIM1_CurrentCount - TIM1_PreviousCount; + else + TIM1_DiffCount = -((TIM1_ARR_VALUE + 1) - TIM1_CurrentCount) + - TIM1_PreviousCount; + } else { + /* check for counter overflow */ + if (TIM1_CurrentCount >= TIM1_PreviousCount) + TIM1_DiffCount = TIM1_CurrentCount - TIM1_PreviousCount; + else + TIM1_DiffCount = ((TIM1_ARR_VALUE + 1) - TIM1_PreviousCount) + + TIM1_CurrentCount; + } + TIM1_PreviousCount = TIM1_CurrentCount; + //TIM1->SR&=~TIM_SR_BIF; + //TIM1->SR&=~TIM_SR_COMIF; + //TIM1->SR=1606; + + return (float) TIM1_DiffCount / (TIM1_ARR_VALUE * TS) * 2 * M_PI; +} +#define TIM5_ARR_VALUE 4000 +void set_pwm(float duty, char motor, char mode) { + // Limit the duty cycle to ±70% + duty = fabsf(duty) > 0.7f ? (duty / fabsf(duty)) * 0.7f : duty; + + if (motor == 'r') { + duty = -duty; + if (duty >= 0) { // rotate forward + if (mode == 'c') { // coast + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, + duty * TIM5_ARR_VALUE); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, 0); + } else if (mode == 'b') { // brake + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, TIM5_ARR_VALUE); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, + TIM5_ARR_VALUE - duty * TIM5_ARR_VALUE); + } + } else { // rotate backward + if (mode == 'c') { // coast + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, 0); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, + (-duty) * TIM5_ARR_VALUE); + } else if (mode == 'b') { // brake + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, + TIM5_ARR_VALUE - (-duty) * TIM5_ARR_VALUE); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, TIM5_ARR_VALUE); + } + } + } + + if (motor == 'l') { + duty = -duty; + if (duty >= 0) { // rotate forward + if (mode == 'c') { + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, + duty * TIM5_ARR_VALUE); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_4, 0); + } else if (mode == 'b') { + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, TIM5_ARR_VALUE); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_4, + TIM5_ARR_VALUE - duty * TIM5_ARR_VALUE); + } + } else { // rotate backward + if (mode == 'c') { + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, 0); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_4, + (-duty) * TIM5_ARR_VALUE); + } else if (mode == 'b') { + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, + TIM5_ARR_VALUE - (-duty) * TIM5_ARR_VALUE); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_4, TIM5_ARR_VALUE); + } + } + } +} +/* USER CODE END 0 */ + +/** + * @brief The application entry point. + * @retval int + */ +int main(void) +{ + + /* USER CODE BEGIN 1 */ + + /* 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_I2C1_Init(); + MX_TIM3_Init(); + MX_TIM4_Init(); + MX_TIM5_Init(); + MX_USART6_UART_Init(); + /* USER CODE BEGIN 2 */ + + /* USER CODE END 2 */ + + /* Init scheduler */ + osKernelInitialize(); + + /* USER CODE BEGIN RTOS_MUTEX */ + /* add mutexes, ... */ + /* USER CODE END RTOS_MUTEX */ + + /* USER CODE BEGIN RTOS_SEMAPHORES */ + /* add semaphores, ... */ + /* USER CODE END RTOS_SEMAPHORES */ + + /* USER CODE BEGIN RTOS_TIMERS */ + /* start timers, add new ones, ... */ + /* USER CODE END RTOS_TIMERS */ + + /* USER CODE BEGIN RTOS_QUEUES */ + /* add queues, ... */ + /* USER CODE END RTOS_QUEUES */ + + /* Create the thread(s) */ + /* creation of defaultTask */ + defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); + + /* creation of balance */ + balanceHandle = osThreadNew(start_balance, NULL, &balance_attributes); + + /* creation of motor */ + motorHandle = osThreadNew(start_motor, NULL, &motor_attributes); + + /* USER CODE BEGIN RTOS_THREADS */ + /* add threads, ... */ + /* USER CODE END RTOS_THREADS */ + + /* USER CODE BEGIN RTOS_EVENTS */ + /* add events, ... */ + /* USER CODE END RTOS_EVENTS */ + + /* Start scheduler */ + osKernelStart(); + + /* We should never get here as control is now taken by the scheduler */ + + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) { + /* 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}; + + /** Configure the main internal regulator output voltage + */ + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2); + + /** 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_NONE; + 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_HSI; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) + { + Error_Handler(); + } +} + +/** + * @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.ClockSpeed = 100000; + hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; + hi2c1.Init.OwnAddress1 = 0; + hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + hi2c1.Init.OwnAddress2 = 0; + hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + if (HAL_I2C_Init(&hi2c1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN I2C1_Init 2 */ + + /* USER CODE END I2C1_Init 2 */ + +} + +/** + * @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 */ + HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); + /* USER CODE END TIM3_Init 2 */ + +} + +/** + * @brief TIM4 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM4_Init(void) +{ + + /* USER CODE BEGIN TIM4_Init 0 */ + + /* USER CODE END TIM4_Init 0 */ + + TIM_Encoder_InitTypeDef sConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + + /* USER CODE BEGIN TIM4_Init 1 */ + + /* USER CODE END TIM4_Init 1 */ + htim4.Instance = TIM4; + htim4.Init.Prescaler = 0; + htim4.Init.CounterMode = TIM_COUNTERMODE_UP; + htim4.Init.Period = 65535; + htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim4.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(&htim4, &sConfig) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM4_Init 2 */ + HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); + /* USER CODE END TIM4_Init 2 */ + +} + +/** + * @brief TIM5 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM5_Init(void) +{ + + /* USER CODE BEGIN TIM5_Init 0 */ + + /* USER CODE END TIM5_Init 0 */ + + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_OC_InitTypeDef sConfigOC = {0}; + + /* USER CODE BEGIN TIM5_Init 1 */ + + /* USER CODE END TIM5_Init 1 */ + htim5.Instance = TIM5; + htim5.Init.Prescaler = 15; + htim5.Init.CounterMode = TIM_COUNTERMODE_UP; + htim5.Init.Period = 4000; + htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_PWM_Init(&htim5) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &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(&htim5, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM5_Init 2 */ + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, 100); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, 100); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, 100); + __HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_4, 100); + + HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_4); + + /* USER CODE END TIM5_Init 2 */ + HAL_TIM_MspPostInit(&htim5); + +} + +/** + * @brief USART6 Initialization Function + * @param None + * @retval None + */ +static void MX_USART6_UART_Init(void) +{ + + /* USER CODE BEGIN USART6_Init 0 */ + + /* USER CODE END USART6_Init 0 */ + + /* USER CODE BEGIN USART6_Init 1 */ + + /* USER CODE END USART6_Init 1 */ + huart6.Instance = USART6; + huart6.Init.BaudRate = 115200; + huart6.Init.WordLength = UART_WORDLENGTH_8B; + huart6.Init.StopBits = UART_STOPBITS_1; + huart6.Init.Parity = UART_PARITY_NONE; + huart6.Init.Mode = UART_MODE_TX_RX; + huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart6.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart6) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART6_Init 2 */ + + /* USER CODE END USART6_Init 2 */ + +} + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +static void MX_GPIO_Init(void) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + /* USER CODE BEGIN MX_GPIO_Init_1 */ + + /* USER CODE END MX_GPIO_Init_1 */ + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET); + + /*Configure GPIO pin : PC13 */ + GPIO_InitStruct.Pin = GPIO_PIN_13; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + /* USER CODE BEGIN MX_GPIO_Init_2 */ + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET); //turn it on + /* USER CODE END MX_GPIO_Init_2 */ +} + +/* USER CODE BEGIN 4 */ + +/* USER CODE END 4 */ + +/* USER CODE BEGIN Header_StartDefaultTask */ +/** + * @brief Function implementing the defaultTask thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_StartDefaultTask */ +void StartDefaultTask(void *argument) +{ + /* USER CODE BEGIN 5 */ + /* Infinite loop */ + for (;;) { + osDelay(500); + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET); //turn it on + osDelay(500); + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET); //turn it on + } + /* USER CODE END 5 */ +} + +/* USER CODE BEGIN Header_start_balance */ +struct motorstruct { + float wr, ur; + float wl, ul; +} motor; + +#define MPU6050_ADDR (0x68 << 1) +#define MPU6050_REG_DATA 0x3B +#define MPU6050_PWR_MGMT_1 0x6B + +// Sensitivity (default ±2g, ±250°/s) +#define ACCEL_SENS 16384.0f +#define GYRO_SENS 131.0f + +typedef struct { + float accel[3]; // g + float gyro[3]; // °/s + float temp; // °C +} MPU6050_Data; + +void MPU6050_Init(void) { + uint8_t wake = 0x00; + HAL_I2C_Mem_Write(&hi2c1, + MPU6050_ADDR, + MPU6050_PWR_MGMT_1, + I2C_MEMADD_SIZE_8BIT, &wake, 1, 100); + HAL_Delay(100); +} + +HAL_StatusTypeDef MPU6050_Read(MPU6050_Data *data) { + uint8_t raw[14]; + HAL_StatusTypeDef ret; + + ret = HAL_I2C_Mem_Read(&hi2c1, + MPU6050_ADDR, + MPU6050_REG_DATA, + I2C_MEMADD_SIZE_8BIT, raw, 14, 100); + + if (ret != HAL_OK) + return ret; + + int16_t accel_raw[3], gyro_raw[3], temp_raw; + + accel_raw[0] = (int16_t) (raw[0] << 8 | raw[1]); + accel_raw[1] = (int16_t) (raw[2] << 8 | raw[3]); + accel_raw[2] = (int16_t) (raw[4] << 8 | raw[5]); + + temp_raw = (int16_t) (raw[6] << 8 | raw[7]); + + gyro_raw[0] = (int16_t) (raw[8] << 8 | raw[9]); + gyro_raw[1] = (int16_t) (raw[10] << 8 | raw[11]); + gyro_raw[2] = (int16_t) (raw[12] << 8 | raw[13]); + + // Scale to real units + data->accel[0] = accel_raw[0] / ACCEL_SENS; + data->accel[1] = accel_raw[1] / ACCEL_SENS; + data->accel[2] = accel_raw[2] / ACCEL_SENS; + + data->gyro[0] = gyro_raw[0] / GYRO_SENS; + data->gyro[1] = gyro_raw[1] / GYRO_SENS; + data->gyro[2] = gyro_raw[2] / GYRO_SENS; + + data->temp = (temp_raw / 340.0f) + 36.53f; + + return HAL_OK; +} +/** + * @brief Function implementing the balance thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_start_balance */ +void start_balance(void *argument) +{ + /* USER CODE BEGIN start_balance */ + /* Infinite loop */ + MPU6050_Data sensor; + MPU6050_Init(); + + HAL_Delay(500); + float alpha = 0.9; + float Kp = 5; //80 good + float Ki = 5 * 0.15*0; + float Kd = 100 * 0; + + float integral; + float sat_diff; + float pitch; + float last_pitch; + float Kw = 0.01; + + float position; + float position_kp = -10; + for (;;) { + vTaskDelay(TSmillis); + if (MPU6050_Read(&sensor) == HAL_OK) { + // Now sensor.accel[], sensor.gyro[], sensor.temp are ready to use + // Example: print over UART, use in control loop, etc. + // XYZ + float gyro_pitch = sensor.gyro[0] * (M_PI / 180.0f) * TS; // convert °/s → rad/s + float acc_pitch = atan2f(sensor.accel[1], sensor.accel[2]); + pitch = alpha * (pitch + gyro_pitch) + (1.0f - alpha) * acc_pitch; + + float error = pitch; + // + // float anti_windup_1 = Ki * error1 - sat_diff_1 * Kw; + // integral1 += anti_windup_1 * TS; + // float control1 = integral1 + Kp * error1; + // float unsat_control_1 = control1; + // control1 = + // fabsf(control1) > 0.7f ? + // (control1 / fabsf(control1)) * 0.7f : control1; + // sat_diff_1 = unsat_control_1 - control1; + integral += error; + float control = Kp * error + integral * Ki + + (last_pitch - pitch) * Kd + position * position_kp; + + set_pwm(control, 'l', 'c'); + set_pwm(-control , 'r', 'c'); + + + //motor.ul = motor.ur; + + last_pitch = pitch; + + position += motor.wl * 0.03 * 2 * M_PI * TS; //3cm wheel radius + + } else { + //motor.ur = 0; + //motor.ul = motor.ur + set_pwm(0, 'l', 'c'); + set_pwm(0, 'r', 'c'); + } + + //char uart_buf[20]; + //int len = sprintf(uart_buf, "%.3fp\r\n", pitch); + //HAL_UART_Transmit(&huart6, (uint8_t*) uart_buf, len, HAL_MAX_DELAY); + + } + /* USER CODE END start_balance */ +} + +/* USER CODE BEGIN Header_start_motor */ +/** + * @brief Function implementing the motor thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_start_motor */ +void start_motor(void *argument) +{ + /* USER CODE BEGIN start_motor */ + /* Infinite loop */ + float tempspeed; + float Ki = 4; + float Kp = 0.005 * 10; + float Kw = 5; + float integral1 = 0; + float integral2 = 0; + float sat_diff_1 = 0; + float sat_diff_2 = 0; + static char statsBuffer[200]; + motor.ul = 0.0 * M_PI; + motor.ur = 0.0 * M_PI; + + for (;;) { + vTaskDelay(TSmillis); + + //vTaskGetRunTimeStats(statsBuffer); + //motor.wr = encoder_TIM3_speed_rad(); + + tempspeed = -encoder_TIM4_speed_rad(); + if (fabsf(tempspeed - motor.wr) < 700.0f) { + motor.wr = tempspeed; + + } + tempspeed = encoder_TIM1_speed_rad(); + if (fabsf(tempspeed - motor.wl) > 70.0f) { + + } else { + motor.wl = tempspeed; + } + + // Calculation for Motor 1 control signal + float error1 = (-1) * motor.ur - motor.wr; + if (error1 > 10) { + int t = 0; + } + float anti_windup_1 = Ki * error1 - sat_diff_1 * Kw; + integral1 += anti_windup_1 * TS; + float control1 = integral1 + Kp * error1; + float unsat_control_1 = control1; + control1 = + fabsf(control1) > 0.7f ? + (control1 / fabsf(control1)) * 0.7f : control1; + sat_diff_1 = unsat_control_1 - control1; + + //set_pwm(control1, 'r', 'c'); + //set_pwm(0.15, 'r', 'c'); + + // Calculation for Motor 2 control signal + float error2 = motor.ul - motor.wl; + if (error2 > 10) { + int t = 0; + } + float anti_windup_2 = Ki * error2 - sat_diff_2 * Kw; + integral2 += anti_windup_2 * TS; + float control2 = integral2 + Kp * error2; + float unsat_control_2 = control2; + control2 = + fabsf(control2) > 0.7f ? + (control2 / fabsf(control2)) * 0.7f : control2; + sat_diff_2 = unsat_control_2 - control2; + + //set_pwm(control2, 'l', 'c'); + //set_pwm(0.15, 'l', 'c'); + + } + /* USER CODE END start_motor */ +} + +/** + * @brief Period elapsed callback in non blocking mode + * @note This function is called when TIM10 interrupt took place, inside + * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment + * a global variable "uwTick" used as application time base. + * @param htim : TIM handle + * @retval None + */ +void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) +{ + /* USER CODE BEGIN Callback 0 */ + + /* USER CODE END Callback 0 */ + if (htim->Instance == TIM10) + { + HAL_IncTick(); + } + /* USER CODE BEGIN Callback 1 */ + + /* USER CODE END Callback 1 */ +} + +/** + * @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 */ diff --git a/balance.ioc b/balance.ioc new file mode 100644 index 0000000..618e665 --- /dev/null +++ b/balance.ioc @@ -0,0 +1,181 @@ +#MicroXplorer Configuration settings - do not modify +CAD.formats= +CAD.pinconfig= +CAD.provider= +FREERTOS.FootprintOK=true +FREERTOS.IPParameters=Tasks01,configUSE_NEWLIB_REENTRANT,FootprintOK +FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL;balance,8,128,start_balance,Default,NULL,Dynamic,NULL,NULL;motor,8,128,start_motor,Default,NULL,Dynamic,NULL,NULL +FREERTOS.configUSE_NEWLIB_REENTRANT=1 +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +Mcu.CPN=STM32F401CCU6TR +Mcu.Family=STM32F4 +Mcu.IP0=FREERTOS +Mcu.IP1=I2C1 +Mcu.IP2=NVIC +Mcu.IP3=RCC +Mcu.IP4=SYS +Mcu.IP5=TIM3 +Mcu.IP6=TIM4 +Mcu.IP7=TIM5 +Mcu.IP8=USART6 +Mcu.IPNb=9 +Mcu.Name=STM32F401C(B-C)Ux +Mcu.Package=UFQFPN48 +Mcu.Pin0=PC13-ANTI_TAMP +Mcu.Pin1=PA0-WKUP +Mcu.Pin10=PA14 +Mcu.Pin11=PA15 +Mcu.Pin12=PB3 +Mcu.Pin13=PB6 +Mcu.Pin14=PB7 +Mcu.Pin15=PB8 +Mcu.Pin16=PB9 +Mcu.Pin17=VP_FREERTOS_VS_CMSIS_V2 +Mcu.Pin18=VP_SYS_VS_tim10 +Mcu.Pin2=PA1 +Mcu.Pin3=PA2 +Mcu.Pin4=PA3 +Mcu.Pin5=PA6 +Mcu.Pin6=PA7 +Mcu.Pin7=PA11 +Mcu.Pin8=PA12 +Mcu.Pin9=PA13 +Mcu.PinsNb=19 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32F401CCUx +MxCube.Version=6.15.0 +MxDb.Version=DB.6.0.150 +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false +NVIC.SavedPendsvIrqHandlerGenerated=true +NVIC.SavedSvcallIrqHandlerGenerated=true +NVIC.SavedSystickIrqHandlerGenerated=true +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false +NVIC.TIM1_UP_TIM10_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true +NVIC.TimeBase=TIM1_UP_TIM10_IRQn +NVIC.TimeBaseIP=TIM10 +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +PA0-WKUP.Signal=S_TIM5_CH1 +PA1.Signal=S_TIM5_CH2 +PA11.Mode=Asynchronous +PA11.Signal=USART6_TX +PA12.Mode=Asynchronous +PA12.Signal=USART6_RX +PA13.Mode=JTAG_4_pins +PA13.Signal=SYS_JTMS-SWDIO +PA14.Mode=JTAG_4_pins +PA14.Signal=SYS_JTCK-SWCLK +PA15.Mode=JTAG_4_pins +PA15.Signal=SYS_JTDI +PA2.Signal=S_TIM5_CH3 +PA3.Signal=S_TIM5_CH4 +PA6.Signal=S_TIM3_CH1 +PA7.Signal=S_TIM3_CH2 +PB3.Mode=JTAG_4_pins +PB3.Signal=SYS_JTDO-SWO +PB6.Signal=S_TIM4_CH1 +PB7.Signal=S_TIM4_CH2 +PB8.Mode=I2C +PB8.Signal=I2C1_SCL +PB9.Mode=I2C +PB9.Signal=I2C1_SDA +PC13-ANTI_TAMP.Locked=true +PC13-ANTI_TAMP.Signal=GPIO_Output +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=false +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32F401CCUx +ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.28.3 +ProjectManager.FreePins=false +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x200 +ProjectManager.KeepUserCode=true +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Core/Src +ProjectManager.NoMain=false +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=balance.ioc +ProjectManager.ProjectName=balance +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x400 +ProjectManager.TargetToolchain=STM32CubeIDE +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_I2C1_Init-I2C1-false-HAL-true,4-MX_TIM3_Init-TIM3-false-HAL-true,5-MX_TIM4_Init-TIM4-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true +RCC.AHBFreq_Value=16000000 +RCC.APB1Freq_Value=16000000 +RCC.APB2Freq_Value=16000000 +RCC.CortexFreq_Value=16000000 +RCC.HSE_VALUE=25000000 +RCC.HSI_VALUE=16000000 +RCC.I2SClocksFreq_Value=96000000 +RCC.IPParameters=AHBFreq_Value,APB1Freq_Value,APB2Freq_Value,CortexFreq_Value,HSE_VALUE,HSI_VALUE,I2SClocksFreq_Value,LSE_VALUE,LSI_VALUE,PLLCLKFreq_Value,PLLQCLKFreq_Value,RTCFreq_Value,RTCHSEDivFreq_Value,SYSCLKFreq_VALUE,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VcooutputI2S +RCC.LSE_VALUE=32768 +RCC.LSI_VALUE=32000 +RCC.PLLCLKFreq_Value=96000000 +RCC.PLLQCLKFreq_Value=48000000 +RCC.RTCFreq_Value=32000 +RCC.RTCHSEDivFreq_Value=12500000 +RCC.SYSCLKFreq_VALUE=16000000 +RCC.VCOI2SOutputFreq_Value=192000000 +RCC.VCOInputFreq_Value=1000000 +RCC.VCOOutputFreq_Value=192000000 +RCC.VcooutputI2S=96000000 +SH.S_TIM3_CH1.0=TIM3_CH1,Encoder_Interface +SH.S_TIM3_CH1.ConfNb=1 +SH.S_TIM3_CH2.0=TIM3_CH2,Encoder_Interface +SH.S_TIM3_CH2.ConfNb=1 +SH.S_TIM4_CH1.0=TIM4_CH1,Encoder_Interface +SH.S_TIM4_CH1.ConfNb=1 +SH.S_TIM4_CH2.0=TIM4_CH2,Encoder_Interface +SH.S_TIM4_CH2.ConfNb=1 +SH.S_TIM5_CH1.0=TIM5_CH1,PWM Generation1 CH1 +SH.S_TIM5_CH1.ConfNb=1 +SH.S_TIM5_CH2.0=TIM5_CH2,PWM Generation2 CH2 +SH.S_TIM5_CH2.ConfNb=1 +SH.S_TIM5_CH3.0=TIM5_CH3,PWM Generation3 CH3 +SH.S_TIM5_CH3.ConfNb=1 +SH.S_TIM5_CH4.0=TIM5_CH4,PWM Generation4 CH4 +SH.S_TIM5_CH4.ConfNb=1 +TIM3.EncoderMode=TIM_ENCODERMODE_TI12 +TIM3.IPParameters=EncoderMode +TIM4.EncoderMode=TIM_ENCODERMODE_TI12 +TIM4.IPParameters=EncoderMode +TIM5.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM5.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 +TIM5.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 +TIM5.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4 +TIM5.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Channel-PWM Generation4 CH4,Prescaler,Period +TIM5.Period=4000 +TIM5.Prescaler=15 +USART6.IPParameters=VirtualMode +USART6.VirtualMode=VM_ASYNC +VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 +VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 +VP_SYS_VS_tim10.Mode=TIM10 +VP_SYS_VS_tim10.Signal=SYS_VS_tim10 +board=custom +rtos.0.ip=FREERTOS +isbadioc=false