Showing
1 changed file
with
372 additions
and
0 deletions
code/robot_arm_main.c
0 → 100644
1 | +/* USER CODE BEGIN Header */ | ||
2 | +/** | ||
3 | + ****************************************************************************** | ||
4 | + * @file : main.c | ||
5 | + * @brief : Main program body | ||
6 | + ****************************************************************************** | ||
7 | + * @attention | ||
8 | + * | ||
9 | + * <h2><center>© 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 | +/* Includes ------------------------------------------------------------------*/ | ||
21 | +#include "main.h" | ||
22 | +#include "i2c.h" | ||
23 | +#include "i2s.h" | ||
24 | +#include "spi.h" | ||
25 | +#include "tim.h" | ||
26 | +#include "usart.h" | ||
27 | +#include "usb_host.h" | ||
28 | +#include "gpio.h" | ||
29 | + | ||
30 | +/* Private includes ----------------------------------------------------------*/ | ||
31 | +/* USER CODE BEGIN Includes */ | ||
32 | +#include <stdbool.h> | ||
33 | +/* USER CODE END Includes */ | ||
34 | + | ||
35 | +/* Private typedef -----------------------------------------------------------*/ | ||
36 | +/* USER CODE BEGIN PTD */ | ||
37 | + | ||
38 | +/* USER CODE END PTD */ | ||
39 | + | ||
40 | +/* Private define ------------------------------------------------------------*/ | ||
41 | +/* USER CODE BEGIN PD */ | ||
42 | +/* USER CODE END PD */ | ||
43 | + | ||
44 | +/* Private macro -------------------------------------------------------------*/ | ||
45 | +/* USER CODE BEGIN PM */ | ||
46 | + | ||
47 | +/* USER CODE END PM */ | ||
48 | + | ||
49 | +/* Private variables ---------------------------------------------------------*/ | ||
50 | + | ||
51 | +/* USER CODE BEGIN PV */ | ||
52 | +bool distance_flag = false; | ||
53 | +int grab_mode = 0; | ||
54 | + | ||
55 | +uint8_t rx3_data = 0; | ||
56 | + | ||
57 | + | ||
58 | +uint8_t tmp_num1 = 1; | ||
59 | +uint8_t tmp_num2 = 2; | ||
60 | +uint8_t tmp_num3 = 3; | ||
61 | +uint8_t tmp_num4 = 4; | ||
62 | +uint8_t tmp_num5 = 5; | ||
63 | +uint8_t tmp_num6 = 6; | ||
64 | +uint8_t tmp_num11 = 11; | ||
65 | +uint8_t tmp_num12 = 12; | ||
66 | +uint8_t tmp_num13 = 13; | ||
67 | +int MOTOR_PWM[5]; | ||
68 | +int MOTOR_PWM_MEAN[5]; | ||
69 | +int MOTOR_PWM_MAX[5]; | ||
70 | +int MOTOR_PWM_MIN[5]; | ||
71 | +int mode[5]={1,1,1,1,1}; | ||
72 | +char direction; | ||
73 | +char response; | ||
74 | + | ||
75 | +volatile uint32_t distance; | ||
76 | +#define Delay_ms HAL_Delay | ||
77 | +#define millis() HAL_GetTick() | ||
78 | +#define SYS_CLOCK 168 | ||
79 | +#define SYSTICK_LOAD 167999 | ||
80 | +__IO uint32_t uwTick=0; | ||
81 | +extern __IO uint32_t uwTick; | ||
82 | +/* USER CODE END PV */ | ||
83 | + | ||
84 | +/* Private function prototypes -----------------------------------------------*/ | ||
85 | +void SystemClock_Config(void); | ||
86 | +static void MX_NVIC_Init(void); | ||
87 | +void MX_USB_HOST_Process(void); | ||
88 | + | ||
89 | +/* USER CODE BEGIN PFP */ | ||
90 | + | ||
91 | +/* USER CODE END PFP */ | ||
92 | + | ||
93 | +/* Private user code ---------------------------------------------------------*/ | ||
94 | +/* USER CODE BEGIN 0 */ | ||
95 | +uint32_t micros() { | ||
96 | + return (uwTick&0x3FFFFF)*1000 + (SYSTICK_LOAD-SysTick->VAL)/SYS_CLOCK; | ||
97 | +} | ||
98 | + | ||
99 | +void Delay_us(uint32_t us) { | ||
100 | + uint32_t temp = micros(); | ||
101 | + uint32_t comp = temp + us; | ||
102 | + uint8_t flag = 0; | ||
103 | + while(comp > temp){ | ||
104 | + if(((uwTick&0x3FFFFF)==0)&&(flag==0)){ | ||
105 | + flag = 1; | ||
106 | + } | ||
107 | + if(flag) temp = micros() + 0x400000UL * 1000; | ||
108 | + else temp = micros(); | ||
109 | + } | ||
110 | +} | ||
111 | + | ||
112 | +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) //External interrupt for Sonar | ||
113 | +{ | ||
114 | + static uint32_t ss=0; | ||
115 | + uint32_t temp = GPIOC->IDR & 0x0002;//PC1이니까 2^(1)=2 | ||
116 | + switch (temp) { | ||
117 | + case 0x0002: | ||
118 | + ss = micros(); | ||
119 | + break; | ||
120 | + | ||
121 | + case 0x0000 : | ||
122 | + distance = (micros() - ss) / 58; | ||
123 | + if(distance <= 3){ | ||
124 | + distance_flag = true; | ||
125 | + } | ||
126 | + break; | ||
127 | + } | ||
128 | +} | ||
129 | + | ||
130 | + | ||
131 | +/* USER CODE END 0 */ | ||
132 | + | ||
133 | +/** | ||
134 | + * @brief The application entry point. | ||
135 | + * @retval int | ||
136 | + */ | ||
137 | +int main(void) | ||
138 | +{ | ||
139 | + /* USER CODE BEGIN 1 */ | ||
140 | + | ||
141 | + | ||
142 | + /* USER CODE END 1 */ | ||
143 | + | ||
144 | + /* MCU Configuration--------------------------------------------------------*/ | ||
145 | + | ||
146 | + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ | ||
147 | + HAL_Init(); | ||
148 | + | ||
149 | + /* USER CODE BEGIN Init */ | ||
150 | + | ||
151 | + /* USER CODE END Init */ | ||
152 | + | ||
153 | + /* Configure the system clock */ | ||
154 | + SystemClock_Config(); | ||
155 | + | ||
156 | + /* USER CODE BEGIN SysInit */ | ||
157 | + | ||
158 | + /* USER CODE END SysInit */ | ||
159 | + | ||
160 | + /* Initialize all configured peripherals */ | ||
161 | + MX_GPIO_Init(); | ||
162 | + MX_I2C1_Init(); | ||
163 | + MX_I2S3_Init(); | ||
164 | + MX_SPI1_Init(); | ||
165 | + MX_TIM3_Init(); | ||
166 | + MX_TIM12_Init(); | ||
167 | + MX_USB_HOST_Init(); | ||
168 | + MX_TIM1_Init(); | ||
169 | + MX_USART2_UART_Init(); | ||
170 | + MX_USART3_UART_Init(); | ||
171 | + | ||
172 | + /* Initialize interrupts */ | ||
173 | + MX_NVIC_Init(); | ||
174 | + /* USER CODE BEGIN 2 */ | ||
175 | + | ||
176 | + //raspberryPi to robotArm | ||
177 | + HAL_UART_Receive_IT(&huart3,&rx3_data,1); | ||
178 | + | ||
179 | + //Robot Arm | ||
180 | + HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1); | ||
181 | + HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2); | ||
182 | + HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3); | ||
183 | + HAL_TIM_PWM_Start(&htim12,TIM_CHANNEL_1); | ||
184 | + HAL_TIM_PWM_Start(&htim12,TIM_CHANNEL_2); | ||
185 | + | ||
186 | + //SONAR | ||
187 | + HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1); | ||
188 | + | ||
189 | + | ||
190 | + /* USER CODE END 2 */ | ||
191 | + | ||
192 | + /* Infinite loop */ | ||
193 | + /* USER CODE BEGIN WHILE */ | ||
194 | + while (1) | ||
195 | + { | ||
196 | + TIM3->CCR3= 300; // 1 top | ||
197 | + TIM12->CCR1= 600;//2 숫자 높을수록 올라옴 | ||
198 | + TIM12->CCR2= 450; // 3 | ||
199 | + TIM3->CCR2= 450; //4 | ||
200 | + TIM3->CCR1 = 620; // 5 숫자 커질수록 왼 | ||
201 | + if(rx3_data == 1){ | ||
202 | + | ||
203 | + HAL_UART_Transmit(&huart2, &tmp_num1, 1, 100); | ||
204 | + | ||
205 | + }else if(rx3_data == 2){ | ||
206 | + HAL_UART_Transmit(&huart2, &tmp_num2, 1, 100); | ||
207 | + } | ||
208 | + /* | ||
209 | + else if(rx3_data == 3){ | ||
210 | + HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
211 | + while(1){ | ||
212 | + if(distance_flag){ | ||
213 | + HAL_UART_Transmit(&huart2, &tmp_num5, 1, 100); | ||
214 | + TIM3->CCR3=400; | ||
215 | + break; | ||
216 | + } | ||
217 | + | ||
218 | + HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
219 | + } | ||
220 | + | ||
221 | + | ||
222 | + }*/ | ||
223 | + | ||
224 | + else if(rx3_data == 3){ | ||
225 | + HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
226 | + } | ||
227 | + | ||
228 | + else if(rx3_data == 6){ | ||
229 | + HAL_UART_Transmit(&huart2, &tmp_num6, 1, 100); | ||
230 | + } | ||
231 | + /* | ||
232 | + else if(rx3_data == 11){ | ||
233 | + HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
234 | + HAL_Delay(2000); | ||
235 | + TIM3->CCR3=300; | ||
236 | + } | ||
237 | + else if(rx3_data == 12){ | ||
238 | + HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
239 | + HAL_Delay(2000); | ||
240 | + TIM3->CCR3=300; | ||
241 | + } | ||
242 | + else if(rx3_data == 13){ | ||
243 | + HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
244 | + HAL_Delay(2000); | ||
245 | + TIM3->CCR3=300; | ||
246 | + } | ||
247 | + */ | ||
248 | + if(distance_flag){ | ||
249 | + HAL_UART_Transmit(&huart2, &tmp_num5, 1, 100); | ||
250 | + TIM3->CCR3=400; | ||
251 | + HAL_Delay(5000); | ||
252 | + //HAL_UART_Transmit(&huart2, &tmp_num3, 1, 100); | ||
253 | + } | ||
254 | + distance_flag =false; | ||
255 | + rx3_data = 0; | ||
256 | + | ||
257 | + /* USER CODE END WHILE */ | ||
258 | + MX_USB_HOST_Process(); | ||
259 | + | ||
260 | + /* USER CODE BEGIN 3 */ | ||
261 | + } | ||
262 | + | ||
263 | + | ||
264 | + /* USER CODE END 3 */ | ||
265 | + } | ||
266 | + | ||
267 | + | ||
268 | + | ||
269 | +/** | ||
270 | + * @brief System Clock Configuration | ||
271 | + * @retval None | ||
272 | + */ | ||
273 | +void SystemClock_Config(void) | ||
274 | +{ | ||
275 | + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; | ||
276 | + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; | ||
277 | + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; | ||
278 | + | ||
279 | + /** Configure the main internal regulator output voltage | ||
280 | + */ | ||
281 | + __HAL_RCC_PWR_CLK_ENABLE(); | ||
282 | + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); | ||
283 | + /** Initializes the RCC Oscillators according to the specified parameters | ||
284 | + * in the RCC_OscInitTypeDef structure. | ||
285 | + */ | ||
286 | + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; | ||
287 | + RCC_OscInitStruct.HSEState = RCC_HSE_ON; | ||
288 | + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; | ||
289 | + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; | ||
290 | + RCC_OscInitStruct.PLL.PLLM = 8; | ||
291 | + RCC_OscInitStruct.PLL.PLLN = 336; | ||
292 | + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; | ||
293 | + RCC_OscInitStruct.PLL.PLLQ = 7; | ||
294 | + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) | ||
295 | + { | ||
296 | + Error_Handler(); | ||
297 | + } | ||
298 | + /** Initializes the CPU, AHB and APB buses clocks | ||
299 | + */ | ||
300 | + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK | ||
301 | + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; | ||
302 | + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; | ||
303 | + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; | ||
304 | + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; | ||
305 | + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; | ||
306 | + | ||
307 | + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) | ||
308 | + { | ||
309 | + Error_Handler(); | ||
310 | + } | ||
311 | + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_I2S; | ||
312 | + PeriphClkInitStruct.PLLI2S.PLLI2SN = 192; | ||
313 | + PeriphClkInitStruct.PLLI2S.PLLI2SR = 2; | ||
314 | + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) | ||
315 | + { | ||
316 | + Error_Handler(); | ||
317 | + } | ||
318 | +} | ||
319 | + | ||
320 | +/** | ||
321 | + * @brief NVIC Configuration. | ||
322 | + * @retval None | ||
323 | + */ | ||
324 | +static void MX_NVIC_Init(void) | ||
325 | +{ | ||
326 | + /* EXTI1_IRQn interrupt configuration */ | ||
327 | + HAL_NVIC_SetPriority(EXTI1_IRQn, 0, 0); | ||
328 | + HAL_NVIC_EnableIRQ(EXTI1_IRQn); | ||
329 | + /* USART3_IRQn interrupt configuration */ | ||
330 | + HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); | ||
331 | + HAL_NVIC_EnableIRQ(USART3_IRQn); | ||
332 | +} | ||
333 | + | ||
334 | +/* USER CODE BEGIN 4 */ | ||
335 | +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) | ||
336 | +{ | ||
337 | + if(huart->Instance==USART3){ | ||
338 | + HAL_UART_Receive_IT(&huart3,&rx3_data,1); | ||
339 | + } | ||
340 | +} | ||
341 | +/* USER CODE END 4 */ | ||
342 | + | ||
343 | +/** | ||
344 | + * @brief This function is executed in case of error occurrence. | ||
345 | + * @retval None | ||
346 | + */ | ||
347 | +void Error_Handler(void) | ||
348 | +{ | ||
349 | + /* USER CODE BEGIN Error_Handler_Debug */ | ||
350 | + /* User can add his own implementation to report the HAL error return state */ | ||
351 | + | ||
352 | + /* USER CODE END Error_Handler_Debug */ | ||
353 | +} | ||
354 | + | ||
355 | +#ifdef USE_FULL_ASSERT | ||
356 | +/** | ||
357 | + * @brief Reports the name of the source file and the source line number | ||
358 | + * where the assert_param error has occurred. | ||
359 | + * @param file: pointer to the source file name | ||
360 | + * @param line: assert_param error line source number | ||
361 | + * @retval None | ||
362 | + */ | ||
363 | +void assert_failed(uint8_t *file, uint32_t line) | ||
364 | +{ | ||
365 | + /* USER CODE BEGIN 6 */ | ||
366 | + /* User can add his own implementation to report the file name and line number, | ||
367 | + tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ | ||
368 | + /* USER CODE END 6 */ | ||
369 | +} | ||
370 | +#endif /* USE_FULL_ASSERT */ | ||
371 | + | ||
372 | +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |
-
Please register or login to post a comment