[Snek] [PATCH 4/4] Add support for EV3 tacho motors
Mikhail Gusarov
dottedmag at dottedmag.net
Sat Feb 22 15:09:04 PST 2020
---
ports/ev3/Makefile | 3 +-
ports/ev3/motors.c | 563 +++++++++++++++++++++++++++++++++++++
ports/ev3/motors.h | 23 ++
ports/ev3/snek-ev3.builtin | 34 ++-
ports/ev3/snek-main.c | 3 +
5 files changed, 624 insertions(+), 2 deletions(-)
create mode 100644 ports/ev3/motors.c
create mode 100644 ports/ev3/motors.h
diff --git a/ports/ev3/Makefile b/ports/ev3/Makefile
index bdfce97..8b08faa 100644
--- a/ports/ev3/Makefile
+++ b/ports/ev3/Makefile
@@ -26,9 +26,10 @@ SNEK_LOCAL_SRC = \
snek-math.c \
snek-input.c \
sensors.c \
+ motors.c \
utils.c
-SNEK_LOCAL_INC = snek-ev3.h utils.h sensors.h
+SNEK_LOCAL_INC = snek-ev3.h utils.h sensors.h motors.h
SNEK_LOCAL_CFLAGS = -DSNEK_USE_GLIBC_2_4_MATH
SNEK_LOCAL_BUILTINS = \
snek-ev3.builtin \
diff --git a/ports/ev3/motors.c b/ports/ev3/motors.c
new file mode 100644
index 0000000..01e9105
--- /dev/null
+++ b/ports/ev3/motors.c
@@ -0,0 +1,563 @@
+/*
+ * Copyright © 2020 Mikhail Gusarov <dottedmag at dottedmag.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ */
+#include "snek.h"
+#include "utils.h"
+#include "motors.h"
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <limits.h>
+
+#define OUT_PORTS_N 4
+
+#define EV3_PORTS_OUT_PREFIX "ev3-ports:out"
+#define EV3_PORTS_OUT_PREFIX_LEN (sizeof(EV3_PORTS_OUT_PREFIX) - 1)
+
+#define MOTOR_DIR "/sys/class/tacho-motor"
+
+#define MOTOR_PREFIX "motor"
+#define MOTOR_PREFIX_LEN (sizeof(MOTOR_PREFIX) - 1)
+
+#define UNIT_PER_ROT 360
+#define MIN_SPEED -1050
+#define MAX_SPEED 1050
+
+#define DURATION_SCALE 1000
+
+#define CMD_RUN_FOREVER "run-forever"
+#define CMD_RUN_FOREVER_LEN (sizeof(CMD_RUN_FOREVER) - 1)
+
+#define CMD_STOP "stop"
+#define CMD_STOP_LEN (sizeof(CMD_STOP) - 1)
+
+#define CMD_RUN_TIMED "run-timed"
+#define CMD_RUN_TIMED_LEN (sizeof(CMD_RUN_TIMED) - 1)
+
+#define CMD_RUN_TO_ABS_POS "run-to-abs-pos"
+#define CMD_RUN_TO_ABS_POS_LEN (sizeof(CMD_RUN_TO_ABS_POS) - 1)
+
+#define POLARITY_NORMAL "normal"
+#define POLARITY_NORMAL_LEN (sizeof(POLARITY_NORMAL) - 1)
+
+#define POLARITY_INVERSED "inversed"
+#define POLARITY_INVERSED_LEN (sizeof(POLARITY_INVERSED) - 1)
+
+#define STOP_COAST "coast"
+#define STOP_COAST_LEN (sizeof(STOP_COAST) - 1)
+
+#define STOP_BRAKE "brake"
+#define STOP_BRAKE_LEN (sizeof(STOP_BRAKE) - 1)
+
+#define STOP_HOLD "hold"
+#define STOP_HOLD_LEN (sizeof(STOP_HOLD) - 1)
+
+/* global data */
+
+static int motors[OUT_PORTS_N];
+static int selected_motor = -1;
+
+/* end of global data */
+
+/*
+ * All functions in this file error call snek_error and return -1 or SNEK_NULL
+ * on error, except where documented otherwise
+ */
+
+#define ADDRESS_LEN 64
+
+static int
+get_selected_motor(void)
+{
+ if (selected_motor == -1) {
+ snek_error("no selected motor");
+ return -1;
+ }
+ return selected_motor;
+}
+
+static int
+discover_motor2(int fd, int num)
+{
+ char address[ADDRESS_LEN] = "";
+ if (sysfs_read(fd, "address", address, ADDRESS_LEN - 1) == -1) {
+ snek_error("unable to find a port of motor: %s", strerror(errno));
+ return -1;
+ }
+
+ const char *portsuffix = cutprefix(address, EV3_PORTS_OUT_PREFIX, EV3_PORTS_OUT_PREFIX_LEN);
+ if (portsuffix == NULL) {
+ snek_error("unexpected port name: %s", address);
+ return -1;
+ }
+
+ if (portsuffix[0] < 'A' || portsuffix[0] > 'D' || portsuffix[1] != '\n') {
+ snek_error("unexpected port name, Lego EV3 only has output ports A..D, not %s", address);
+ return -1;
+ }
+
+ motors[portsuffix[0] - 'A'] = num;
+ return 0;
+}
+
+static int
+discover_motor(int motors_fd, const char *filename, int num)
+{
+ int sd = openat(motors_fd, filename, O_RDONLY | O_DIRECTORY);
+ if (sd == -1) {
+ /* ENOENT here means the motor was disconnected during directory enumeration. */
+ if (errno == ENOENT)
+ return 0;
+ snek_error("unable to open motor directory: %s", strerror(errno));
+ return -1;
+ }
+
+ int ret = discover_motor2(sd, num);
+
+ close(sd);
+ return ret;
+}
+
+static int
+discover_motors2(int motors_fd, DIR *motors_dir)
+{
+ for (struct dirent *ent = readdir(motors_dir); ent; ent = readdir(motors_dir)) {
+ if (!strcmp(ent->d_name, ".") || !strcmp(ent->d_name, "..")) {
+ continue;
+ }
+
+ const char *suffix = cutprefix(ent->d_name, MOTOR_PREFIX, MOTOR_PREFIX_LEN);
+ if (suffix == NULL) {
+ snek_error("unexpected filename in motors directory: %s", ent->d_name);
+ return -1;
+ }
+
+ int num;
+ if (sscanf(suffix, "%d", &num) != 1) {
+ snek_error("unexpected filename in motors directory: %s", ent->d_name);
+ return -1;
+ }
+
+ if (discover_motor(motors_fd, ent->d_name, num) == -1)
+ return -1;
+ }
+
+ return 0;
+}
+
+static int
+discover_motors(void)
+{
+ memset(motors, 0, sizeof(motors));
+
+ int fd = open(MOTOR_DIR, O_RDONLY | O_DIRECTORY);
+ if (fd == -1) {
+ snek_error("unable to open motors directory: %s", strerror(errno));
+ return -1;
+ }
+
+ DIR *dir = fdopendir(fd);
+ if (dir == NULL) {
+ close(fd);
+ snek_error("unable to enumerate motors: %s", strerror(errno));
+ return -1;
+ }
+
+ int ret = discover_motors2(fd, dir);
+
+ closedir(dir);
+ return ret;
+}
+
+#define MOTOR_PATH_BUF_LEN 256
+
+/* Returns -1 and sets errno on error, does not call snek_error */
+static int
+open_motor(int p)
+{
+ char buf[MOTOR_PATH_BUF_LEN];
+ sprintf(buf, MOTOR_DIR "/" MOTOR_PREFIX "%d", motors[p]);
+ return open(buf, O_RDONLY | O_DIRECTORY);
+}
+
+static int
+get_motori(int port)
+{
+ int fd = open_motor(port);
+ if (fd == -1) {
+ if (discover_motors() == -1)
+ return -1;
+ fd = open_motor(port);
+ }
+
+ if (fd == -1) {
+ if (errno == ENOENT)
+ snek_error("no motor attached to port %c", port + 'A');
+ else
+ snek_error("unable to open motor attached to port %c :%s", port + 'A', strerror(errno));
+ }
+ return fd;
+}
+
+static int
+get_motor_port(snek_poly_t port)
+{
+ char *c = snek_poly_to_string(port);
+ if (c[0] < 'A' || c[0] > 'D' || c[1] != '\0') {
+ snek_error("Lego EV3 has only output ports A..D, not %s", c);
+ return -1;
+ }
+
+ return c[0] - 'A';
+}
+
+static int
+get_motor(snek_poly_t port)
+{
+ int iport = get_motor_port(port);
+ if (iport == -1)
+ return -1;
+
+ return get_motori(iport);
+}
+
+static snek_poly_t
+motor_speed(int fd)
+{
+ int val;
+ if (sysfs_read_int(fd, "speed", &val) == -1) {
+ snek_error("failed to read speed from motor: %s", strerror(errno));
+ return SNEK_NULL;
+ }
+
+ /* TODO: hardcoded scale. May need to be parameterized for new motors */
+ return snek_float_to_poly(((float) val) / UNIT_PER_ROT);
+}
+
+snek_poly_t
+snek_ev3_motors_read(snek_poly_t port)
+{
+ int fd = get_motor(port);
+ if (fd == -1)
+ return SNEK_NULL;
+
+ snek_poly_t ret = motor_speed(fd);
+
+ close(fd);
+ return ret;
+}
+
+static int
+driver_speed(float speed)
+{
+ int s = speed * UNIT_PER_ROT;
+ if (s < MIN_SPEED)
+ s = MIN_SPEED;
+ if (s > MAX_SPEED)
+ s = MAX_SPEED;
+ return s;
+}
+
+snek_poly_t
+snek_builtin_talkto(snek_poly_t port)
+{
+ int iport = get_motor_port(port);
+ if (iport == -1)
+ return SNEK_NULL;
+
+ selected_motor = iport;
+ return SNEK_NULL;
+}
+
+typedef snek_poly_t (*motor_fn0)(int fd);
+
+static snek_poly_t
+call_with_current_motor0(motor_fn0 fn)
+{
+ int port = get_selected_motor();
+ if (port == -1)
+ return SNEK_NULL;
+
+ int fd = get_motori(port);
+ if (fd == -1)
+ return SNEK_NULL;
+
+ snek_poly_t ret = fn(fd);
+
+ close(fd);
+ return ret;
+}
+
+typedef snek_poly_t (*motor_fn1)(int fd, snek_poly_t arg);
+
+static snek_poly_t
+call_with_current_motor1(motor_fn1 fn, snek_poly_t arg)
+{
+ int port = get_selected_motor();
+ if (port == -1)
+ return SNEK_NULL;
+
+ int fd = get_motori(port);
+ if (fd == -1)
+ return SNEK_NULL;
+
+ snek_poly_t ret = fn(fd, arg);
+
+ close(fd);
+ return ret;
+}
+
+static snek_poly_t
+motor_setspeed(int fd, snek_poly_t speed)
+{
+ if (snek_poly_type(speed) != snek_float) {
+ snek_error_type_1(speed);
+ return SNEK_NULL;
+ }
+
+ float fspeed = snek_poly_get_float(speed);
+
+ if (sysfs_write_int(fd, "speed_sp", driver_speed(fspeed)) == -1) {
+ snek_error("unable to set speed: %s", strerror(errno));
+ }
+
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_setpower(snek_poly_t power)
+{
+ return call_with_current_motor1(motor_setspeed, power);
+}
+
+static snek_poly_t
+motor_setleft(int fd)
+{
+ if (sysfs_write(fd, "polarity", POLARITY_INVERSED, POLARITY_INVERSED_LEN) == -1) {
+ snek_error("unable to set motor to rotate counterclockwise: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_setleft(void)
+{
+ return call_with_current_motor0(motor_setleft);
+}
+
+static snek_poly_t
+motor_setright(int fd)
+{
+ if (sysfs_write(fd, "polarity", POLARITY_NORMAL, POLARITY_NORMAL_LEN) == -1) {
+ snek_error("unable to set motor to rotate clockwise: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_setright(void)
+{
+ return call_with_current_motor0(motor_setright);
+ return SNEK_NULL;
+}
+
+static snek_poly_t
+motor_on(int fd)
+{
+ if (sysfs_write(fd, "command", CMD_RUN_FOREVER, CMD_RUN_FOREVER_LEN) == -1) {
+ snek_error("unable to start motor: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_on(void)
+{
+ return call_with_current_motor0(motor_on);
+}
+
+static snek_poly_t
+motor_off(int fd)
+{
+ if (sysfs_write(fd, "command", CMD_STOP, CMD_STOP_LEN) == -1) {
+ snek_error("unable to stop motor: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_off(void)
+{
+ return call_with_current_motor0(motor_off);
+}
+
+static snek_poly_t
+motor_onfor(int fd, snek_poly_t duration)
+{
+ if (snek_poly_type(duration) != snek_float) {
+ snek_error_type_1(duration);
+ return SNEK_NULL;
+ }
+
+ float fduration = snek_poly_get_float(duration);
+
+ if (sysfs_write_int(fd, "time_sp", ((int) fduration * DURATION_SCALE)) == -1) {
+ snek_error("unable to set run duration: %s", strerror(errno));
+ return SNEK_NULL;
+ }
+
+ if (sysfs_write(fd, "command", CMD_RUN_TIMED, CMD_RUN_TIMED_LEN) == -1) {
+ snek_error("unable to start motor: %s", strerror(errno));
+ }
+
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_onfor(snek_poly_t duration)
+{
+ return call_with_current_motor1(motor_onfor, duration);
+}
+
+static snek_poly_t
+motor_setposition(int fd, snek_poly_t position)
+{
+ if (snek_poly_type(position) != snek_float) {
+ snek_error_type_1(position);
+ return SNEK_NULL;
+ }
+
+ int iposition = snek_poly_get_float(position) * UNIT_PER_ROT;
+
+ if (sysfs_write_int(fd, "position_sp", iposition) == -1) {
+ snek_error("unable to set target position: %s", strerror(errno));
+ return SNEK_NULL;
+ }
+
+ if (sysfs_write(fd, "command", CMD_RUN_TO_ABS_POS, CMD_RUN_TO_ABS_POS_LEN) == -1) {
+ snek_error("unable to start motor: %s", strerror(errno));
+ }
+
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_setposition(snek_poly_t position)
+{
+ return call_with_current_motor1(motor_setposition, position);
+}
+
+static snek_poly_t
+motor_position(int fd)
+{
+ int val;
+ if (sysfs_read_int(fd, "position", &val) == -1) {
+ snek_error("failed to read speed position from motor: %s", strerror(errno));
+ return SNEK_NULL;
+ }
+
+ /* TODO: hardcoded divisor. use "count_per_rot" file */
+ return snek_float_to_poly(((float) val) / UNIT_PER_ROT);
+}
+
+snek_poly_t
+snek_builtin_position(void)
+{
+ return call_with_current_motor0(motor_position);
+}
+
+static snek_poly_t
+motor_coast(int fd)
+{
+ if (sysfs_write(fd, "stop_action", STOP_COAST, STOP_COAST_LEN) == -1) {
+ snek_error("unable to set stop action: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_coast(void)
+{
+ return call_with_current_motor0(motor_coast);
+}
+
+static snek_poly_t
+motor_brake(int fd)
+{
+ if (sysfs_write(fd, "stop_action", STOP_BRAKE, STOP_BRAKE_LEN) == -1) {
+ snek_error("unable to set stop action: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_brake(void)
+{
+ return call_with_current_motor0(motor_brake);
+}
+
+static snek_poly_t
+motor_hold(int fd)
+{
+ if (sysfs_write(fd, "stop_action", STOP_HOLD, STOP_HOLD_LEN) == -1) {
+ snek_error("unable to set stop action: %s", strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+snek_poly_t
+snek_builtin_hold(void)
+{
+ return call_with_current_motor0(motor_hold);
+}
+
+static snek_poly_t
+motor_ramp(int fd, const char *file, snek_poly_t time, const char *name)
+{
+ if (snek_poly_type(time) != snek_float) {
+ snek_error_type_1(time);
+ return SNEK_NULL;
+ }
+
+ /* driver time is in milliseconds */
+ int driver_time = (int) (1000 * snek_poly_get_float(time));
+
+ if (sysfs_write_int(fd, file, driver_time) == -1) {
+ snek_error("unable to set %s time: %s", name, strerror(errno));
+ }
+ return SNEK_NULL;
+}
+
+static snek_poly_t
+motor_ramp_up(int fd, snek_poly_t time)
+{
+ return motor_ramp(fd, "ramp_up_sp", time, "ramp-up");
+}
+
+snek_poly_t
+snek_builtin_ramp_up(snek_poly_t time)
+{
+ return call_with_current_motor1(motor_ramp_up, time);
+}
+
+static snek_poly_t
+motor_ramp_down(int fd, snek_poly_t time)
+{
+ return motor_ramp(fd, "ramp_down_sp", time, "ramp-down");
+}
+
+snek_poly_t
+snek_builtin_ramp_down(snek_poly_t time)
+{
+ return call_with_current_motor1(motor_ramp_down, time);
+}
diff --git a/ports/ev3/motors.h b/ports/ev3/motors.h
new file mode 100644
index 0000000..ac06a7e
--- /dev/null
+++ b/ports/ev3/motors.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright © 2020 Mikhail Gusarov <dottedmag at dottedmag.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ */
+#ifndef _SNEK_EV3_MOTORS_H_
+#define _SNEK_EV3_MOTORS_H_
+
+#include "snek.h"
+
+/* read() implementation for motors */
+snek_poly_t
+snek_ev3_motors_read(snek_poly_t port);
+
+#endif /* _SNEK_EV3_MOTORS_H_ */
diff --git a/ports/ev3/snek-ev3.builtin b/ports/ev3/snek-ev3.builtin
index a0645da..c1726fb 100644
--- a/ports/ev3/snek-ev3.builtin
+++ b/ports/ev3/snek-ev3.builtin
@@ -23,8 +23,40 @@ light_ambient, 1
light_color, 1
light_rgb, 1
-# sensor values
+# sensor values, motor speed in rotations per second, 0..3
read, 1
+# select the motor to talk to
+talkto, 1
+
+# set motor speed (power), in rotations per second, 0..3
+setpower, 1
+
+# motors: direction
+setleft, 0
+setright, 0
+
+# motors: start/stop
+on, 0
+off, 0
+onfor, 1
+
+# motors: rotate until the position (in rotations) is reached
+setposition, 1
+position, 0
+
+# tacho motors: what to do when the position is reached?
+# - stop applying force and coast to stop
+coast, 0
+# - brake to get to the expact position
+brake, 0
+# - brake and hold the position
+hold, 0
+
+# set ramp up/down time, in seconds
+ramp_up, 1
+ramp_down, 1
+
+
#include "snek-ev3.h"
#define SNEK_POOL 262144
diff --git a/ports/ev3/snek-main.c b/ports/ev3/snek-main.c
index 2d09baf..ffa48b2 100644
--- a/ports/ev3/snek-main.c
+++ b/ports/ev3/snek-main.c
@@ -15,6 +15,7 @@
#include "snek.h"
#include <getopt.h>
#include "sensors.h"
+#include "motors.h"
static FILE *snek_posix_input;
@@ -128,6 +129,8 @@ snek_builtin_read(snek_poly_t port)
switch (snek_poly_type(port)) {
case snek_float:
return snek_ev3_sensors_read(port);
+ case snek_string:
+ return snek_ev3_motors_read(port);
default:
snek_error_type_1(port);
return SNEK_NULL;
--
2.24.0
More information about the Snek
mailing list