-/* USER CODE BEGIN Header */\r
-/**\r
- ******************************************************************************\r
- * @file : main.c\r
- * @brief : Main program body\r
- ******************************************************************************\r
- * @attention\r
- *\r
- * <h2><center>© Copyright (c) 2019 STMicroelectronics.\r
- * All rights reserved.</center></h2>\r
- *\r
- * This software component is licensed by ST under BSD 3-Clause license,\r
- * the "License"; You may not use this file except in compliance with the\r
- * License. You may obtain a copy of the License at:\r
- * opensource.org/licenses/BSD-3-Clause\r
- *\r
- ******************************************************************************\r
- */\r
-/* USER CODE END Header */\r
-/* Includes ------------------------------------------------------------------*/\r
-#include "main.h"\r
-#include "crc.h"\r
-#include "dma.h"\r
-#include "tim.h"\r
-#include "usart.h"\r
-#include "gpio.h"\r
-\r
-/* Private includes ----------------------------------------------------------*/\r
-/* USER CODE BEGIN Includes */\r
-\r
-#include <string.h> // memcpy\r
-\r
-#include "control/encoder.h"\r
-#include "control/odometry.h"\r
-#include "control/motor_controller.h"\r
-#include "control/pid.h"\r
-\r
-#include "communication/otto_messages.h"\r
-\r
-/* USER CODE END Includes */\r
-\r
-/* Private typedef -----------------------------------------------------------*/\r
-/* USER CODE BEGIN PTD */\r
-\r
-/* USER CODE END PTD */\r
-\r
-/* Private define ------------------------------------------------------------*/\r
-/* USER CODE BEGIN PD */\r
-/* USER CODE END PD */\r
-\r
-/* Private macro -------------------------------------------------------------*/\r
-/* USER CODE BEGIN PM */\r
-\r
-/* USER CODE END PM */\r
-\r
-/* Private variables ---------------------------------------------------------*/\r
-\r
-/* USER CODE BEGIN PV */\r
-\r
-static Encoder encoders[2] = {\r
- {\r
- .timer = &htim5,\r
- },\r
- {\r
- .timer = &htim2,\r
- },\r
-};\r
-Encoder *encoder_right = &encoders[0];\r
-Encoder *encoder_left = &encoders[1];\r
-\r
-Odometry odom = {0};\r
-\r
-Pid pid_left = {0};\r
-Pid pid_right = {0};\r
-Pid pid_cross = {0};\r
-\r
-int32_t pid_max = 0;\r
-int32_t pid_min = 0;\r
-\r
-static MotorController motors[2] = {\r
- {\r
- // Right motor\r
- .sleep_gpio_port = sleep1_GPIO_Port,\r
- .sleep_pin = sleep1_Pin,\r
- .dir_gpio_port = dir1_GPIO_Port,\r
- .dir_pin = dir1_Pin,\r
- .pwm_timer = &htim4,\r
- .pwm_channel = TIM_CHANNEL_4,\r
- },\r
- {\r
- // Left motor\r
- .sleep_gpio_port = sleep2_GPIO_Port,\r
- .sleep_pin = sleep2_Pin,\r
- .dir_gpio_port = dir2_GPIO_Port,\r
- .dir_pin = dir2_Pin,\r
- .pwm_timer = &htim4,\r
- .pwm_channel = TIM_CHANNEL_3,\r
- },\r
-};\r
-MotorController *motor_right = &motors[0];\r
-MotorController *motor_left = &motors[1];\r
-\r
-//Communication\r
-ConfigMessage config_msg;\r
-VelocityMessage vel_msg;\r
-StatusMessage status_msg;\r
-\r
-volatile int32_t left_ticks;\r
-volatile int32_t right_ticks;\r
-volatile float previous_tx_millis;\r
-volatile uint8_t tx_done_flag = 1;\r
-volatile MessageStatusCode otto_status = MessageStatusCode_Waiting4Config;\r
-\r
-/* USER CODE END PV */\r
-\r
-/* Private function prototypes -----------------------------------------------*/\r
-void SystemClock_Config(void);\r
-static void MX_NVIC_Init(void);\r
-/* USER CODE BEGIN PFP */\r
-\r
-/* USER CODE END PFP */\r
-\r
-/* Private user code ---------------------------------------------------------*/\r
-/* USER CODE BEGIN 0 */\r
-/* USER CODE END 0 */\r
-\r
-/**\r
- * @brief The application entry point.\r
- * @retval int\r
- */\r
-int main(void) {\r
- /* USER CODE BEGIN 1 */\r
- /* USER CODE END 1 */\r
-\r
- /* MCU Configuration--------------------------------------------------------*/\r
-\r
- /* Reset of all peripherals, Initializes the Flash interface and the Systick. */\r
- HAL_Init();\r
-\r
- /* USER CODE BEGIN Init */\r
-\r
- /* USER CODE END Init */\r
-\r
- /* Configure the system clock */\r
- SystemClock_Config();\r
-\r
- /* USER CODE BEGIN SysInit */\r
-\r
- /* USER CODE END SysInit */\r
-\r
- /* Initialize all configured peripherals */\r
- MX_GPIO_Init();\r
- MX_DMA_Init();\r
- MX_TIM2_Init();\r
- MX_TIM4_Init();\r
- MX_TIM5_Init();\r
- MX_USART6_UART_Init();\r
- MX_TIM6_Init();\r
- MX_CRC_Init();\r
-\r
- /* Initialize interrupts */\r
- MX_NVIC_Init();\r
- /* USER CODE BEGIN 2 */\r
-\r
- // NOTE(lb): timeout is in milliseconds\r
- // wait for config\r
- HAL_StatusTypeDef config_status =\r
- HAL_UART_Receive(&huart6, (uint8_t*)&config_msg,\r
- sizeof config_msg, 60 * 1000); // 60sec\r
- uint32_t config_crc =\r
- HAL_CRC_Calculate(&hcrc, (uint32_t*)&config_msg,\r
- (sizeof config_msg) - (sizeof config_msg.crc));\r
- if (config_crc != config_msg.crc || config_status != HAL_OK) {\r
- status_msg.status = MessageStatusCode_Error_Config;\r
- status_msg.crc =\r
- HAL_CRC_Calculate(&hcrc, (uint32_t*)&status_msg, sizeof(status_msg) - 4);\r
- for (;;) {\r
- HAL_UART_Transmit(&huart6, (uint8_t*)&status_msg,\r
- sizeof(status_msg), 1000); // 1sec\r
- }\r
- }\r
-\r
- // ======================================================================\r
- // NOTE(lb): all of this should be transformed in compile time constants\r
- odom.baseline = config_msg.baseline;\r
- encoder_left->wheel_circumference = config_msg.left_wheel_circumference;\r
- encoder_left->ticks_per_revolution = config_msg.ticks_per_revolution;\r
- encoder_right->wheel_circumference = config_msg.right_wheel_circumference;\r
- encoder_right->ticks_per_revolution = config_msg.ticks_per_revolution;\r
-\r
- // NOTE(lb): maybe even this but i'm not sure. And at this point\r
- // i'm not even sure that there is a need for a config message.\r
- memcpy(&pid_left.ks, &config_msg.pid_ks_left, sizeof pid_left.ks);\r
- memcpy(&pid_right.ks, &config_msg.pid_ks_right, sizeof pid_right.ks);\r
- memcpy(&pid_cross.ks, &config_msg.pid_ks_cross, sizeof pid_cross.ks);\r
- // ======================================================================\r
-\r
- encoder_init(encoders);\r
- motorcontroller_init(motors);\r
-\r
- //right and left motors have the same parameters\r
- pid_max = (int32_t)htim4.Instance->ARR;\r
- pid_min = -pid_max;\r
-\r
- motorcontroller_brake(motor_left);\r
- motorcontroller_brake(motor_right);\r
-\r
- //Enables TIM6 interrupt (used for PID control)\r
- HAL_TIM_Base_Start_IT(&htim6);\r
-\r
- //Enables UART RX interrupt\r
- HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, 12);\r
-\r
- /* USER CODE END 2 */\r
-\r
- /* Infinite loop */\r
- /* USER CODE BEGIN WHILE */\r
- while (1) {\r
- /* USER CODE END WHILE */\r
-\r
- /* USER CODE BEGIN 3 */\r
- }\r
- /* USER CODE END 3 */\r
-}\r
-\r
-/**\r
- * @brief System Clock Configuration\r
- * @retval None\r
- */\r
-void SystemClock_Config(void) {\r
- RCC_OscInitTypeDef RCC_OscInitStruct = { 0 };\r
- RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };\r
- RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = { 0 };\r
-\r
- /** Configure the main internal regulator output voltage\r
- */\r
- __HAL_RCC_PWR_CLK_ENABLE();\r
- __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);\r
- /** Initializes the RCC Oscillators according to the specified parameters\r
- * in the RCC_OscInitTypeDef structure.\r
- */\r
- RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;\r
- RCC_OscInitStruct.HSIState = RCC_HSI_ON;\r
- RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;\r
- RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;\r
- RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;\r
- RCC_OscInitStruct.PLL.PLLM = 8;\r
- RCC_OscInitStruct.PLL.PLLN = 216;\r
- RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;\r
- RCC_OscInitStruct.PLL.PLLQ = 2;\r
- RCC_OscInitStruct.PLL.PLLR = 2;\r
- if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {\r
- Error_Handler();\r
- }\r
- /** Activate the Over-Drive mode\r
- */\r
- if (HAL_PWREx_EnableOverDrive() != HAL_OK) {\r
- Error_Handler();\r
- }\r
- /** Initializes the CPU, AHB and APB buses clocks\r
- */\r
- RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK\r
- | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;\r
- RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;\r
- RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;\r
- RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;\r
- RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;\r
-\r
- if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7) != HAL_OK) {\r
- Error_Handler();\r
- }\r
- PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART6;\r
- PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;\r
- if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) {\r
- Error_Handler();\r
- }\r
-}\r
-\r
-/**\r
- * @brief NVIC Configuration.\r
- * @retval None\r
- */\r
-static void MX_NVIC_Init(void) {\r
- /* TIM6_DAC_IRQn interrupt configuration */\r
- HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 1, 2);\r
- HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);\r
- /* USART6_IRQn interrupt configuration */\r
- HAL_NVIC_SetPriority(USART6_IRQn, 0, 0);\r
- HAL_NVIC_EnableIRQ(USART6_IRQn);\r
- /* DMA2_Stream6_IRQn interrupt configuration */\r
- HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 0, 0);\r
- HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);\r
- /* DMA2_Stream1_IRQn interrupt configuration */\r
- HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 0, 0);\r
- HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);\r
-}\r
-\r
-/* USER CODE BEGIN 4 */\r
-void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {\r
-\r
- //TIMER 100Hz PID control\r
-\r
- //accumulate ticks for transmission\r
- left_ticks += encoder_count_get(encoder_left);\r
- right_ticks += encoder_count_get(encoder_right);\r
-\r
- //PID control\r
- encoder_update(encoder_left);\r
- float left_velocity = encoder_linear_velocity(encoder_left);\r
- int left_dutycycle = pid_update(&pid_left, left_velocity);\r
-\r
- encoder_update(encoder_right);\r
- float right_velocity = encoder_linear_velocity(encoder_right);\r
- int right_dutycycle = pid_update(&pid_right, right_velocity);\r
-\r
- float difference = left_velocity - right_velocity;\r
- int cross_dutycycle = pid_update(&pid_cross, difference);\r
-\r
- left_dutycycle += cross_dutycycle;\r
- right_dutycycle -= cross_dutycycle;\r
-\r
- motorcontroller_speed_set(motor_left, left_dutycycle);\r
- motorcontroller_speed_set(motor_right, right_dutycycle);\r
-}\r
-\r
-uint8_t porcoddio = 0; // NOTE(lb): LOL\r
-void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {\r
- /*\r
- * Manage received message\r
- */\r
-\r
- uint32_t crc_rx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &vel_msg, 8);\r
-\r
- float linear_velocity;\r
- float angular_velocity;\r
-\r
- if (crc_rx == vel_msg.crc) {\r
- linear_velocity = vel_msg.linear_velocity;\r
- angular_velocity = vel_msg.angular_velocity;\r
- otto_status = MessageStatusCode_Running;\r
- } else {\r
- linear_velocity = 0;\r
- angular_velocity = 0;\r
- otto_status = MessageStatusCode_Error_Velocity;\r
- }\r
-\r
- odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity);\r
- float left_setpoint = odom.velocity.left;\r
- float right_setpoint = odom.velocity.right;\r
-\r
- pid_left.setpoint = left_setpoint;\r
- pid_right.setpoint = right_setpoint;\r
-\r
- float cross_setpoint = left_setpoint - right_setpoint;\r
- pid_cross.setpoint = cross_setpoint;\r
-\r
- /*\r
- * Manage new transmission\r
- */\r
-\r
- int32_t left_ticks_tx = left_ticks + encoder_count_get(encoder_left);\r
- int32_t right_ticks_tx = right_ticks + encoder_count_get(encoder_right);\r
-\r
- status_msg.left_ticks = left_ticks_tx;\r
- status_msg.right_ticks = right_ticks_tx;\r
-\r
- left_ticks = 0;\r
- right_ticks = 0;\r
-\r
- float current_tx_millis = HAL_GetTick();\r
- status_msg.delta_millis = current_tx_millis - previous_tx_millis;\r
- previous_tx_millis = current_tx_millis;\r
-\r
- status_msg.status = otto_status;\r
-\r
- uint32_t crc_tx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &status_msg, 12);\r
-\r
- status_msg.crc = crc_tx;\r
-\r
- if (tx_done_flag == 1) {\r
- HAL_UART_Transmit_DMA(&huart6, (uint8_t*) &status_msg, sizeof(status_msg));\r
- tx_done_flag = 0;\r
- }\r
-\r
- HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, sizeof(vel_msg));\r
-}\r
-\r
-\r
-void HAL_UART_TxCpltCallback(UART_HandleTypeDef *UartHandle) {\r
- tx_done_flag = 1;\r
-}\r
-\r
-uint8_t uart_err = 0;\r
-void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) {\r
- uart_err += 1;\r
-}\r
-\r
-void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {\r
- //Blue user button on the NUCLEO board\r
- if (GPIO_Pin == user_button_Pin) {\r
- //TODO ci può servire il bottone blu?\r
- } else if (GPIO_Pin == fault1_Pin) {\r
- motorcontroller_brake(motor_left);\r
- motorcontroller_brake(motor_right);\r
- //stop TIM6 interrupt (used for PID control)\r
- HAL_TIM_Base_Stop_IT(&htim6);\r
- otto_status = MessageStatusCode_Fault_HBridge;\r
- } else if (GPIO_Pin == fault2_Pin) {\r
- motorcontroller_brake(motor_left);\r
- motorcontroller_brake(motor_right);\r
- //stop TIM6 interrupt (used for PID control)\r
- HAL_TIM_Base_Stop_IT(&htim6);\r
- otto_status = MessageStatusCode_Fault_HBridge;\r
- }\r
-\r
-}\r
-/* USER CODE END 4 */\r
-\r
-/**\r
- * @brief This function is executed in case of error occurrence.\r
- * @retval None\r
- */\r
-void Error_Handler(void) {\r
- /* USER CODE BEGIN Error_Handler_Debug */\r
- /* User can add his own implementation to report the HAL error return state */\r
-\r
- /* USER CODE END Error_Handler_Debug */\r
-}\r
-\r
-#ifdef USE_FULL_ASSERT\r
-/**\r
- * @brief Reports the name of the source file and the source line number\r
- * where the assert_param error has occurred.\r
- * @param file: pointer to the source file name\r
- * @param line: assert_param error line source number\r
- * @retval None\r
- */\r
-void assert_failed(uint8_t *file, uint32_t line)\r
-{\r
- /* USER CODE BEGIN 6 */\r
- /* User can add his own implementation to report the file name and line number,\r
- tex: printf("Wrong parameduty_cycleters value: file %s on line %d\r\n", file, line) */\r
- /* USER CODE END 6 */\r
-}\r
-#endif /* USE_FULL_ASSERT */\r
-\r
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/\r
+/* USER CODE BEGIN Header */
+/**
+ ******************************************************************************
+ * @file : main.c
+ * @brief : Main program body
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>© Copyright (c) 2019 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"
+#include "crc.h"
+#include "dma.h"
+#include "tim.h"
+#include "usart.h"
+#include "gpio.h"
+
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+
+#include <string.h> // memcpy
+
+#include "control/encoder.h"
+#include "control/odometry.h"
+#include "control/motor_controller.h"
+#include "control/pid.h"
+
+#include "communication/otto_messages.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 ---------------------------------------------------------*/
+
+/* USER CODE BEGIN PV */
+
+static Encoder encoders[2] = {
+ {
+ .timer = &htim5,
+ },
+ {
+ .timer = &htim2,
+ },
+};
+Encoder *encoder_right = &encoders[0];
+Encoder *encoder_left = &encoders[1];
+
+Odometry odom = {0};
+
+Pid pid_left = {0};
+Pid pid_right = {0};
+Pid pid_cross = {0};
+
+int32_t pid_max = 0;
+int32_t pid_min = 0;
+
+static MotorController motors[2] = {
+ {
+ // Right motor
+ .sleep_gpio_port = sleep1_GPIO_Port,
+ .sleep_pin = sleep1_Pin,
+ .dir_gpio_port = dir1_GPIO_Port,
+ .dir_pin = dir1_Pin,
+ .pwm_timer = &htim4,
+ .pwm_channel = TIM_CHANNEL_4,
+ },
+ {
+ // Left motor
+ .sleep_gpio_port = sleep2_GPIO_Port,
+ .sleep_pin = sleep2_Pin,
+ .dir_gpio_port = dir2_GPIO_Port,
+ .dir_pin = dir2_Pin,
+ .pwm_timer = &htim4,
+ .pwm_channel = TIM_CHANNEL_3,
+ },
+};
+MotorController *motor_right = &motors[0];
+MotorController *motor_left = &motors[1];
+
+//Communication
+ConfigMessage config_msg;
+VelocityMessage vel_msg;
+StatusMessage status_msg;
+
+volatile int32_t left_ticks;
+volatile int32_t right_ticks;
+volatile float previous_tx_millis;
+volatile uint8_t tx_done_flag = 1;
+volatile MessageStatusCode otto_status = MessageStatusCode_Waiting4Config;
+
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+void SystemClock_Config(void);
+static void MX_NVIC_Init(void);
+/* USER CODE BEGIN PFP */
+
+/* USER CODE END PFP */
+
+/* Private user code ---------------------------------------------------------*/
+/* USER CODE BEGIN 0 */
+/* 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_DMA_Init();
+ MX_TIM2_Init();
+ MX_TIM4_Init();
+ MX_TIM5_Init();
+ MX_USART6_UART_Init();
+ MX_TIM6_Init();
+ MX_CRC_Init();
+
+ /* Initialize interrupts */
+ MX_NVIC_Init();
+ /* USER CODE BEGIN 2 */
+
+ // NOTE(lb): timeout is in milliseconds
+ // wait for config
+ HAL_StatusTypeDef config_status =
+ HAL_UART_Receive(&huart6, (uint8_t*)&config_msg,
+ sizeof config_msg, 60 * 1000); // 60sec
+ uint32_t config_crc =
+ HAL_CRC_Calculate(&hcrc, (uint32_t*)&config_msg,
+ (sizeof config_msg) - (sizeof config_msg.crc));
+ if (config_crc != config_msg.crc || config_status != HAL_OK) {
+ status_msg.status = MessageStatusCode_Error_Config;
+ status_msg.crc =
+ HAL_CRC_Calculate(&hcrc, (uint32_t*)&status_msg, sizeof(status_msg) - 4);
+ for (;;) {
+ HAL_UART_Transmit(&huart6, (uint8_t*)&status_msg,
+ sizeof(status_msg), 1000); // 1sec
+ }
+ }
+
+ // ======================================================================
+ // NOTE(lb): all of this should be transformed in compile time constants
+ odom.baseline = config_msg.baseline;
+ encoder_left->wheel_circumference = config_msg.left_wheel_circumference;
+ encoder_left->ticks_per_revolution = config_msg.ticks_per_revolution;
+ encoder_right->wheel_circumference = config_msg.right_wheel_circumference;
+ encoder_right->ticks_per_revolution = config_msg.ticks_per_revolution;
+
+ // NOTE(lb): maybe even this but i'm not sure. And at this point
+ // i'm not even sure that there is a need for a config message.
+ memcpy(&pid_left.ks, &config_msg.pid_ks_left, sizeof pid_left.ks);
+ memcpy(&pid_right.ks, &config_msg.pid_ks_right, sizeof pid_right.ks);
+ memcpy(&pid_cross.ks, &config_msg.pid_ks_cross, sizeof pid_cross.ks);
+ // ======================================================================
+
+ encoder_init(encoders);
+ motorcontroller_init(motors);
+
+ //right and left motors have the same parameters
+ pid_max = (int32_t)htim4.Instance->ARR;
+ pid_min = -pid_max;
+
+ motorcontroller_brake(motor_left);
+ motorcontroller_brake(motor_right);
+
+ //Enables TIM6 interrupt (used for PID control)
+ HAL_TIM_Base_Start_IT(&htim6);
+
+ //Enables UART RX interrupt
+ HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, 12);
+
+ /* USER CODE END 2 */
+
+ /* 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 };
+ RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = { 0 };
+
+ /** Configure the main internal regulator output voltage
+ */
+ __HAL_RCC_PWR_CLK_ENABLE();
+ __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
+ /** Initializes the RCC Oscillators according to the specified parameters
+ * in the RCC_OscInitTypeDef structure.
+ */
+ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+ RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+ RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+ RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+ RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
+ RCC_OscInitStruct.PLL.PLLM = 8;
+ RCC_OscInitStruct.PLL.PLLN = 216;
+ RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
+ RCC_OscInitStruct.PLL.PLLQ = 2;
+ RCC_OscInitStruct.PLL.PLLR = 2;
+ if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
+ Error_Handler();
+ }
+ /** Activate the Over-Drive mode
+ */
+ if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
+ Error_Handler();
+ }
+ /** Initializes the CPU, AHB and APB buses clocks
+ */
+ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
+ | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
+ RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+ RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+ RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
+ RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
+
+ if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7) != HAL_OK) {
+ Error_Handler();
+ }
+ PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART6;
+ PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;
+ if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) {
+ Error_Handler();
+ }
+}
+
+/**
+ * @brief NVIC Configuration.
+ * @retval None
+ */
+static void MX_NVIC_Init(void) {
+ /* TIM6_DAC_IRQn interrupt configuration */
+ HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 1, 2);
+ HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
+ /* USART6_IRQn interrupt configuration */
+ HAL_NVIC_SetPriority(USART6_IRQn, 0, 0);
+ HAL_NVIC_EnableIRQ(USART6_IRQn);
+ /* DMA2_Stream6_IRQn interrupt configuration */
+ HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 0, 0);
+ HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
+ /* DMA2_Stream1_IRQn interrupt configuration */
+ HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 0, 0);
+ HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
+}
+
+/* USER CODE BEGIN 4 */
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
+
+ //TIMER 100Hz PID control
+
+ //accumulate ticks for transmission
+ left_ticks += encoder_count_get(encoder_left);
+ right_ticks += encoder_count_get(encoder_right);
+
+ //PID control
+ encoder_update(encoder_left);
+ float left_velocity = encoder_linear_velocity(encoder_left);
+ int left_dutycycle = pid_update(&pid_left, left_velocity);
+
+ encoder_update(encoder_right);
+ float right_velocity = encoder_linear_velocity(encoder_right);
+ int right_dutycycle = pid_update(&pid_right, right_velocity);
+
+ float difference = left_velocity - right_velocity;
+ int cross_dutycycle = pid_update(&pid_cross, difference);
+
+ left_dutycycle += cross_dutycycle;
+ right_dutycycle -= cross_dutycycle;
+
+ motorcontroller_speed_set(motor_left, left_dutycycle);
+ motorcontroller_speed_set(motor_right, right_dutycycle);
+}
+
+uint8_t porcoddio = 0; // NOTE(lb): LOL
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
+ /*
+ * Manage received message
+ */
+
+ uint32_t crc_rx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &vel_msg, 8);
+
+ float linear_velocity;
+ float angular_velocity;
+
+ if (crc_rx == vel_msg.crc) {
+ linear_velocity = vel_msg.linear_velocity;
+ angular_velocity = vel_msg.angular_velocity;
+ otto_status = MessageStatusCode_Running;
+ } else {
+ linear_velocity = 0;
+ angular_velocity = 0;
+ otto_status = MessageStatusCode_Error_Velocity;
+ }
+
+ odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity);
+ float left_setpoint = odom.velocity.left;
+ float right_setpoint = odom.velocity.right;
+
+ pid_left.setpoint = left_setpoint;
+ pid_right.setpoint = right_setpoint;
+
+ float cross_setpoint = left_setpoint - right_setpoint;
+ pid_cross.setpoint = cross_setpoint;
+
+ /*
+ * Manage new transmission
+ */
+
+ int32_t left_ticks_tx = left_ticks + encoder_count_get(encoder_left);
+ int32_t right_ticks_tx = right_ticks + encoder_count_get(encoder_right);
+
+ status_msg.left_ticks = left_ticks_tx;
+ status_msg.right_ticks = right_ticks_tx;
+
+ left_ticks = 0;
+ right_ticks = 0;
+
+ float current_tx_millis = HAL_GetTick();
+ status_msg.delta_millis = current_tx_millis - previous_tx_millis;
+ previous_tx_millis = current_tx_millis;
+
+ status_msg.status = otto_status;
+
+ uint32_t crc_tx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &status_msg, 12);
+
+ status_msg.crc = crc_tx;
+
+ if (tx_done_flag == 1) {
+ HAL_UART_Transmit_DMA(&huart6, (uint8_t*) &status_msg, sizeof(status_msg));
+ tx_done_flag = 0;
+ }
+
+ HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, sizeof(vel_msg));
+}
+
+
+void HAL_UART_TxCpltCallback(UART_HandleTypeDef *UartHandle) {
+ tx_done_flag = 1;
+}
+
+uint8_t uart_err = 0;
+void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) {
+ uart_err += 1;
+}
+
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+ //Blue user button on the NUCLEO board
+ if (GPIO_Pin == user_button_Pin) {
+ //TODO ci può servire il bottone blu?
+ } else if (GPIO_Pin == fault1_Pin) {
+ motorcontroller_brake(motor_left);
+ motorcontroller_brake(motor_right);
+ //stop TIM6 interrupt (used for PID control)
+ HAL_TIM_Base_Stop_IT(&htim6);
+ otto_status = MessageStatusCode_Fault_HBridge;
+ } else if (GPIO_Pin == fault2_Pin) {
+ motorcontroller_brake(motor_left);
+ motorcontroller_brake(motor_right);
+ //stop TIM6 interrupt (used for PID control)
+ HAL_TIM_Base_Stop_IT(&htim6);
+ otto_status = MessageStatusCode_Fault_HBridge;
+ }
+
+}
+/* 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 */
+
+ /* 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,
+ tex: printf("Wrong parameduty_cycleters value: file %s on line %d\r\n", file, line) */
+ /* USER CODE END 6 */
+}
+#endif /* USE_FULL_ASSERT */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
projects / pioneer-stm32 / commitdiff