lucordes/touchscreengames
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
91a7a8529a
touchscreengames/touchscreengames/Core/Src/main.c
lucordes 91a7a8529a direct control
2025-09-06 19:42:33 +02:00

1326 lines
36 KiB
C
Raw Blame History

/* 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 */
//#include "ili9341.h"
#include "ILI9341_STM32_Driver.h"
#include "ILI9341_GFX.h"
#include "snake.h"
/* 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;
SPI_HandleTypeDef hspi2;
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
TIM_HandleTypeDef htim5;
UART_HandleTypeDef huart6;
/* Definitions for control */
osThreadId_t controlHandle;
const osThreadAttr_t control_attributes = { .name = "control", .stack_size = 512
* 4, .priority = (osPriority_t) osPriorityBelowNormal, };
/* Definitions for draw */
osThreadId_t drawHandle;
const osThreadAttr_t draw_attributes = { .name = "draw", .stack_size = 512 * 4,
.priority = (osPriority_t) osPriorityLow, };
/* Definitions for menu */
osThreadId_t menuHandle;
const osThreadAttr_t menu_attributes = { .name = "menu", .stack_size = 256 * 4,
.priority = (osPriority_t) osPriorityLow, };
/* Definitions for balance */
osThreadId_t balanceHandle;
const osThreadAttr_t balance_attributes = { .name = "balance", .stack_size = 128
* 4, .priority = (osPriority_t) osPriorityLow7, };
/* Definitions for presses */
osMessageQueueId_t pressesHandle;
const osMessageQueueAttr_t presses_attributes = { .name = "presses" };
/* Definitions for display */
osMutexId_t displayHandle;
const osMutexAttr_t display_attributes = { .name = "display" };
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM4_Init(void);
static void MX_TIM5_Init(void);
static void MX_USART6_UART_Init(void);
static void MX_I2C1_Init(void);
static void MX_TIM3_Init(void);
static void MX_SPI2_Init(void);
static void MX_TIM1_Init(void);
void start_control(void *argument);
void snakegame(void *argument);
void handle_menu(void *argument);
void start_balance(void *argument);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int run_snake = 0;
int run_menu = 1;
#define RX_BUFFER_SIZE 1
#define LINE_BUFFER_SIZE 128
uint8_t rx_buffer[RX_BUFFER_SIZE];
char line_buffer[LINE_BUFFER_SIZE];
uint16_t line_pos = 0;
//menu
int redraw_menu_pending;
#define GRID_X 2
#define GRID_Y 3
uint8_t menu_status[GRID_X][GRID_Y] = { 0 };
struct motorstruct {
float wr, ur;
float wl, ul;
} motor;
void process_line(const char *line) {
if (strncmp(line, "ABS:", 4) == 0) {
char axis[32];
int value;
if (sscanf(line, "ABS:%31[^:]:%d:", axis, &value) == 2) {
// Handle axis input
if (strcmp(axis, "ABS_X") == 0) {
motor.ul = 2 * 3.1415 / 32000.1 * value;
// Do something with X axis
} else if (strcmp(axis, "ABS_Y") == 0) {
// Do something with Y axis
}
}
} else if (strncmp(line, "KEY:", 4) == 0) {
char key[32];
int state;
if (sscanf(line, "KEY:%31[^:]:%d:", key, &state) == 2) {
// Handle button press/release
//we only care about presses
if (state) {
char dir;
dir = key[0];
if (dir == 'L' || dir == 'R' || dir == 'D' || dir == 'U'
|| dir == 'A') {
if (dir == 'U') {
motor.ul = -1 * motor.ul;
}
osStatus_t status = osMessageQueuePut(pressesHandle, &dir,
0, 0);
if (status != osOK) {
// Optional: Handle error
}
}
}
if (key[0] == 'B' && state == 1) { // B goes to the menu
run_snake = 0;
run_menu = 1;
redraw_menu_pending = 1;
}
if (key[0] == 'Y' && state == 1) { // Y reboot
}
}
}
}
void UART_CALLBACK() {
char ch = rx_buffer[0];
if (ch == '\r') {
// ignore
} else if (ch == '\n') {
//line_buffer[line_pos] = '\0';
if (line_buffer[1] == 'K' || line_buffer[1] == 'A') {
process_line(&line_buffer[1]);
} else {
process_line(line_buffer);
}
line_pos = 0;
} else if (line_pos < LINE_BUFFER_SIZE - 1) {
line_buffer[line_pos++] = ch;
}
HAL_UART_Receive_IT(&huart6, rx_buffer, RX_BUFFER_SIZE);
}
#define TIM4_ARR_VALUE 1024
uint32_t TIM4_CurrentCount;
uint32_t TIM4_PreviousCount = 0;
#define TS 0.005
#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_TIM4_Init();
MX_TIM5_Init();
MX_USART6_UART_Init();
MX_I2C1_Init();
MX_TIM3_Init();
MX_SPI2_Init();
MX_TIM1_Init();
/* USER CODE BEGIN 2 */
ILI9341_Init();
ILI9341_SetRotation(SCREEN_HORIZONTAL_1);
//ILI9341_FillScreen(WHITE);
//void XPT2046_Init();
//Flash_Write_HighScore(100);
/* USER CODE END 2 */
/* Init scheduler */
osKernelInitialize();
/* Create the mutex(es) */
/* creation of display */
displayHandle = osMutexNew(&display_attributes);
/* 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 */
/* Create the queue(s) */
/* creation of presses */
pressesHandle = osMessageQueueNew(16, sizeof(char), &presses_attributes);
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* creation of control */
controlHandle = osThreadNew(start_control, NULL, &control_attributes);
/* creation of draw */
drawHandle = osThreadNew(snakegame, NULL, &draw_attributes);
/* creation of menu */
menuHandle = osThreadNew(handle_menu, NULL, &menu_attributes);
/* creation of balance */
balanceHandle = osThreadNew(start_balance, NULL, &balance_attributes);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
char dir;
// check the queue blocking
//osStatus_t status = osMessageQueueGet(pressesHandle, &dir, NULL,
//osWaitForever); // blocking
//if (dir == 'L')
// drawHandle = osThreadNew(snakegame, NULL, &draw_attributes);
//if (dir == 'R')
//reactiongameHandle = osThreadNew(startreactiongame, NULL,
// &reactiongame_attributes);
/* 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 */
//int a[4] = { ILI9341_GREEN, ILI9341_WHITE, ILI9341_BLACK, ILI9341_BLUE };
int cnt;
//while (hspi1.State != HAL_SPI_STATE_READY)
//;
//while (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
//;
while (1) {
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
//tempspeed = encoder_TIM4_speed_rad();
HAL_Delay(10);
}
/* 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 */
//HAL_RCC_I2C1_FORCE_RESET();
HAL_Delay(10);
//HAL_RCC_I2C1_RELEASE_RESET();
/* 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 SPI2 Initialization Function
* @param None
* @retval None
*/
static void MX_SPI2_Init(void) {
/* USER CODE BEGIN SPI2_Init 0 */
/* USER CODE END SPI2_Init 0 */
/* USER CODE BEGIN SPI2_Init 1 */
/* USER CODE END SPI2_Init 1 */
/* SPI2 parameter configuration*/
hspi2.Instance = SPI2;
hspi2.Init.Mode = SPI_MODE_MASTER;
hspi2.Init.Direction = SPI_DIRECTION_2LINES;
hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
hspi2.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi2.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi2.Init.NSS = SPI_NSS_SOFT;
hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi2.Init.CRCPolynomial = 10;
if (HAL_SPI_Init(&hspi2) != HAL_OK) {
Error_Handler();
}
/* USER CODE BEGIN SPI2_Init 2 */
/* USER CODE END SPI2_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_Encoder_InitTypeDef sConfig = { 0 };
TIM_MasterConfigTypeDef sMasterConfig = { 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 = 65535;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
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(&htim1, &sConfig) != HAL_OK) {
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig)
!= HAL_OK) {
Error_Handler();
}
/* USER CODE BEGIN TIM1_Init 2 */
/* USER CODE END TIM1_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 = 15;
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 = 15;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 15;
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 */
/* Start motor PWM */
__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 */
uint8_t rx_byte;
HAL_UART_Receive_IT(&huart6, &rx_byte, 1);
/* 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(LED_BLUE_GPIO_Port, LED_BLUE_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOA, DISPLAY_CS_Pin | DISPLAY_RES_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(DISPLAY_DC_GPIO_Port, DISPLAY_DC_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : LED_BLUE_Pin */
GPIO_InitStruct.Pin = LED_BLUE_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(LED_BLUE_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : DISPLAY_CS_Pin DISPLAY_RES_Pin */
GPIO_InitStruct.Pin = DISPLAY_CS_Pin | DISPLAY_RES_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pin : DISPLAY_DC_Pin */
GPIO_InitStruct.Pin = DISPLAY_DC_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(DISPLAY_DC_GPIO_Port, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/* USER CODE BEGIN Header_start_control */
/**
* @brief Function implementing the control thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_start_control */
void start_control(void *argument) {
/* USER CODE BEGIN 5 */
/* 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;
// char uart_buf[20];
// int whole = (int) motor.wr;
// int frac = (int) ((motor.wr - whole) * 1000); // 3 decimal digits
//
// if (frac < 0)
// frac = -frac; // handle negative numbers
////
// int len = 0;
// if (motor.wr < 0 && whole == 0)
// len = sprintf(uart_buf, "-0.%03d\r\n", frac);
// else
// len = sprintf(uart_buf, "%d.%03d\r\n", whole, frac);
//
// HAL_UART_Transmit(&huart6, (uint8_t*) uart_buf, len, HAL_MAX_DELAY);
}
tempspeed = encoder_TIM1_speed_rad();
if (fabsf(tempspeed - motor.wl) > 70.0f) {
} else {
motor.wl = tempspeed;
// char uart_buf[20];
// int whole = (int) motor.wl;
// int frac = (int) ((motor.wl - whole) * 1000); // 3 decimal digits
//
// if (frac < 0)
// frac = -frac; // handle negative numbers
////
// int len = 0;
// if (motor.wl < 0 && whole == 0)
// len = sprintf(uart_buf, "-0.%03d\r\n", frac);
// else
// len = sprintf(uart_buf, "%d.%03d\r\n", whole, frac);
//
// HAL_UART_Transmit(&huart6, (uint8_t*) uart_buf, len, HAL_MAX_DELAY);
}
// 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 5 */
}
/* USER CODE BEGIN Header_snakegame */
/**
* @brief Function implementing the draw thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_snakegame */
void snakegame(void *argument) {
/* USER CODE BEGIN snakegame */
/* Infinite loop */
//ILI9341_FillScreen(BLACK);
GameInit();
for (;;) {
if (run_snake) {
GameRender();
static char statsBuffer[200];
vTaskGetRunTimeStats(statsBuffer);
vTaskDelay(300);
if (isGameOver()) {
vTaskDelay(2000);
GameInit();
}
UpdateGame();
} else {
vTaskDelay(10);
}
}
/* USER CODE END snakegame */
}
/* USER CODE BEGIN Header_handle_menu */
/**
* @brief Function implementing the menu thread.
* @param argument: Not used
* @retval None
*/
#define CELL_WIDTH 120 // Adjust as needed for your screen
#define CELL_HEIGHT 40
#define START_X 10
#define START_Y 10
#define COLOR_TEXT BLACK
#define COLOR_BG WHITE
#define COLOR_SELECTED GREEN
#define COLOR_ACTIVATED RED
#define COLOR_ACTIVATED_AND_SELECTED MAGENTA
int sel_x;
int sel_y;
//redraw_menu_pending is for redrawing
// Customize grid labels as needed
const char *grid_labels[GRID_X][GRID_Y] = { { "Snake", "Options", "About" }, {
"Highscore", "Credits", "Exit" } };
void redraw_menu() { //only done onece saves resources
// Redraw grid
if (osMutexAcquire(displayHandle, osWaitForever) == osOK) {
ILI9341_FillScreen(WHITE);
osMutexRelease(displayHandle);
}
for (int i = 0; i < GRID_X; i++) {
for (int j = 0; j < GRID_Y; j++) {
uint16_t x = START_X + i * CELL_WIDTH;
uint16_t y = START_Y + j * CELL_HEIGHT;
uint16_t bg;
if (menu_status[i][j]) {
if (i == sel_x && j == sel_y) {
bg = COLOR_ACTIVATED_AND_SELECTED;
} else {
bg = COLOR_ACTIVATED;
}
} else {
if (i == sel_x && j == sel_y) {
bg = COLOR_SELECTED;
} else {
bg = COLOR_BG;
}
}
if (osMutexAcquire(displayHandle, osWaitForever) == osOK) {
ILI9341_DrawText(grid_labels[i][j], FONT2, x, y, COLOR_TEXT,
bg);
osMutexRelease(displayHandle);
}
}
}
}
/* USER CODE END Header_handle_menu */
void handle_menu(void *argument) {
/* USER CODE BEGIN handle_menu */
/* Infinite loop */
char dir;
vTaskDelay(50);
redraw_menu();
while (1) {
// Check input
if (run_menu) {
osStatus_t status = osMessageQueueGet(pressesHandle, &dir, NULL,
osWaitForever);
if (status == osOK) {
//what was selected?
if (dir == 'A') {
if (grid_labels[sel_x][sel_y] == "Snake") {
run_snake = 1;
run_menu = 0;
}
menu_status[sel_x][sel_y] = !menu_status[sel_x][sel_y];
}
if (dir == 'U' || dir == 'D' || dir == 'L' || dir == 'R') {
if (menu_status[sel_x][sel_y]) {
if (osMutexAcquire(displayHandle, osWaitForever)
== osOK) {
ILI9341_DrawText(grid_labels[sel_x][sel_y], FONT2,
START_X + sel_x * CELL_WIDTH,
START_Y + sel_y * CELL_HEIGHT, COLOR_TEXT,
COLOR_ACTIVATED);
osMutexRelease(displayHandle);
}
} else {
if (osMutexAcquire(displayHandle, osWaitForever)
== osOK) {
ILI9341_DrawText(grid_labels[sel_x][sel_y], FONT2,
START_X + sel_x * CELL_WIDTH,
START_Y + sel_y * CELL_HEIGHT, COLOR_TEXT,
COLOR_BG);
osMutexRelease(displayHandle);
}
}
}
if (dir == 'U' && sel_y > 0)
sel_y--;
else if (dir == 'D' && sel_y < GRID_Y - 1)
sel_y++;
else if (dir == 'L' && sel_x > 0)
sel_x--;
else if (dir == 'R' && sel_x < GRID_X - 1)
sel_x++;
}
if (menu_status[sel_x][sel_y]) {
if (osMutexAcquire(displayHandle, osWaitForever) == osOK) {
ILI9341_DrawText(grid_labels[sel_x][sel_y], FONT2,
START_X + sel_x * CELL_WIDTH, START_Y + sel_y * CELL_HEIGHT,
COLOR_TEXT,
COLOR_ACTIVATED_AND_SELECTED);
osMutexRelease(displayHandle);
}
} else {
if (osMutexAcquire(displayHandle, osWaitForever) == osOK) {
ILI9341_DrawText(grid_labels[sel_x][sel_y], FONT2,
START_X + sel_x * CELL_WIDTH, START_Y + sel_y * CELL_HEIGHT,
COLOR_TEXT,
COLOR_SELECTED);
osMutexRelease(displayHandle);
}
}
if (redraw_menu_pending) {
redraw_menu();
redraw_menu_pending = 0;
}
}
osDelay(10);
}
/* USER CODE END handle_menu */
}
/* USER CODE BEGIN Header_start_balance */
#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;
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');
}
}
/* USER CODE END start_balance */
}
/**
* @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 */
Reference in New Issue View Git Blame Copy Permalink