1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <memory.h>
4
5 /*
6 * Copyright (C) 2022 HiHope Open Source Organization .
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http:// www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 *
18 * limitations under the License.
19 */
20
21 #include "ohos_init.h"
22 #include "cmsis_os2.h"
23 #include "iot_gpio.h"
24 #include "hi_io.h"
25 #include "hi_time.h"
26 #include "hi_adc.h"
27 #include "iot_errno.h"
28 #include "robot_hcsr04.h"
29 #include "robot_l9110s.h"
30 #include "robot_sg90.h"
31 #include "trace_model.h"
32 #include "robot_control.h"
33
34 #define GPIO5 5
35 #define FUNC_GPIO 0
36 #define IOT_IO_PULL_UP 1
37 #define VLT_MIN (100)
38 #define OLED_FLAG_ON ((unsigned char)0x01)
39 #define OLED_FLAG_OFF ((unsigned char)0x00)
40 unsigned short g_gpio5_adc_buf[ADC_TEST_LENGTH] = {0 };
41 unsigned int g_gpio5_tick = 0;
42 unsigned int g_car_control_demo_task_id = 0;
43 unsigned char g_car_status = CAR_STOP_STATUS;
44
switch_init(void)45 void switch_init(void)
46 {
47 IoTGpioInit(GPIO5);
48 hi_io_set_func(GPIO5, FUNC_GPIO);
49 IoTGpioSetDir(GPIO5, IOT_GPIO_DIR_IN);
50 hi_io_set_pull(GPIO5, IOT_IO_PULL_UP);
51 }
52
GetCarStatus(void)53 unsigned char GetCarStatus(void)
54 {
55 return g_car_status;
56 }
57
58 // 按键中断响应函数
gpio5_isr_func_mode(void)59 void gpio5_isr_func_mode(void)
60 {
61 printf("gpio5_isr_func_mode start\n");
62 unsigned int tick_interval = 0;
63 unsigned int current_gpio5_tick = 0;
64
65 current_gpio5_tick = hi_get_tick();
66 tick_interval = current_gpio5_tick - g_gpio5_tick;
67
68 if (tick_interval < KEY_INTERRUPT_PROTECT_TIME) {
69 return NULL;
70 }
71 g_gpio5_tick = current_gpio5_tick;
72
73 if (g_car_status == CAR_STOP_STATUS) {
74 g_car_status = CAR_TRACE_STATUS; // 寻迹
75 printf("trace\n");
76 } else if (g_car_status == CAR_TRACE_STATUS) {
77 g_car_status = CAR_OBSTACLE_AVOIDANCE_STATUS; // 超声波
78 printf("ultrasonic\n");
79 } else if (g_car_status == CAR_OBSTACLE_AVOIDANCE_STATUS) {
80 g_car_status = CAR_STOP_STATUS; // 停止
81 printf("stop\n");
82 }
83 }
84
get_gpio5_voltage(void * param)85 unsigned char get_gpio5_voltage(void *param)
86 {
87 int i;
88 unsigned short data;
89 unsigned int ret;
90 unsigned short vlt;
91 float voltage;
92 float vlt_max = 0;
93 float vlt_min = VLT_MIN;
94 float a = 1.8;
95 float c = 4096.0;
96 int b = 4;
97 float vlt_1 = 0.6;
98 float vlt_2 = 1.0;
99
100 hi_unref_param(param);
101 memset_s(g_gpio5_adc_buf, sizeof(g_gpio5_adc_buf), 0x0, sizeof(g_gpio5_adc_buf));
102 for (i = 0; i < ADC_TEST_LENGTH; i++) {
103 ret = hi_adc_read(HI_ADC_CHANNEL_2, &data, HI_ADC_EQU_MODEL_4, HI_ADC_CUR_BAIS_DEFAULT, 0xF0);
104 // ADC_Channal_2 自动识别模式 CNcomment:4次平均算法模式 CNend
105 if (ret != IOT_SUCCESS) {
106 printf("ADC Read Fail\n");
107 return NULL;
108 }
109 g_gpio5_adc_buf[i] = data;
110 }
111
112 for (i = 0; i < ADC_TEST_LENGTH; i++) {
113 vlt = g_gpio5_adc_buf[i];
114 voltage = (float)vlt * a * b / c;
115 /* vlt * 1.8* 4 / 4096.0为将码字转换为电压 */
116 vlt_max = (voltage > vlt_max) ? voltage : vlt_max;
117 vlt_min = (voltage < vlt_min) ? voltage : vlt_min;
118 }
119 printf("vlt_max is %f\r\n", vlt_max);
120 if (vlt_max > vlt_1 && vlt_max < vlt_2) {
121 gpio5_isr_func_mode();
122 }
123 }
124
125 // 按键中断
interrupt_monitor(void)126 void interrupt_monitor(void)
127 {
128 unsigned int ret = 0;
129 /* gpio5 switch2 mode */
130 g_gpio5_tick = hi_get_tick();
131 ret = IoTGpioRegisterIsrFunc(GPIO5, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_FALL_LEVEL_LOW, get_gpio5_voltage, NULL);
132 if (ret == IOT_SUCCESS) {
133 printf(" register gpio5\r\n");
134 }
135 }
136
137 /* Judge steering gear */
engine_go_where(void)138 static unsigned int engine_go_where(void)
139 {
140 float left_distance = 0;
141 float right_distance = 0;
142 unsigned int time = 100;
143 /* 舵机往左转动测量左边障碍物的距离 */
144
145 engine_turn_left();
146 hi_sleep(time);
147 left_distance = GetDistance();
148 hi_sleep(time);
149
150 /* 归中 */
151 regress_middle();
152 hi_sleep(time);
153
154 /* 舵机往右转动测量右边障碍物的距离 */
155 engine_turn_right();
156 hi_sleep(time);
157 right_distance = GetDistance();
158 hi_sleep(time);
159
160 /* 归中 */
161 regress_middle();
162 if (left_distance > right_distance) {
163 return CAR_TURN_LEFT;
164 } else {
165 return CAR_TURN_RIGHT;
166 }
167 }
168
169 /* 根据障碍物的距离来判断小车的行走方向
170 1、距离大于等于20cm继续前进
171 2、距离小于20cm，先停止再后退0.5s, 再继续进行测距, 再进行判断
172 */
173 /* Judge the direction of the car */
car_where_to_go(float distance)174 static void car_where_to_go(float distance)
175 {
176 unsigned int time = 500;
177 if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {
178 car_stop();
179 hi_sleep(time);
180 car_backward();
181 hi_sleep(time);
182 car_stop();
183 unsigned int ret = engine_go_where();
184 printf("ret is %ud\r\n", ret);
185 if (ret == CAR_TURN_LEFT) {
186 car_left();
187 hi_sleep(time);
188 } else if (ret == CAR_TURN_RIGHT) {
189 car_right();
190 hi_sleep(time);
191 }
192 car_stop();
193 } else {
194 car_forward();
195 }
196 }
197
198 /* car mode control func */
car_mode_control_func(void)199 static void car_mode_control_func(void)
200 {
201 float m_distance = 0.0;
202 regress_middle();
203 unsigned int time = 20;
204 while (1) {
205 if (g_car_status != CAR_OBSTACLE_AVOIDANCE_STATUS) {
206 printf("car_mode_control_func 1 module changed\n");
207 regress_middle();
208 break;
209 }
210
211 /* 获取前方物体的距离 */
212 m_distance = GetDistance();
213 car_where_to_go(m_distance);
214 hi_sleep(time);
215 }
216 }
217
RobotCarTestTask(void * param)218 void *RobotCarTestTask(void* param)
219 {
220 printf("switch\r\n");
221 switch_init();
222 interrupt_monitor();
223 unsigned int time = 20;
224
225 while (1) {
226 switch (g_car_status) {
227 case CAR_STOP_STATUS:
228 car_stop();
229 break;
230 case CAR_OBSTACLE_AVOIDANCE_STATUS:
231 car_mode_control_func();
232 break;
233 case CAR_TRACE_STATUS:
234 trace_module();
235 break;
236 default:
237 break;
238 }
239 IoTWatchDogDisable();
240 osDelay(time);
241 }
242 }
243
RobotCarDemo(void)244 void RobotCarDemo(void)
245 {
246 osThreadAttr_t attr;
247
248 attr.name = "RobotCarTestTask";
249 attr.attr_bits = 0U;
250 attr.cb_mem = NULL;
251 attr.cb_size = 0U;
252 attr.stack_mem = NULL;
253 attr.stack_size = CAR_CONTROL_DEMO_TASK_STAK_SIZE;
254 attr.priority = CAR_CONTROL_DEMO_TASK_PRIORITY;
255
256 if (osThreadNew(RobotCarTestTask, NULL, &attr) == NULL) {
257 printf("[Ssd1306TestDemo] Failed to create RobotCarTestTask!\n");
258 }
259 }
260 APP_FEATURE_INIT(RobotCarDemo);
261