From 7d27e956a909c5b7b99a0b4e4bd012cc8f59a5df Mon Sep 17 00:00:00 2001 From: gda <92425917+Gdaadg@users.noreply.github.com> Date: Mon, 7 Oct 2024 00:15:40 +0300 Subject: [PATCH] add gk7205v200-motors (#1525) --- general/package/gk7205v200-motors/Config.in | 4 + .../gk7205v200-motors/gk7205v200-motors.mk | 19 +++ .../package/gk7205v200-motors/src/Makefile | 3 + general/package/gk7205v200-motors/src/motor.c | 154 ++++++++++++++++++ 4 files changed, 180 insertions(+) create mode 100644 general/package/gk7205v200-motors/Config.in create mode 100644 general/package/gk7205v200-motors/gk7205v200-motors.mk create mode 100644 general/package/gk7205v200-motors/src/Makefile create mode 100644 general/package/gk7205v200-motors/src/motor.c diff --git a/general/package/gk7205v200-motors/Config.in b/general/package/gk7205v200-motors/Config.in new file mode 100644 index 00000000..0f8a1483 --- /dev/null +++ b/general/package/gk7205v200-motors/Config.in @@ -0,0 +1,4 @@ +config BR2_PACKAGE_GK7205V200_MOTORS + bool "gk7205v200-motors" + help + Sample code to control gk7205v200 motors. diff --git a/general/package/gk7205v200-motors/gk7205v200-motors.mk b/general/package/gk7205v200-motors/gk7205v200-motors.mk new file mode 100644 index 00000000..de07e1f7 --- /dev/null +++ b/general/package/gk7205v200-motors/gk7205v200-motors.mk @@ -0,0 +1,19 @@ +################################################################################ +# +# gk7205v200-motors +# +################################################################################ + +GK7205V200_MOTORS_SITE_METHOD = local +GK7205V200_MOTORS_SITE = $(GK7205V200_MOTORS_PKGDIR)/src + +define GK7205V200_MOTORS_BUILD_CMDS + $(MAKE) CC=$(TARGET_CC) -C $(@D) +endef + +define GK7205V200_MOTORS_INSTALL_TARGET_CMDS + $(INSTALL) -m 755 -d $(TARGET_DIR)/usr/bin + $(INSTALL) -m 755 -t $(TARGET_DIR)/usr/bin $(@D)/output/motor +endef + +$(eval $(generic-package)) diff --git a/general/package/gk7205v200-motors/src/Makefile b/general/package/gk7205v200-motors/src/Makefile new file mode 100644 index 00000000..d87bd380 --- /dev/null +++ b/general/package/gk7205v200-motors/src/Makefile @@ -0,0 +1,3 @@ +motor: + mkdir -p output + $(CC) $@.c -o output/$@ -s -Wall diff --git a/general/package/gk7205v200-motors/src/motor.c b/general/package/gk7205v200-motors/src/motor.c new file mode 100644 index 00000000..68d02cac --- /dev/null +++ b/general/package/gk7205v200-motors/src/motor.c @@ -0,0 +1,154 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define STEP_TIME 1000 +#define STEP_COUNT 1 +#define SEQ_COUNT 8 + +typedef struct { + const char *name; + int gpio_x[4]; + int gpio_y[4]; +} config; + +static config list[] = { + { "gk7205v200", { 52, 53, 56, 57 }, { 69, 70, 59, 58 }, }, +}; + +const char* dev_name[] = { "/dev/gpiochip0", "/dev/gpiochip1", "/dev/gpiochip2", "/dev/gpiochip3", + "/dev/gpiochip4", "/dev/gpiochip5", "/dev/gpiochip6", "/dev/gpiochip7", + }; + +static int sequence[][4] = { + { 1, 0, 0, 0 }, { 1, 1, 0, 0 }, { 0, 1, 0, 0 }, { 0, 1, 1, 0 }, + { 0, 0, 1, 0 }, { 0, 0, 1, 1 }, { 0, 0, 0, 1 }, { 1, 0, 0, 1 }, + { 1, 0, 0, 1 }, { 0, 0, 0, 1 }, { 0, 0, 1, 1 }, { 0, 0, 1, 0 }, + { 0, 1, 1, 0 }, { 0, 1, 0, 0 }, { 1, 1, 0, 0 }, { 1, 0, 0, 0 }, +}; + +static int write_gpio(int pin, int val) { + struct gpiohandle_request rq; + struct gpiohandle_data data; + unsigned char gpiochip = ((pin >> 3) == 0) ? 0 : (pin >> 3) - 1; + pin = (int) (pin & 7); + int fd = open(dev_name[gpiochip], O_RDONLY); + if (fd < 0) { + printf("Unable to open chip: %s\n", strerror(errno)); + return 1; + } + + rq.lineoffsets[0] = pin; + rq.flags = GPIOHANDLE_REQUEST_OUTPUT; + rq.lines = 1; + + if (ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &rq) < 0) { + printf("Unable to request gpio %d: [%d] %s\n", pin, errno, strerror(errno)); + return 1; + } + + close(fd); + data.values[0] = val; + + if (ioctl(rq.fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data) < 0) { + printf("Unable to set value %d: %s\n", val, strerror(errno)); + return 1; + } + + close(rq.fd); + + return 0; +} + +static int motor_control(int *gpio, int count) { + for (int i = count; i < count + SEQ_COUNT; i++) { + for (int j = 0; j < 4; j++) { + if (write_gpio(gpio[j], sequence[i][j])) { + return 1; + } + + usleep(STEP_TIME); + } + } + + return 0; +} + +static int gpio_export(int *gpio) { + for (int i = 0; i < 4; i++) { + if (write_gpio(gpio[i], 0)) { + return 1; + } + } + + return 0; +} + +int main(int argc, char **argv) { + if (argc < 3 || argc > 4) { + printf("Usage: %s [device] [x_step] [y_step]\n", argv[0]); + return -1; + } + + int dev = -1; + for (int i = 0; i < sizeof(list) / sizeof(config); i++) { + if (strstr(argv[1], list[i].name)) { + dev = i; + break; + } + } + + if (dev < 0) { + printf("Device not supported\n"); + return -1; + } + + int pid = open("/var/run/motor.pid", O_RDWR | O_CREAT, 0644); + if (flock(pid, LOCK_EX | LOCK_NB)) { + printf("Control in progress\n"); + close(pid); + return -1; + } + + int x = argv[2] ? atoi(argv[2]) : 0; + int y = argv[3] ? atoi(argv[3]) : 0; + + if (gpio_export(list[dev].gpio_x) || gpio_export(list[dev].gpio_y)) { + close(pid); + return -1; + } + + int count_x = abs(x) ? STEP_COUNT : 0; + int count_y = abs(y) ? STEP_COUNT : 0; + + while (count_x || count_y) { + if (count_x) { + if (motor_control(list[dev].gpio_x, (x < 0) ? SEQ_COUNT : 0)) { + goto reset; + } + + count_x--; + } + + if (count_y) { + if (motor_control(list[dev].gpio_y, (y < 0) ? 0 : SEQ_COUNT)) { + goto reset; + } + + count_y--; + } + } + +reset: + gpio_export(list[dev].gpio_x); + gpio_export(list[dev].gpio_y); + close(pid); + + return 0; +}