2021-1-capstone-design1 / HEN_Project2

Go to a project
Toggle navigation Toggle navigation pinning
신동해
9dc9a554 1 parent 89546e6f

(add) main_arm.c, main_cart.c

Inline Side-by-side
Showing 2 changed files with 737 additions and 0 deletions
1 +/* USER CODE BEGIN Header */
2 +/**
3 + ******************************************************************************
4 + * @file : main.c
5 + * @brief : Main program body
6 + ******************************************************************************
7 + * @attention
8 + *
9 + * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
10 + * All rights reserved.</center></h2>
11 + *
12 + * This software component is licensed by ST under Ultimate Liberty license
13 + * SLA0044, the "License"; You may not use this file except in compliance with
14 + * the License. You may obtain a copy of the License at:
15 + * www.st.com/SLA0044
16 + *
17 + ******************************************************************************
18 + */
19 +/* USER CODE END Header */
20 +
21 +/* Includes ------------------------------------------------------------------*/
22 +#include "main.h"
23 +#include "i2c.h"
24 +#include "i2s.h"
25 +#include "spi.h"
26 +#include "tim.h"
27 +#include "usart.h"
28 +#include "usb_host.h"
29 +#include "gpio.h"
30 +
31 +/* Private includes ----------------------------------------------------------*/
32 +/* USER CODE BEGIN Includes */
33 +#include <stdbool.h>
34 +/* USER CODE END Includes */
35 +
36 +/* Private typedef -----------------------------------------------------------*/
37 +/* USER CODE BEGIN PTD */
38 +
39 +/* USER CODE END PTD */
40 +
41 +/* Private define ------------------------------------------------------------*/
42 +/* USER CODE BEGIN PD */
43 +/* USER CODE END PD */
44 +
45 +/* Private macro -------------------------------------------------------------*/
46 +/* USER CODE BEGIN PM */
47 +
48 +/* USER CODE END PM */
49 +
50 +/* Private variables ---------------------------------------------------------*/
51 +
52 +/* USER CODE BEGIN PV */
53 +bool distance_flag = false;
54 +int grab_mode = 0;
55 +
56 +uint8_t rx3_data = 0;
57 +
58 +
59 +uint8_t tmp_stop = 1;
60 +uint8_t tmp_speed1 = 2;
61 +uint8_t tmp_speed2 = 3;
62 +uint8_t tmp_speed3 = 4;
63 +
64 +int MOTOR_PWM[5];
65 +int MOTOR_PWM_MEAN[5];
66 +int MOTOR_PWM_MAX[5];
67 +int MOTOR_PWM_MIN[5];
68 +int mode[5]={1,1,1,1,1};
69 +char direction;
70 +char response;
71 +
72 +volatile uint32_t distance;
73 +#define Delay_ms HAL_Delay
74 +#define millis() HAL_GetTick()
75 +#define SYS_CLOCK 168
76 +#define SYSTICK_LOAD 167999
77 +__IO uint32_t uwTick=0;
78 +extern __IO uint32_t uwTick;
79 +/* USER CODE END PV */
80 +
81 +/* Private function prototypes -----------------------------------------------*/
82 +void SystemClock_Config(void);
83 +static void MX_NVIC_Init(void);
84 +void MX_USB_HOST_Process(void);
85 +
86 +/* USER CODE BEGIN PFP */
87 +
88 +/* USER CODE END PFP */
89 +
90 +/* Private user code ---------------------------------------------------------*/
91 +/* USER CODE BEGIN 0 */
92 +uint32_t micros() {
93 + return (uwTick&0x3FFFFF)*1000 + (SYSTICK_LOAD-SysTick->VAL)/SYS_CLOCK;
94 +}
95 +
96 +void Delay_us(uint32_t us) {
97 + uint32_t temp = micros();
98 + uint32_t comp = temp + us;
99 + uint8_t flag = 0;
100 + while(comp > temp){
101 + if(((uwTick&0x3FFFFF)==0)&&(flag==0)){
102 + flag = 1;
103 + }
104 + if(flag) temp = micros() + 0x400000UL * 1000;
105 + else temp = micros();
106 + }
107 +}
108 +
109 +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) //External interrupt for Sonar
110 +{
111 + static uint32_t ss=0;
112 + uint32_t temp = GPIOC->IDR & 0x0002;//PC1이니까 2^(1)=2
113 + switch (temp) {
114 + case 0x0002:
115 + ss = micros();
116 + break;
117 +
118 + case 0x0000 :
119 + distance = (micros() - ss) / 58;
120 + if(distance <= 2){
121 + distance_flag = true;
122 + }
123 + break;
124 + }
125 +}
126 +
127 +
128 +/* USER CODE END 0 */
129 +
130 +/**
131 + * @brief The application entry point.
132 + * @retval int
133 + */
134 +int main(void)
135 +{
136 + /* USER CODE BEGIN 1 */
137 +
138 +
139 + /* USER CODE END 1 */
140 +
141 + /* MCU Configuration--------------------------------------------------------*/
142 +
143 + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
144 + HAL_Init();
145 +
146 + /* USER CODE BEGIN Init */
147 +
148 + /* USER CODE END Init */
149 +
150 + /* Configure the system clock */
151 + SystemClock_Config();
152 +
153 + /* USER CODE BEGIN SysInit */
154 +
155 + /* USER CODE END SysInit */
156 +
157 + /* Initialize all configured peripherals */
158 + MX_GPIO_Init();
159 + MX_I2C1_Init();
160 + MX_I2S3_Init();
161 + MX_SPI1_Init();
162 + MX_TIM3_Init();
163 + MX_TIM12_Init();
164 + MX_USB_HOST_Init();
165 + MX_TIM1_Init();
166 + MX_USART2_UART_Init();
167 + MX_USART3_UART_Init();
168 +
169 + /* Initialize interrupts */
170 + MX_NVIC_Init();
171 + /* USER CODE BEGIN 2 */
172 +
173 + //raspberryPi to robotArm
174 + HAL_UART_Receive_IT(&huart3,&rx3_data,1);
175 +
176 + //Robot Arm
177 + HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
178 + HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
179 + HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
180 + HAL_TIM_PWM_Start(&htim12,TIM_CHANNEL_1);
181 + HAL_TIM_PWM_Start(&htim12,TIM_CHANNEL_2);
182 +
183 + //SONAR
184 + HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
185 +
186 +
187 + /* USER CODE END 2 */
188 +
189 + /* Infinite loop */
190 + /* USER CODE BEGIN WHILE */
191 +
192 + while (1)
193 + {
194 +
195 + TIM3->CCR3=300; // 1 top
196 + TIM12->CCR1=350; //2
197 + TIM12->CCR2=300; // 3
198 + TIM3->CCR2= 300; //4
199 + TIM3->CCR1 = 500; // 5
200 +
201 + if(rx3_data == 1){ // 전방 차량과의 거리가 70cm 미만일 때 stm_cart에 신호값 1 전달
202 + HAL_UART_Transmit(&huart2, &tmp_stop, 1, 100);
203 + }
204 + else if(rx3_data == 2){ // 전방 차량과의 거리가 70cm 이상 100cm 미만일 때 stm_cart에 신호값 2 전달
205 + HAL_UART_Transmit(&huart2, &tmp_speed1, 1, 100);
206 + }
207 + else if(rx3_data == 3){ // 전방 차량과의 거리가 100cm 이상 150cm 미만일 stm_cart에 신호값 3 전달
208 + HAL_UART_Transmit(&huart2, &tmp_speed2, 1, 100);
209 + }
210 + else (rx3_data == 4){ // 전방에 차량이 없거나 거리가 150cm 이상일 때 stm_cart에 신호값 4 전달
211 + HAL_UART_Transmit(&huart2, &tmp_speed3, 1, 100);
212 + }
213 +
214 + distance_flag = false;
215 + rx3_data = 0;
216 +
217 +
218 + /* USER CODE END WHILE */
219 + MX_USB_HOST_Process();
220 +
221 + /* USER CODE BEGIN 3 */
222 +
223 +
224 +
225 + }
226 + /* USER CODE END 3 */
227 +}
228 +
229 +/**
230 + * @brief System Clock Configuration
231 + * @retval None
232 + */
233 +void SystemClock_Config(void)
234 +{
235 + RCC_OscInitTypeDef RCC_OscInitStruct = {0};
236 + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
237 + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
238 +
239 + /** Configure the main internal regulator output voltage
240 + */
241 + __HAL_RCC_PWR_CLK_ENABLE();
242 + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
243 + /** Initializes the CPU, AHB and APB busses clocks
244 + */
245 + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
246 + RCC_OscInitStruct.HSEState = RCC_HSE_ON;
247 + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
248 + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
249 + RCC_OscInitStruct.PLL.PLLM = 8;
250 + RCC_OscInitStruct.PLL.PLLN = 336;
251 + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
252 + RCC_OscInitStruct.PLL.PLLQ = 7;
253 + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
254 + {
255 + Error_Handler();
256 + }
257 + /** Initializes the CPU, AHB and APB busses clocks
258 + */
259 + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
260 + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
261 + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
262 + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
263 + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
264 + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
265 +
266 + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
267 + {
268 + Error_Handler();
269 + }
270 + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_I2S;
271 + PeriphClkInitStruct.PLLI2S.PLLI2SN = 192;
272 + PeriphClkInitStruct.PLLI2S.PLLI2SR = 2;
273 + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
274 + {
275 + Error_Handler();
276 + }
277 +}
278 +
279 +/**
280 + * @brief NVIC Configuration.
281 + * @retval None
282 + */
283 +static void MX_NVIC_Init(void)
284 +{
285 + /* EXTI1_IRQn interrupt configuration */
286 + HAL_NVIC_SetPriority(EXTI1_IRQn, 0, 0);
287 + HAL_NVIC_EnableIRQ(EXTI1_IRQn);
288 + /* USART3_IRQn interrupt configuration */
289 + HAL_NVIC_SetPriority(USART3_IRQn, 0, 0);
290 + HAL_NVIC_EnableIRQ(USART3_IRQn);
291 +}
292 +
293 +/* USER CODE BEGIN 4 */
294 +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
295 +{
296 + if(huart->Instance==USART3){
297 + HAL_UART_Receive_IT(&huart3,&rx3_data,1);
298 + }
299 +}
300 +/* USER CODE END 4 */
301 +
302 +/**
303 + * @brief This function is executed in case of error occurrence.
304 + * @retval None
305 + */
306 +void Error_Handler(void)
307 +{
308 + /* USER CODE BEGIN Error_Handler_Debug */
309 + /* User can add his own implementation to report the HAL error return state */
310 +
311 + /* USER CODE END Error_Handler_Debug */
312 +}
313 +
314 +#ifdef USE_FULL_ASSERT
315 +/**
316 + * @brief Reports the name of the source file and the source line number
317 + * where the assert_param error has occurred.
318 + * @param file: pointer to the source file name
319 + * @param line: assert_param error line source number
320 + * @retval None
321 + */
322 +void assert_failed(uint8_t *file, uint32_t line)
323 +{
324 + /* USER CODE BEGIN 6 */
325 + /* User can add his own implementation to report the file name and line number,
326 + tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
327 + /* USER CODE END 6 */
328 +}
329 +#endif /* USE_FULL_ASSERT */
330 +
331 +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
1 +/* USER CODE BEGIN Header */
2 +/* USER CODE END Header */
3 +
4 +/* Includes ------------------------------------------------------------------*/
5 +#include "main.h"
6 +#include "tim.h"
7 +#include "usart.h"
8 +#include "gpio.h"
9 +
10 +/* Private includes ----------------------------------------------------------*/
11 +/* USER CODE BEGIN Includes */
12 +#include <math.h>
13 +#include <stdbool.h>
14 +/* USER CODE END Includes */
15 +
16 +/* Private typedef -----------------------------------------------------------*/
17 +/* USER CODE BEGIN PTD */
18 +
19 +/* USER CODE END PTD */
20 +
21 +/* Private define ------------------------------------------------------------*/
22 +/* USER CODE BEGIN PD */
23 +/* USER CODE END PD */
24 +
25 +/* Private macro -------------------------------------------------------------*/
26 +/* USER CODE BEGIN PM */
27 +
28 +/* USER CODE END PM */
29 +
30 +/* Private variables ---------------------------------------------------------*/
31 +
32 +/* USER CODE BEGIN PV */
33 +uint8_t rx2_data = 0;
34 +
35 +uint16_t speed = 6000;
36 +uint16_t speed_LR = 9000;
37 +uint16_t speed_turn = 4000;
38 +
39 +uint16_t speed_user = 2000;
40 +
41 +uint32_t desired_speed = 3000;
42 +uint32_t MOTER_PWM[4] = {0};
43 +uint32_t Kp = 1;
44 +uint32_t encoder_cnt[4] = {0};
45 +uint32_t encoder_speed[4] = {0};
46 +int32_t error_speed[4] = {0};
47 +int32_t PID_speed[4] = {0};
48 +uint32_t old_PID_speed[4] = {3000, 3000, 3000, 3000};
49 +
50 +/***********lidar************/
51 +//response data
52 +bool scan_start = false;
53 +uint8_t rx3_start[7] = {0};
54 +uint8_t rx3_data[5] = {0};
55 +uint8_t Q = 0;
56 +bool S = false;
57 +uint16_t angle;
58 +uint16_t d;
59 +uint16_t distance[360] = {0};
60 +int16_t avg_DIFF = 0;
61 +
62 +//protocal
63 +uint8_t scan_command[2] = {0xA5,0x20};
64 +uint8_t stop_command[2] = {0xA5,0x25};
65 +uint8_t soft_reboot[2] = {0xA5,0x40};
66 +uint8_t scan_express[2] = {0xA5,0x82};
67 +uint8_t scan_force[2] = {0xA5,0x21};
68 +uint8_t device_info[2] = {0xA5,0x50};
69 +uint8_t health_status[2] = {0xA5,0x52};
70 +uint8_t sample_rate[2] = {0xA5,0x59};
71 +uint8_t scan_response[7] = {0xa5, 0x5a, 0x5, 0x0, 0x0, 0x40, 0x81};
72 +/* USER CODE END PV */
73 +
74 +/* Private function prototypes -----------------------------------------------*/
75 +void SystemClock_Config(void);
76 +static void MX_NVIC_Init(void);
77 +/* USER CODE BEGIN PFP */
78 +
79 +/* USER CODE END PFP */
80 +
81 +/* Private user code ---------------------------------------------------------*/
82 +/* USER CODE BEGIN 0 */
83 +bool array_element_of_index_equal(uint8_t a[], uint8_t b[], uint8_t size) {
84 + uint8_t i;
85 + for(i=0; i<size; i++){
86 + if( a[i] != b[i] )
87 + return false;
88 + }
89 + return true;
90 +}
91 +int16_t array_avg_compare(uint16_t distance[]){
92 +
93 + uint32_t sum_R = 0;
94 + uint32_t sum_L = 0;
95 + uint8_t len_L = 0;
96 + uint8_t len_R = 0;
97 + uint16_t avg_R = 0;
98 + uint16_t avg_L = 0;
99 + int16_t avg_diff = 0;
100 +
101 + for(int i=0; i<90; i++){
102 + sum_R += distance[i];
103 + if(distance[i]!=0){
104 + len_R++;
105 + }
106 + }
107 + avg_R = sum_R/len_R;
108 +
109 + for(int i=270; i<360; i++){
110 + sum_L += distance[i];
111 + if(distance[i]!=0){
112 + len_L++;
113 + }
114 + }
115 + avg_L = sum_L/len_L;
116 +
117 + avg_diff = avg_R - avg_L; //왼쪽 거리가 클때 -
118 +
119 + return avg_diff;
120 +}
121 +void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) //Timer interrupt every 20ms
122 +{
123 + /*if(htim->Instance == TIM6){
124 +
125 + //HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
126 + encoder_cnt[0] = TIM2->CNT;
127 + TIM2->CNT=0;
128 +
129 + encoder_cnt[1] = TIM3->CNT;
130 + TIM3->CNT=0;
131 +
132 + encoder_cnt[2] = TIM4->CNT;
133 + TIM4->CNT=0;
134 +
135 + encoder_cnt[3] = TIM5->CNT;
136 + TIM5->CNT=0;
137 +
138 + encoder_speed[0] = 164.18 * exp(0.0112*encoder_cnt[0]);
139 + encoder_speed[1] = 164.18 * exp(0.0112*encoder_cnt[1]);
140 + encoder_speed[2] = 164.18 * exp(0.0112*encoder_cnt[2]);
141 + encoder_speed[3] = 164.18 * exp(0.0112*encoder_cnt[3]);
142 +
143 + error_speed[0] = desired_speed - encoder_speed[0];
144 + error_speed[1] = desired_speed - encoder_speed[1];
145 + error_speed[2] = desired_speed - encoder_speed[2];
146 + error_speed[3] = desired_speed - encoder_speed[3];
147 +
148 + PID_speed[0] = old_PID_speed[0] + Kp*error_speed[0];
149 + PID_speed[1] = old_PID_speed[1] + Kp*error_speed[1];
150 + PID_speed[2] = old_PID_speed[2] + Kp*error_speed[2];
151 + PID_speed[3] = old_PID_speed[3] + Kp*error_speed[3];
152 +
153 + old_PID_speed[0] = PID_speed[0];
154 + old_PID_speed[1] = PID_speed[1];
155 + old_PID_speed[2] = PID_speed[2];
156 + old_PID_speed[3] = PID_speed[3];
157 + }
158 + */
159 +}
160 +
161 +/* USER CODE END 0 */
162 +
163 +/**
164 + * @brief The application entry point.
165 + * @retval int
166 + */
167 +int main(void)
168 +{
169 + /* USER CODE BEGIN 1 */
170 +
171 + /* USER CODE END 1 */
172 +
173 + /* MCU Configuration--------------------------------------------------------*/
174 +
175 + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
176 + HAL_Init();
177 +
178 + /* USER CODE BEGIN Init */
179 +
180 + /* USER CODE END Init */
181 +
182 + /* Configure the system clock */
183 + SystemClock_Config();
184 +
185 + /* USER CODE BEGIN SysInit */
186 +
187 + /* USER CODE END SysInit */
188 +
189 + /* Initialize all configured peripherals */
190 + MX_GPIO_Init();
191 + MX_TIM1_Init();
192 + MX_TIM2_Init();
193 + MX_TIM3_Init();
194 + MX_TIM4_Init();
195 + MX_TIM6_Init();
196 + MX_USART2_UART_Init();
197 + MX_USART3_UART_Init();
198 + MX_TIM5_Init();
199 +
200 + /* Initialize interrupts */
201 + MX_NVIC_Init();
202 + /* USER CODE BEGIN 2 */
203 +
204 + HAL_Delay(3000);
205 +
206 + //Initialize for motor PWM
207 + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
208 + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
209 + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
210 + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
211 +
212 + //Initialize for motor direction
213 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, SET);
214 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, RESET);
215 +
216 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, SET);
217 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, RESET);
218 +
219 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8, SET);
220 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_9, RESET);
221 +
222 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_10, SET);
223 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, RESET);
224 +
225 + //Initialize for encoder count value
226 + TIM2->CNT = 0;
227 + TIM3->CNT = 0;
228 + TIM4->CNT = 0;
229 + TIM5->CNT = 0;
230 +
231 + //Initialize for Encoder
232 + HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
233 + HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
234 + HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
235 + HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL);
236 +
237 + //Initialize for timer interrupt initialization for Encoder (50ms)
238 + HAL_TIM_Base_Start_IT(&htim6);
239 +
240 + //LIDAR_scan_start
241 + //HAL_UART_Transmit(&huart3, &scan_command, 2, 100);
242 + HAL_UART_Transmit(&huart3, &scan_command, 2, 100);
243 + //robotArm to Cart
244 + HAL_UART_Receive_IT(&huart2,&rx2_data,1);
245 +
246 +
247 + /* USER CODE END 2 */
248 +
249 + /* Infinite loop */
250 + /* USER CODE BEGIN WHILE */
251 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, SET);
252 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, RESET);
253 +
254 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, SET);
255 + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, RESET);
256 +
257 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8, SET);
258 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_9, RESET);
259 +
260 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_10, SET);
261 + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, RESET);
262 + while (1)
263 + {
264 + if(rx2_data == 1){ // 전방 카트와의 거리가 70cm 미만일 때 -> 정지
265 + TIM1->CCR1 = 0;
266 + TIM1->CCR2 = 0;
267 + TIM1->CCR3 = 0;
268 + TIM1->CCR4 = 0;
269 +
270 + } else if(rx2_data == 2){ // 전방 카트와의 거리가 70cm 이상 100cm 미만일 때 -> speed = 2400기준 16%씩 감소
271 + if (speed_user > 400){
272 + speed_user -= 400;
273 + }
274 + else{
275 + speed_user = 0
276 + }
277 + TIM1->CCR1 = speed_user;
278 + TIM1->CCR2 = speed_user;
279 + TIM1->CCR3 = speed_user;
280 + TIM1->CCR4 = speed_user;
281 + HAL_Delay(500);
282 + } else if(rx2_data == 3){ // 전방 카트와의 거리가 100cm 이상 150cm 미만일 때 -> speed = 2400기준 8%씩 감소
283 + if (speed_user > 200){
284 + speed_user -= 200;
285 + }
286 + TIM1->CCR1 = speed_user;
287 + TIM1->CCR2 = speed_user;
288 + TIM1->CCR3 = speed_user;
289 + TIM1->CCR4 = speed_user;
290 + HAL_Delay(500);
291 + } else if(rx2_data == 4){ // 전방에 카트가 없거나 거리가 150cm 이상일 때 -> speed = 2400기준 33%씩 증가
292 + if (speed_user < 1600) {
293 + speed_user = 2400;
294 + }
295 + else{
296 + speed_user += 800;
297 + }
298 + TIM1->CCR1 = speed_user;
299 + TIM1->CCR2 = speed_user;
300 + TIM1->CCR3 = speed_user;
301 + TIM1->CCR4 = speed_user;
302 + HAL_Delay(500);
303 + }
304 +
305 + /* USER CODE END WHILE */
306 +
307 + /* USER CODE BEGIN 3 */
308 + }
309 + /* USER CODE END 3 */
310 +}
311 +
312 +/**
313 + * @brief System Clock Configuration
314 + * @retval None
315 + */
316 +void SystemClock_Config(void)
317 +{
318 + RCC_OscInitTypeDef RCC_OscInitStruct = {0};
319 + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
320 +
321 + /** Configure the main internal regulator output voltage
322 + */
323 + __HAL_RCC_PWR_CLK_ENABLE();
324 + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
325 + /** Initializes the CPU, AHB and APB busses clocks
326 + */
327 + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
328 + RCC_OscInitStruct.HSEState = RCC_HSE_ON;
329 + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
330 + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
331 + RCC_OscInitStruct.PLL.PLLM = 4;
332 + RCC_OscInitStruct.PLL.PLLN = 168;
333 + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
334 + RCC_OscInitStruct.PLL.PLLQ = 7;
335 + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
336 + {
337 + Error_Handler();
338 + }
339 + /** Initializes the CPU, AHB and APB busses clocks
340 + */
341 + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
342 + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
343 + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
344 + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
345 + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
346 + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
347 +
348 + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
349 + {
350 + Error_Handler();
351 + }
352 +}
353 +
354 +/**
355 + * @brief NVIC Configuration.
356 + * @retval None
357 + */
358 +static void MX_NVIC_Init(void)
359 +{
360 + /* USART2_IRQn interrupt configuration */
361 + HAL_NVIC_SetPriority(USART2_IRQn, 0, 0);
362 + HAL_NVIC_EnableIRQ(USART2_IRQn);
363 + /* TIM6_DAC_IRQn interrupt configuration */
364 + HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0);
365 + HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
366 +}
367 +
368 +/* USER CODE BEGIN 4 */
369 +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
370 +{
371 + if(huart->Instance==USART2){
372 + HAL_UART_Receive_IT(&huart2,&rx2_data,1);
373 + }
374 +}
375 +/* USER CODE END 4 */
376 +
377 +/**
378 + * @brief This function is executed in case of error occurrence.
379 + * @retval None
380 + */
381 +void Error_Handler(void)
382 +{
383 + /* USER CODE BEGIN Error_Handler_Debug */
384 + /* User can add his own implementation to report the HAL error return state */
385 +
386 + /* USER CODE END Error_Handler_Debug */
387 +}
388 +
389 +#ifdef USE_FULL_ASSERT
390 +/**
391 + * @brief Reports the name of the source file and the source line number
392 + * where the assert_param error has occurred.
393 + * @param file: pointer to the source file name
394 + * @param line: assert_param error line source number
395 + * @retval None
396 + */
397 +void assert_failed(uint8_t *file, uint32_t line)
398 +{
399 + /* USER CODE BEGIN 6 */
400 + /* User can add his own implementation to report the file name and line number,
401 + tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
402 + /* USER CODE END 6 */
403 +}
404 +#endif /* USE_FULL_ASSERT */
405 +
406 +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/