1 /*
2 * Copyright (c) 2019 Maksim Masalski maxxliferobot@gmail.com
3 * A Line follower robot program using DFRobot Maqueen Robot and microbit
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/sys/printk.h>
9 #include <zephyr/drivers/gpio.h>
10 #include <zephyr/drivers/i2c.h>
11 #include <zephyr/device.h>
12
13 enum motor {
14 MOTOR_LEFT,
15 MOTOR_RIGHT,
16 };
17
18 static const struct gpio_dt_spec left_gpio =
19 GPIO_DT_SPEC_GET(DT_PATH(zephyr_user), left_gpios);
20 static const struct gpio_dt_spec right_gpio =
21 GPIO_DT_SPEC_GET(DT_PATH(zephyr_user), right_gpios);
22
23 static const struct i2c_dt_spec motorctl =
24 I2C_DT_SPEC_GET(DT_NODELABEL(motorctl));
25
26 static struct gpio_callback left_cb;
27 static struct gpio_callback right_cb;
28
29 static int left_line;
30 static int right_line;
31
left_irq(const struct device * dev,struct gpio_callback * cb,uint32_t pins)32 static void left_irq(const struct device *dev, struct gpio_callback *cb,
33 uint32_t pins)
34 {
35 left_line = gpio_pin_get_dt(&left_gpio);
36 }
37
right_irq(const struct device * dev,struct gpio_callback * cb,uint32_t pins)38 static void right_irq(const struct device *dev, struct gpio_callback *cb,
39 uint32_t pins)
40 {
41 right_line = gpio_pin_get_dt(&right_gpio);
42 }
43
motor_control(enum motor motor,int16_t speed)44 static void motor_control(enum motor motor, int16_t speed)
45 {
46 uint8_t buf[3];
47
48 if (motor == MOTOR_LEFT) {
49 buf[0] = 0x00U;
50 } else {
51 buf[0] = 0x02U;
52 }
53
54 if (speed < 0) {
55 buf[1] = 0x01U;
56 buf[2] = (uint8_t)(speed * (-1));
57 } else {
58 buf[1] = 0x00U;
59 buf[2] = (uint8_t)speed;
60 }
61
62 i2c_write_dt(&motorctl, buf, sizeof(buf));
63 }
64
65 /* Line follower algorithm for the robot */
line_follow(void)66 static void line_follow(void)
67 {
68 if ((left_line == 0) && (right_line == 0)) {
69 motor_control(MOTOR_LEFT, 200);
70 motor_control(MOTOR_RIGHT, 200);
71 } else {
72 if ((left_line == 0) && (right_line == 1)) {
73 motor_control(MOTOR_LEFT, 0);
74 motor_control(MOTOR_RIGHT, 200);
75 if ((left_line == 1) && (right_line == 1)) {
76 motor_control(MOTOR_LEFT, 0);
77 motor_control(MOTOR_RIGHT, 200);
78 }
79 } else {
80 if ((left_line == 1) && (right_line == 0)) {
81 motor_control(MOTOR_LEFT, 200);
82 motor_control(MOTOR_RIGHT, 0);
83 if ((left_line == 1) &&
84 (right_line == 1)) {
85 motor_control(MOTOR_LEFT, 200);
86 motor_control(MOTOR_RIGHT, 0);
87 }
88 if ((left_line == 1) && (right_line == 0)) {
89 motor_control(MOTOR_LEFT, 200);
90 } else {
91 motor_control(MOTOR_RIGHT, 0);
92 }
93 }
94 }
95 }
96 }
97
main(void)98 int main(void)
99 {
100 if (!gpio_is_ready_dt(&left_gpio) ||
101 !gpio_is_ready_dt(&right_gpio)) {
102 printk("Left/Right GPIO controllers not ready.\n");
103 return 0;
104 }
105
106 if (!device_is_ready(motorctl.bus)) {
107 printk("Motor controller I2C bus not ready.\n");
108 return 0;
109 }
110
111 /* Setup gpio to read data from digital line sensors of the robot */
112 gpio_pin_configure_dt(&left_gpio, GPIO_INPUT);
113 gpio_pin_configure_dt(&right_gpio, GPIO_INPUT);
114
115 gpio_pin_interrupt_configure_dt(&left_gpio, GPIO_INT_EDGE_BOTH);
116 gpio_pin_interrupt_configure_dt(&right_gpio, GPIO_INT_EDGE_BOTH);
117
118 gpio_init_callback(&left_cb, left_irq, BIT(left_gpio.pin));
119 gpio_add_callback(left_gpio.port, &left_cb);
120 gpio_init_callback(&right_cb, right_irq, BIT(right_gpio.pin));
121 gpio_add_callback(right_gpio.port, &right_cb);
122
123 while (1) {
124 line_follow();
125 }
126 return 0;
127 }
128
