diff options
Diffstat (limited to 'kernel/code/thing.c')
| -rw-r--r-- | kernel/code/thing.c | 207 |
1 files changed, 207 insertions, 0 deletions
diff --git a/kernel/code/thing.c b/kernel/code/thing.c new file mode 100644 index 0000000..d9b5eac --- /dev/null +++ b/kernel/code/thing.c @@ -0,0 +1,207 @@ +#include <stdio.h> +#include <fcntl.h> +#include <stdlib.h> +#include <stdint.h> +#include <errno.h> +#include <string.h> +#include <time.h> +#include <sys/time.h> +#include <sys/ioctl.h> +#include <linux/cp210x.h> + +/* + * bit 0: 1 left leg NPN base + * bit 1: 2 left leg PNP base + * bit 2: 4 right leg PNP base + * bit 3: 8 right leg NPN base + */ + +#define LEFT_NPN_OFF 0 +#define LEFT_NPN_ON 1 +#define LEFT_PNP_OFF 2 +#define LEFT_PNP_ON 0 + +#define LEFT_LOW (LEFT_NPN_ON | LEFT_PNP_OFF) +#define LEFT_HI (LEFT_NPN_OFF | LEFT_PNP_ON) +#define LEFT_OFF (LEFT_NPN_OFF | LEFT_PNP_OFF) + +#define RIGHT_NPN_OFF 0 +#define RIGHT_NPN_ON 8 +#define RIGHT_PNP_OFF 4 +#define RIGHT_PNP_ON 0 + + +#define RIGHT_LOW (RIGHT_NPN_ON | RIGHT_PNP_OFF) +#define RIGHT_HI (RIGHT_NPN_OFF | RIGHT_PNP_ON) +#define RIGHT_OFF (RIGHT_NPN_OFF | RIGHT_PNP_OFF) + +#define FORWARDS (RIGHT_LOW | LEFT_HI) +#define BACKWARDS (RIGHT_HI | LEFT_LOW) +#define OFF (RIGHT_OFF| LEFT_OFF) + +#define TIMEOUT 750000 /*75ms */ +#define RUN_IN 50 +#define RELEASE_TENSION 10 + +int +drive_motor (int fd, int dir) +{ + int i; + + if (dir > 0) + { + printf (" Motor forwards\n"); + i = FORWARDS; + } + else if (dir < 0) + { + printf (" Motor backwards\n"); + i = BACKWARDS; + } + else + { + printf (" Motor off\n"); + i = OFF; + } + + + return ioctl (fd, CPIOC_GPIOSET, &i); +} + +int +sensor_led_power (int fd, int on) +{ + int i = TIOCM_DTR; + + printf (" Rotation sensor power %s\n", on ? "on" : "off"); + + return ioctl (fd, on ? TIOCMBIS : TIOCMBIC, &i); +} + + +static int +count_steps (int fd, int count) +{ + struct timeval tv, tv_changed, tv_diff; + int cts, old_cts = -1; + int counted = 0; + + printf (" Counting %d steps", count); + + gettimeofday (&tv_changed, NULL); + + while (count) + { + ioctl (fd, TIOCMGET, &cts); + cts &= TIOCM_CTS; + + if (cts != old_cts) + { + if (cts) + { + counted++; + count--; + printf ("."); + fflush (stdout); + } + gettimeofday (&tv_changed, NULL); + old_cts = cts; + } + gettimeofday (&tv, NULL); + timersub (&tv, &tv_changed, &tv_diff); + + if (tv_diff.tv_sec || (tv_diff.tv_usec > TIMEOUT)) + break; + + } + + printf ("%d steps\n", counted); + return counted; +} + + + + +static int +drive_motor_home (int fd) +{ + int ret; + sensor_led_power (fd, 1); + drive_motor (fd, -1); + ret = count_steps (fd, -1); + drive_motor (fd, 0); + ret -= count_steps (fd, -1); + sensor_led_power (fd, 0); + return ret; +} + +static int +drive_motor_count (int fd, int dir, int steps) +{ + int ret; + sensor_led_power (fd, 1); + drive_motor (fd, dir); + ret = count_steps (fd, steps); + drive_motor (fd, 0); + ret += count_steps (fd, -1); + sensor_led_power (fd, 0); + return ret; +} + +int +main (int argc, char *argv[]) +{ + int overshoot, guard, pos, wanted, delta; + + int fd = open (argv[1], O_RDWR); + + if (fd < 0) + return -1; + + + pos = drive_motor_home (fd); + + printf ("Motor was %d steps from home\n", pos); + + + if (argc == 2) + { + pos = drive_motor_count (fd, 1, RELEASE_TENSION); + + printf ("Motor now %d steps from home\n", pos); + return 0; + } + + pos = drive_motor_count (fd, 1, RUN_IN); + + overshoot = pos - RUN_IN; /*overshot */ + + wanted = atoi (argv[2]); + + delta = wanted - pos; + printf ("Now at %d want %d, delta %d\n", pos, wanted, delta); + + guard = overshoot * 3; + + if ((delta > -guard) && (delta < guard)) + { + printf ("Too close\n"); + pos += drive_motor_count (fd, 1, RUN_IN); + delta = wanted - pos; + printf ("Now at %d want %d, delta %d\n", pos, wanted, delta); + } + + + if (delta < 0) + { + pos -= drive_motor_count (fd, -1, (-delta) - overshoot); + } + else if (delta > 0) + { + pos += drive_motor_count (fd, 1, delta - overshoot); + } + + printf ("Wanted %d steps, got %d steps\n", wanted, pos); + + return 0; +} |