@@ -129,14 +129,12 @@ static void MX_NVIC_Init(void);
129129/* USER CODE END 0 */
130130
131131/* *
132- * @brief The application entry point.
133- * @retval int
134- */
135- int main (void )
136- {
132+ * @brief The application entry point.
133+ * @retval int
134+ */
135+ int main (void ) {
137136 /* USER CODE BEGIN 1 */
138137 /* USER CODE END 1 */
139-
140138
141139 /* MCU Configuration--------------------------------------------------------*/
142140
@@ -194,7 +192,7 @@ int main(void)
194192
195193 // Enables UART RX interrupt
196194 HAL_UART_Receive_DMA (&huart6, (uint8_t *) &proto_buffer_rx,
197- VelocityCommand_size);
195+ VelocityCommand_size);
198196
199197 /* USER CODE END 2 */
200198
@@ -209,56 +207,51 @@ int main(void)
209207}
210208
211209/* *
212- * @brief System Clock Configuration
213- * @retval None
214- */
215- void SystemClock_Config (void )
216- {
217- RCC_OscInitTypeDef RCC_OscInitStruct = {0 };
218- RCC_ClkInitTypeDef RCC_ClkInitStruct = {0 };
219- RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0 };
210+ * @brief System Clock Configuration
211+ * @retval None
212+ */
213+ void SystemClock_Config (void ) {
214+ RCC_OscInitTypeDef RCC_OscInitStruct = { 0 };
215+ RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };
216+ RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = { 0 };
220217
221218 /* * Configure the main internal regulator output voltage
222- */
219+ */
223220 __HAL_RCC_PWR_CLK_ENABLE ();
224221 __HAL_PWR_VOLTAGESCALING_CONFIG (PWR_REGULATOR_VOLTAGE_SCALE3 );
225222 /* * Initializes the CPU, AHB and APB busses clocks
226- */
223+ */
227224 RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI ;
228225 RCC_OscInitStruct.HSIState = RCC_HSI_ON ;
229226 RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT ;
230227 RCC_OscInitStruct.PLL .PLLState = RCC_PLL_NONE ;
231- if (HAL_RCC_OscConfig (&RCC_OscInitStruct) != HAL_OK )
232- {
228+ if (HAL_RCC_OscConfig (&RCC_OscInitStruct) != HAL_OK ) {
233229 Error_Handler ();
234230 }
235231 /* * Initializes the CPU, AHB and APB busses clocks
236- */
237- RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
238- | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 ;
232+ */
233+ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
234+ | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 ;
239235 RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI ;
240236 RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1 ;
241237 RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1 ;
242238 RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1 ;
243239
244- if (HAL_RCC_ClockConfig (&RCC_ClkInitStruct, FLASH_LATENCY_0 ) != HAL_OK )
245- {
240+ if (HAL_RCC_ClockConfig (&RCC_ClkInitStruct, FLASH_LATENCY_0 ) != HAL_OK ) {
246241 Error_Handler ();
247242 }
248243 PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART6 ;
249244 PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2 ;
250- if (HAL_RCCEx_PeriphCLKConfig (&PeriphClkInitStruct) != HAL_OK )
251- {
245+ if (HAL_RCCEx_PeriphCLKConfig (&PeriphClkInitStruct) != HAL_OK ) {
252246 Error_Handler ();
253247 }
254248}
255249
256250/* *
257- * @brief NVIC Configuration.
258- * @retval None
259- */
260- static void MX_NVIC_Init (void )
261- {
251+ * @brief NVIC Configuration.
252+ * @retval None
253+ */
254+ static void MX_NVIC_Init (void ) {
262255 /* TIM3_IRQn interrupt configuration */
263256 HAL_NVIC_SetPriority (TIM3_IRQn, 2 , 1 );
264257 HAL_NVIC_EnableIRQ (TIM3_IRQn);
@@ -295,7 +288,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
295288 // TIMER 10Hz Transmit
296289 if (htim->Instance == TIM6 ) {
297290
298- pb_ostream_t stream = pb_ostream_from_buffer (proto_buffer_tx, sizeof (proto_buffer_tx));
291+ pb_ostream_t stream = pb_ostream_from_buffer (proto_buffer_tx,
292+ sizeof (proto_buffer_tx));
299293
300294 float left_wheel = left_encoder.GetLinearVelocity ();
301295 float right_wheel = right_encoder.GetLinearVelocity ();
@@ -313,7 +307,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
313307
314308 pb_encode (&stream, StatusMessage_fields, &status_msg);
315309
316- HAL_UART_Transmit_DMA (&huart6,(uint8_t *) &proto_buffer_tx, StatusMessage_size);
310+ HAL_UART_Transmit_DMA (&huart6, (uint8_t *) &proto_buffer_tx,
311+ StatusMessage_size);
317312 }
318313}
319314
@@ -330,7 +325,7 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
330325 float angular_velocity;
331326
332327 pb_istream_t stream = pb_istream_from_buffer (proto_buffer_rx,
333- VelocityCommand_size);
328+ VelocityCommand_size);
334329
335330 bool status = pb_decode (&stream, VelocityCommand_fields, &vel_cmd);
336331
@@ -349,32 +344,82 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
349344 float cross_setpoint = left_setpoint - right_setpoint;
350345 cross_pid.set (cross_setpoint);
351346
352-
353347 }
354348
355349 HAL_UART_Receive_DMA (&huart6, (uint8_t *) &proto_buffer_rx,
356- VelocityCommand_size);
350+ VelocityCommand_size);
357351}
358352
359353void HAL_GPIO_EXTI_Callback (uint16_t GPIO_Pin) {
360354 // Blue user button on the NUCLEO board
361- if (GPIO_Pin == GPIO_PIN_13 ) {
355+ if (GPIO_Pin == user_button_Pin ) {
362356 if (mode == 0 ) {
363357 mode = 1 ;
364358 // Enables TIM3 interrupt (used for PID control)
365359 HAL_TIM_Base_Start_IT (&htim3);
366-
367360 }
361+
362+ } else if (GPIO_Pin == fault1_Pin) {
363+ left_motor.brake ();
364+ right_motor.brake ();
365+ // stop TIM3 interrupt (used for PID control)
366+ HAL_TIM_Base_Stop_IT (&htim3);
367+
368+ // Send status message with error code
369+ status_msg.linear_velocity = 0 ;
370+ status_msg.angular_velocity = 0 ;
371+
372+ float current_tx_millis = HAL_GetTick ();
373+ status_msg.delta_millis = current_tx_millis - previous_tx_millis;
374+ previous_tx_millis = current_tx_millis;
375+
376+ status_msg.status = 4 ;
377+
378+ pb_ostream_t stream = pb_ostream_from_buffer (proto_buffer_tx,
379+ sizeof (proto_buffer_tx));
380+ pb_encode (&stream, StatusMessage_fields, &status_msg);
381+
382+ HAL_UART_Transmit_DMA (&huart6, (uint8_t *) &proto_buffer_tx,
383+ StatusMessage_size);
384+
385+ // loops forever, manual reset is needed
386+ while (1 );
387+
388+ } else if (GPIO_Pin == fault2_Pin) {
389+ left_motor.brake ();
390+ right_motor.brake ();
391+ // stop TIM3 interrupt (used for PID control)
392+ HAL_TIM_Base_Stop_IT (&htim3);
393+
394+ // Send status message with error code
395+ status_msg.linear_velocity = 0 ;
396+ status_msg.angular_velocity = 0 ;
397+
398+ float current_tx_millis = HAL_GetTick ();
399+ status_msg.delta_millis = current_tx_millis - previous_tx_millis;
400+ previous_tx_millis = current_tx_millis;
401+
402+ status_msg.status = 5 ;
403+
404+ pb_ostream_t stream = pb_ostream_from_buffer (proto_buffer_tx,
405+ sizeof (proto_buffer_tx));
406+ pb_encode (&stream, StatusMessage_fields, &status_msg);
407+
408+ HAL_UART_Transmit_DMA (&huart6, (uint8_t *) &proto_buffer_tx,
409+ StatusMessage_size);
410+
411+ // loops forever, manual reset is needed
412+ while (1 );
368413 }
414+
369415}
370416/* USER CODE END 4 */
371417
372418/* *
373- * @brief This function is executed in case of error occurrence.
374- * @retval None
375- */
376- void Error_Handler (void )
377- {
419+ * @brief This function is executed in case of error occurrence.
420+ * @retval None
421+ */
422+ void Error_Handler (void ) {
378423 /* USER CODE BEGIN Error_Handler_Debug */
379424 /* User can add his own implementation to report the HAL error return state */
380425
0 commit comments