text: /* USER CODE BEGIN Header */
  /**
    ******************************************************************************
    * @file           : main.c
    * @brief          : Main program body
    ******************************************************************************
    * @attention
    *
    * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
    * All rights reserved.</center></h2>
    *
    * This software component is licensed by ST under BSD 3-Clause license,
    * the "License"; You may not use this file except in compliance with the
    * License. You may obtain a copy of the License at:
    *                        opensource.org/licenses/BSD-3-Clause
    *
    ******************************************************************************
    */
  /* USER CODE END Header */
  /* Includes ------------------------------------------------------------------*/
  #include "main.h"
  
  /* Private includes ----------------------------------------------------------*/
  /* USER CODE BEGIN Includes */
  #include "pid.h"
  #include "encoder.h"
  #include "motor.h"
  #include "iirFilter.h"
  #include <stdbool.h>
  /* USER CODE END Includes */
  
  /* Private typedef -----------------------------------------------------------*/
  /* USER CODE BEGIN PTD */
  
  /* USER CODE END PTD */
  
  /* Private define ------------------------------------------------------------*/
  /* USER CODE BEGIN PD */
  #define LED_YELLOW_ON HAL_GPIO_WritePin(GPIOB, LED_YELLOW_Pin, GPIO_PIN_SET);
  #define LED_YELLOW_OFF HAL_GPIO_WritePin(GPIOB, LED_YELLOW_Pin, GPIO_PIN_RESET);
  #define LED_BLUE_ON HAL_GPIO_WritePin(GPIOB, LED_BLUE_Pin, GPIO_PIN_SET);
  #define LED_BLUE_OFF HAL_GPIO_WritePin(GPIOB, LED_BLUE_Pin, GPIO_PIN_RESET);
  /* USER CODE END PD */
  
  /* Private macro -------------------------------------------------------------*/
  /* USER CODE BEGIN PM */
  
  /* USER CODE END PM */
  
  /* Private variables ---------------------------------------------------------*/
  ADC_HandleTypeDef hadc1;
  
  CAN_HandleTypeDef hcan2;
  
  SPI_HandleTypeDef hspi1;
  
  TIM_HandleTypeDef htim1;
  TIM_HandleTypeDef htim3;
  TIM_HandleTypeDef htim10;
  
  /* USER CODE BEGIN PV */
  extern PID_Properties_t pidProperties;
  volatile float feedback = 0, filteredFeedback = 0;
  float setpoint = 100;
  volatile uint16_t counter2ms = 0;
  float pidResult;
  
  float minAllowedAngle = -300;
  float maxAllowedAngle = 300;
  
  MotorConfig mot1;
  /* USER CODE END PV */
  
  /* Private function prototypes -----------------------------------------------*/
  void SystemClock_Config(void);
  static void MX_GPIO_Init(void);
  static void MX_CAN2_Init(void);
  static void MX_TIM3_Init(void);
  static void MX_ADC1_Init(void);
  static void MX_SPI1_Init(void);
  static void MX_TIM1_Init(void);
  static void MX_TIM10_Init(void);
  /* USER CODE BEGIN PFP */
  static void MotorInit(){
  	mot1.polarization = Reverse;
  	mot1.Motor_Min = 10;
  	mot1.PWM_Min= 0;
  	mot1.PWM_Max= 1000;
  
  }
  
  
  
  
  
  bool checkIfMovementAllowed(float setpoint, float feedback, float minAllowedAngle, float maxAllowedAngle) {
    bool isNextSetpointInAllowedRange = setpoint < maxAllowedAngle && setpoint > minAllowedAngle;
    return isNextSetpointInAllowedRange;
  }
  
  void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
  	  // Each 2ms (CPU_CLK=168, PSC=999, ARR=335)
  	  if(htim->Instance == TIM10){
  		  InitSPIRec();
  
  
  
  	    counter2ms++;
  	  }
  
  
  
  }
  
  void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi){
  
  	feedback = getAngle();
      filteredFeedback = IIR(feedback, 0.5f); // low pass filter
  
      if(checkIfMovementAllowed(setpoint, filteredFeedback, minAllowedAngle, maxAllowedAngle)){
  
        PID(&pidProperties, setpoint, filteredFeedback, &pidResult, noDerivative, false);
        set_speed((int32_t)pidResult, &mot1);
      }else{
      	set_speed(0,&mot1);
      }
  
  }
  /* 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_CAN2_Init();
    MX_TIM3_Init();
    MX_ADC1_Init();
    MX_SPI1_Init();
    MX_TIM1_Init();
    MX_TIM10_Init();
    /* USER CODE BEGIN 2 */
    MotorInit();
    start_pwm();
    set_speed(0,&mot1);
    init_pid();
    HAL_TIM_Base_Start_IT(&htim10);
  
    /* USER CODE END 2 */
  
    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
  	    if(counter2ms == 100) {
  	      LED_YELLOW_ON;
  	      LED_BLUE_ON;
  	    }else if (counter2ms == 200){
  	      LED_YELLOW_OFF;
  	      LED_BLUE_OFF;
  	      counter2ms = 0;
  	    }
  
  
  
      /* 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_SCALE1);
    /** Initializes the RCC Oscillators according to the specified parameters
    * in the RCC_OscInitTypeDef structure.
    */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLM = 4;
    RCC_OscInitStruct.PLL.PLLN = 168;
    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ = 4;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
      Error_Handler();
    }
    /** Initializes the CPU, AHB and APB buses clocks
    */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                                |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
  
    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
    {
      Error_Handler();
    }
  }
  
  /**
    * @brief ADC1 Initialization Function
    * @param None
    * @retval None
    */
  static void MX_ADC1_Init(void)
  {
  
    /* USER CODE BEGIN ADC1_Init 0 */
  
    /* USER CODE END ADC1_Init 0 */
  
    ADC_ChannelConfTypeDef sConfig = {0};
  
    /* USER CODE BEGIN ADC1_Init 1 */
  
    /* USER CODE END ADC1_Init 1 */
    /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
    */
    hadc1.Instance = ADC1;
    hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
    hadc1.Init.Resolution = ADC_RESOLUTION_12B;
    hadc1.Init.ScanConvMode = DISABLE;
    hadc1.Init.ContinuousConvMode = DISABLE;
    hadc1.Init.DiscontinuousConvMode = DISABLE;
    hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
    hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
    hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
    hadc1.Init.NbrOfConversion = 1;
    hadc1.Init.DMAContinuousRequests = DISABLE;
    hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
    if (HAL_ADC_Init(&hadc1) != HAL_OK)
    {
      Error_Handler();
    }
    /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
    */
    sConfig.Channel = ADC_CHANNEL_11;
    sConfig.Rank = 1;
    sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
    {
      Error_Handler();
    }
    /* USER CODE BEGIN ADC1_Init 2 */
  
    /* USER CODE END ADC1_Init 2 */
  
  }
  
  /**
    * @brief CAN2 Initialization Function
    * @param None
    * @retval None
    */
  static void MX_CAN2_Init(void)
  {
  
    /* USER CODE BEGIN CAN2_Init 0 */
  
    /* USER CODE END CAN2_Init 0 */
  
    /* USER CODE BEGIN CAN2_Init 1 */
  
    /* USER CODE END CAN2_Init 1 */
    hcan2.Instance = CAN2;
    hcan2.Init.Prescaler = 3;
    hcan2.Init.Mode = CAN_MODE_NORMAL;
    hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ;
    hcan2.Init.TimeSeg1 = CAN_BS1_12TQ;
    hcan2.Init.TimeSeg2 = CAN_BS2_2TQ;
    hcan2.Init.TimeTriggeredMode = DISABLE;
    hcan2.Init.AutoBusOff = DISABLE;
    hcan2.Init.AutoWakeUp = DISABLE;
    hcan2.Init.AutoRetransmission = DISABLE;
    hcan2.Init.ReceiveFifoLocked = DISABLE;
    hcan2.Init.TransmitFifoPriority = DISABLE;
    if (HAL_CAN_Init(&hcan2) != HAL_OK)
    {
      Error_Handler();
    }
    /* USER CODE BEGIN CAN2_Init 2 */
  
    /* USER CODE END CAN2_Init 2 */
  
  }
  
  /**
    * @brief SPI1 Initialization Function
    * @param None
    * @retval None
    */
  static void MX_SPI1_Init(void)
  {
  
    /* USER CODE BEGIN SPI1_Init 0 */
  
    /* USER CODE END SPI1_Init 0 */
  
    /* USER CODE BEGIN SPI1_Init 1 */
  
    /* USER CODE END SPI1_Init 1 */
    /* SPI1 parameter configuration*/
    hspi1.Instance = SPI1;
    hspi1.Init.Mode = SPI_MODE_MASTER;
    hspi1.Init.Direction = SPI_DIRECTION_2LINES_RXONLY;
    hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
    hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
    hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
    hspi1.Init.NSS = SPI_NSS_SOFT;
    hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
    hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
    hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
    hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    hspi1.Init.CRCPolynomial = 10;
    if (HAL_SPI_Init(&hspi1) != HAL_OK)
    {
      Error_Handler();
    }
    /* USER CODE BEGIN SPI1_Init 2 */
  
    /* USER CODE END SPI1_Init 2 */
  
  }
  
  /**
    * @brief TIM1 Initialization Function
    * @param None
    * @retval None
    */
  static void MX_TIM1_Init(void)
  {
  
    /* USER CODE BEGIN TIM1_Init 0 */
  
    /* USER CODE END TIM1_Init 0 */
  
    TIM_ClockConfigTypeDef sClockSourceConfig = {0};
    TIM_MasterConfigTypeDef sMasterConfig = {0};
    TIM_OC_InitTypeDef sConfigOC = {0};
    TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
  
    /* USER CODE BEGIN TIM1_Init 1 */
  
    /* USER CODE END TIM1_Init 1 */
    htim1.Instance = TIM1;
    htim1.Init.Prescaler = 16;
    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim1.Init.Period = 999;
    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim1.Init.RepetitionCounter = 0;
    htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
    if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
    {
      Error_Handler();
    }
    sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
    if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
    {
      Error_Handler();
    }
    if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
    {
      Error_Handler();
    }
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
    {
      Error_Handler();
    }
    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = 0;
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
    sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
    if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
    {
      Error_Handler();
    }
    sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
    sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
    sBreakDeadTimeConfig.DeadTime = 0;
    sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
    sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
    sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
    if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
    {
      Error_Handler();
    }
    /* USER CODE BEGIN TIM1_Init 2 */
  
    /* USER CODE END TIM1_Init 2 */
    HAL_TIM_MspPostInit(&htim1);
  
  }
  
  /**
    * @brief 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_ClockConfigTypeDef sClockSourceConfig = {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;
    if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
    {
      Error_Handler();
    }
    sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
    if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
    {
      Error_Handler();
    }
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
    {
      Error_Handler();
    }
    /* USER CODE BEGIN TIM3_Init 2 */
  
    /* USER CODE END TIM3_Init 2 */
  
  }
  
  /**
    * @brief TIM10 Initialization Function
    * @param None
    * @retval None
    */
  static void MX_TIM10_Init(void)
  {
  
    /* USER CODE BEGIN TIM10_Init 0 */
  
    /* USER CODE END TIM10_Init 0 */
  
    /* USER CODE BEGIN TIM10_Init 1 */
  
    /* USER CODE END TIM10_Init 1 */
    htim10.Instance = TIM10;
    htim10.Init.Prescaler = 999;
    htim10.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim10.Init.Period = 335;
    htim10.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim10.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    if (HAL_TIM_Base_Init(&htim10) != HAL_OK)
    {
      Error_Handler();
    }
    /* USER CODE BEGIN TIM10_Init 2 */
  
    /* USER CODE END TIM10_Init 2 */
  
  }
  
  /**
    * @brief GPIO Initialization Function
    * @param None
    * @retval None
    */
  static void MX_GPIO_Init(void)
  {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
  
    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOH_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
  
    /*Configure GPIO pin Output Level */
    HAL_GPIO_WritePin(GPIOA, CS_TEST_Pin|HB_INB_Pin|HB_INA_Pin, GPIO_PIN_RESET);
  
    /*Configure GPIO pin Output Level */
    HAL_GPIO_WritePin(GPIOB, LED_YELLOW_Pin|LED_BLUE_Pin, GPIO_PIN_RESET);
  
    /*Configure GPIO pins : CS_TEST_Pin HB_INB_Pin HB_INA_Pin */
    GPIO_InitStruct.Pin = CS_TEST_Pin|HB_INB_Pin|HB_INA_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 pins : LED_YELLOW_Pin LED_BLUE_Pin */
    GPIO_InitStruct.Pin = LED_YELLOW_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(GPIOB, &GPIO_InitStruct);
  
    /*Configure GPIO pins : HB_DIAGB_Pin HB_DIAGA_Pin */
    GPIO_InitStruct.Pin = HB_DIAGB_Pin|HB_DIAGA_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
  
  }
  
  /* USER CODE BEGIN 4 */
  
  /* USER CODE END 4 */
  
  /**
    * @brief  This function is executed in case of error occurrence.
    * @retval None
    */
  void Error_Handler(void)
  {
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
    /* USER CODE END Error_Handler_Debug */
  }
  
  #ifdef  USE_FULL_ASSERT
  /**
    * @brief  Reports the name of the source file and the source line number
    *         where the assert_param error has occurred.
    * @param  file: pointer to the source file name
    * @param  line: assert_param error line source number
    * @retval None
    */
  void assert_failed(uint8_t *file, uint32_t line)
  {
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
  }
  #endif /* USE_FULL_ASSERT */
  
  /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
  