summaryrefslogtreecommitdiffstats
path: root/kernel/code
diff options
context:
space:
mode:
Diffstat (limited to 'kernel/code')
-rw-r--r--kernel/code/Makefile15
-rw-r--r--kernel/code/gpio.c49
-rw-r--r--kernel/code/libradiator.c463
-rw-r--r--kernel/code/program_radiator.c23
-rw-r--r--kernel/code/radiator.c34
-rw-r--r--kernel/code/radiator.h19
-rw-r--r--kernel/code/set.c63
-rw-r--r--kernel/code/thing.c207
8 files changed, 873 insertions, 0 deletions
diff --git a/kernel/code/Makefile b/kernel/code/Makefile
new file mode 100644
index 0000000..a478c0c
--- /dev/null
+++ b/kernel/code/Makefile
@@ -0,0 +1,15 @@
+
+PROGS=radiator program_radiator
+CSRCS=libradiator.c
+
+OBJS=${CSRCS:%.c=%.o}
+
+default:${PROGS}
+
+
+${PROGS}: %:%.o ${OBJS}
+ ${CC} -o $@ ${OBJS} $@.o
+
+
+clean:
+ /bin/rm -f core ${OBJS} ${PROGS} ${PROGS:%=%.o}
diff --git a/kernel/code/gpio.c b/kernel/code/gpio.c
new file mode 100644
index 0000000..549eb81
--- /dev/null
+++ b/kernel/code/gpio.c
@@ -0,0 +1,49 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+
+#include <sys/ioctl.h>
+#include <linux/cp210x.h>
+
+
+#define log_ioctl(fd,op,arg) do_ioctl(fd,#op,op,arg)
+
+int
+do_ioctl (int fd, char *sop, int op, void *arg)
+{
+ int ret;
+
+ errno = 0;
+
+ ret = ioctl (fd, op, arg);
+
+ printf ("ioctl(%d,%s(0x%x),%p)=%d (errno=%d(%s))\n", fd, sop, op, arg, ret,
+ errno, strerror (errno));
+
+ return ret;
+}
+
+int
+main (int argc, char *argv[])
+{
+ struct cp210x_port_config config;
+
+ int fd = open (argv[1], O_RDWR);
+
+ memset (&config, 0, sizeof (config));
+
+ log_ioctl (fd, CPIOC_PORTCONFGET, &config);
+
+ printf ("reset=(%x,%x,%x), suspend=(%x,%x,%x), enhanced_fxn=%x\n",
+ config.reset.mode,
+ config.reset.low_power,
+ config.reset.latch,
+ config.suspend.mode,
+ config.suspend.low_power,
+ config.suspend.latch, config.enhanced_fxn);
+
+
+}
diff --git a/kernel/code/libradiator.c b/kernel/code/libradiator.c
new file mode 100644
index 0000000..7baed2e
--- /dev/null
+++ b/kernel/code/libradiator.c
@@ -0,0 +1,463 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+#include <malloc.h>
+#include <time.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <linux/cp210x.h>
+#include <sys/termios.h>
+
+#include "radiator.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 BRAKE (RIGHT_LOW | LEFT_LOW)
+#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
+
+static int brake_motor(int fd)
+{
+ int i=BRAKE;
+ printf (" Motor braking\n");
+ return ioctl (fd, CPIOC_GPIOSET, &i);
+}
+
+static 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);
+}
+
+static 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);
+}
+
+
+#if 0
+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;
+}
+#else
+count_steps (int fd, int count)
+{
+ int counted = 0;
+ struct timeval tv;
+ char buf[128];
+ int i;
+ fd_set rfds;
+
+ printf (" Counting %d steps", count);
+
+FD_ZERO(&rfds);
+
+tcflush(fd,TCIFLUSH);
+
+
+ while (count)
+ {
+ tv.tv_sec=0;
+ tv.tv_usec=TIMEOUT;
+ FD_SET(fd,&rfds);
+
+ if (!select(fd+1,&rfds,NULL,NULL,&tv)) break;
+
+ i=read(fd,buf,sizeof(buf));
+ if (i<0) continue;
+
+ counted+=i;
+ if (count>0) {
+ count-=i;
+ if (count<0) count=0;
+ }
+
+ printf (".");
+ fflush (stdout);
+ }
+
+ printf ("%d steps\n", counted);
+ return counted;
+}
+#endif
+
+
+
+
+static int
+drive_motor_count (Radiator * r, int dir, int steps)
+{
+ int s1, s2, ret;
+
+ sensor_led_power (r->fd, 1);
+ drive_motor (r->fd, dir);
+ s1 = count_steps (r->fd, steps);
+ if (s1 != steps) {
+ printf (" Hit end stop\n");
+ drive_motor (r->fd, 0);
+ } else {
+ brake_motor(r->fd);
+ }
+ s2 = count_steps (r->fd, -1);
+ sensor_led_power (r->fd, 0);
+ drive_motor (r->fd, 0);
+
+ /*did we hit an end stop? */
+ if (s1 != steps)
+ ret = s1 - s2;
+ else
+ ret = s1 + s2;
+
+ printf (" %d steps under power (of %d), %d steps coasting -> %d steps\n",
+ s1, steps, s2, ret);
+
+ r->pos += ret * dir;
+
+ /*Recalibrate our notion of ends */
+
+ if (s1 != steps)
+ {
+ if (dir == -1)
+ {
+ r->pos = 0;
+ }
+ else
+ {
+ r->max = r->pos;
+ }
+ }
+
+ return ret;
+}
+
+
+
+
+int
+radiator_set_pos (Radiator * r, int wanted)
+{
+ int guard, delta;
+
+ delta = wanted - r->pos;
+ printf ("Now at %d want %d, delta %d\n", r->pos, wanted, delta);
+
+ if (wanted == 0)
+ {
+ drive_motor_count (r, -1, -1);
+ return 0;
+ }
+ else if (r->max && (wanted >= r->max))
+ {
+ drive_motor_count (r, 1, -1);
+ return 0;
+ }
+
+
+ guard = r->overshoot * 3;
+
+ if ((delta > -guard) && (delta < guard))
+ {
+ printf ("Too close\n");
+
+ drive_motor_count (r, (r->pos > r->half) ? -1 : 1, RUN_IN);
+
+ delta = wanted - r->pos;
+ printf ("Now at %d want %d, delta %d\n", r->pos, wanted, delta);
+ }
+
+
+ if (delta < 0)
+ {
+ drive_motor_count (r, -1, (-delta) - r->overshoot);
+ }
+ else if (delta > 0)
+ {
+ drive_motor_count (r, 1, delta - r->overshoot);
+ }
+
+ printf ("Wanted %d, got %d\n", wanted, r->pos);
+
+ return 0;
+}
+
+int
+radiator_calibrate (Radiator * r)
+{
+ radiator_set_pos (r, 0);
+
+ drive_motor_count (r, 1, RUN_IN);
+
+ r->overshoot = r->pos - RUN_IN;
+}
+
+
+static int utf16(uint8_t *dst,uint8_t *src)
+{
+int len=0;
+
+
+while (*src) {
+ *(dst++)=*(src++);
+ len+=2;
+ *(dst++)=0;
+}
+
+return len;
+}
+
+
+static void make_usb_descriptor(uint8_t *dst,uint8_t *src)
+{
+int len;
+len=utf16(dst+2,src);
+dst[0]=len+2;
+dst[1]=0x3;
+}
+
+
+
+
+int
+radiator_program (Radiator * r)
+{
+ struct cp210x_port_config config;
+ uint8_t buf[256];
+
+
+#if 0
+ memset (&config, 0, sizeof (config));
+ ioctl (r->fd, CPIOC_PORTCONFGET, &config);
+
+
+ printf
+ ("config was reset=(%x,%x,%x), suspend=(%x,%x,%x), enhanced_fxn=%x\n",
+ config.reset.mode, config.reset.low_power, config.reset.latch,
+ config.suspend.mode, config.suspend.low_power, config.suspend.latch,
+ config.enhanced_fxn);
+
+
+ memset (&config, 0, sizeof (config));
+
+ config.reset.mode = ~CP_INPUT_PINS;
+ config.reset.low_power = 0;
+ config.reset.latch = CP_INPUT_PINS | CP_PIN_GPIO_1 | CP_PIN_GPIO_2;
+ config.suspend.mode = ~CP_INPUT_PINS;
+ config.suspend.low_power = 0;
+ config.suspend.latch = CP_INPUT_PINS | CP_PIN_GPIO_1 | CP_PIN_GPIO_2;
+ config.enhanced_fxn = CP_EFXN_ENABLE_WPU;
+
+ ioctl (r->fd, CPIOC_PORTCONFSET, &config);
+
+ memset (&config, 0, sizeof (config));
+ ioctl (r->fd, CPIOC_PORTCONFGET, &config);
+
+
+ printf
+ ("config is reset=(%x,%x,%x), suspend=(%x,%x,%x), enhanced_fxn=%x\n",
+ config.reset.mode, config.reset.low_power, config.reset.latch,
+ config.suspend.mode, config.suspend.low_power, config.suspend.latch,
+ config.enhanced_fxn);
+
+#endif
+ memset(buf,0,sizeof(buf));
+
+{
+int i=0x413c;
+ ioctl (r->fd, CPIOC_SETVID, &i);
+}
+
+{
+int i=0x9500;
+ ioctl (r->fd, CPIOC_SETPID, &i);
+}
+
+ make_usb_descriptor(buf,"USB Radiator Valve");
+ ioctl (r->fd, CPIOC_SETPRODUCT, buf);
+
+ make_usb_descriptor(buf,"000001");
+ ioctl (r->fd, CPIOC_SETSERIAL, buf);
+
+#if 0
+ make_usb_descriptor(buf,"Global Panaceas");
+ ioctl (r->fd, CPIOC_SETMFG, buf);
+#endif
+
+
+ ioctl (r->fd, CPIOC_DEVICERESET, 0);
+
+
+
+
+ return 0;
+}
+
+
+static void tty_setup(int fd)
+{
+struct termios tios;
+
+tcgetattr(fd, &tios);
+
+cfmakeraw( &tios);
+
+ tios.c_iflag = PARMRK | INPCK;
+ tios.c_oflag = NL0 | CR0 | TAB0 | BS0 | VT0 | FF0;
+ tios.c_lflag = 0;
+ tios.c_cflag = CS8 | CREAD | CLOCAL;
+ tios.c_cc[VMIN]=1;
+ tios.c_cc[VTIME]=0;
+
+ cfsetispeed (&tios, B115200);
+ cfsetospeed (&tios, B115200);
+
+ tcsetattr (fd, TCSANOW, &tios);
+}
+
+
+
+static void
+set_nonblocking (int fd)
+{
+ long arg = 0;
+ arg = fcntl (fd, F_GETFL, arg);
+ arg |= O_NONBLOCK;
+ fcntl (fd, F_SETFL, arg);
+}
+
+static void
+set_blocking (int fd)
+{
+ long arg = 0;
+ arg = fcntl (fd, F_GETFL, arg);
+ arg &= ~O_NONBLOCK;
+ fcntl (fd, F_SETFL, arg);
+}
+
+
+Radiator *
+radiator_open (char *s, int quiet)
+{
+ Radiator *r = malloc (sizeof (Radiator));
+
+ r->fd = open (s, O_RDWR |O_NOCTTY);
+
+ if (r->fd < 0)
+ {
+ free (r);
+ return NULL;
+ }
+
+ tty_setup(r->fd);
+ set_nonblocking(r->fd);
+
+ r->half = 5 * RUN_IN;
+ r->overshoot = 0;
+ r->max = 0;
+ r->pos = 0;
+
+ if (!quiet)
+ radiator_calibrate (r);
+}
+
+void
+radiator_close (Radiator * r)
+{
+ close (r->fd);
+ free (r);
+}
diff --git a/kernel/code/program_radiator.c b/kernel/code/program_radiator.c
new file mode 100644
index 0000000..75077da
--- /dev/null
+++ b/kernel/code/program_radiator.c
@@ -0,0 +1,23 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include "radiator.h"
+
+int
+main (int argc, char *argv[])
+{
+ Radiator *r;
+ int pos;
+ char buf[1024];
+
+
+ r = radiator_open (argv[1],1);
+
+ if (!r)
+ return -1;
+
+ radiator_program(r);
+
+ return 0;
+}
diff --git a/kernel/code/radiator.c b/kernel/code/radiator.c
new file mode 100644
index 0000000..2d594da
--- /dev/null
+++ b/kernel/code/radiator.c
@@ -0,0 +1,34 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include "radiator.h"
+
+int
+main (int argc, char *argv[])
+{
+ Radiator *r;
+ int pos;
+ char buf[1024];
+
+
+ r = radiator_open (argv[1],0);
+
+ if (!r)
+ return -1;
+
+
+ for (;;)
+ {
+ printf ("Current position %d (end stop at %d, overshoots by %d)\n",
+ r->pos, r->max, r->overshoot);
+ printf ("Enter new>");
+ fflush (stdout);
+ buf[sizeof (buf) - 1] = 0;
+ fgets (buf, sizeof (buf) - 1, stdin);
+ radiator_set_pos (r, atoi (buf));
+ }
+
+
+ return 0;
+}
diff --git a/kernel/code/radiator.h b/kernel/code/radiator.h
new file mode 100644
index 0000000..a16edb8
--- /dev/null
+++ b/kernel/code/radiator.h
@@ -0,0 +1,19 @@
+#ifndef _RADIATOR_H_
+#define _RADIATOR_H_
+
+typedef struct
+{
+ int fd;
+ int pos;
+ int half;
+ int overshoot;
+ int max;
+} Radiator;
+
+int radiator_set_pos (Radiator * r, int wanted);
+int radiator_calibrate (Radiator * r);
+int radiator_program (Radiator * r);
+Radiator *radiator_open (char *s, int quiet);
+void radiator_close (Radiator * r);
+
+#endif /* _RADIATOR_H_ */
diff --git a/kernel/code/set.c b/kernel/code/set.c
new file mode 100644
index 0000000..4c8de0e
--- /dev/null
+++ b/kernel/code/set.c
@@ -0,0 +1,63 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+
+#include <sys/ioctl.h>
+#include <linux/cp210x.h>
+
+
+#define log_ioctl(fd,op,arg) do_ioctl(fd,#op,op,arg)
+
+int
+do_ioctl (int fd, char *sop, int op, void *arg)
+{
+ int ret;
+
+ errno = 0;
+
+ ret = ioctl (fd, op, arg);
+
+ printf ("ioctl(%d,%s(0x%x),%p)=%d (errno=%d(%s))\n", fd, sop, op, arg, ret,
+ errno, strerror (errno));
+
+ return ret;
+}
+
+int
+main (int argc, char *argv[])
+{
+ struct cp210x_port_config config;
+
+ int fd = open (argv[1], O_RDWR);
+
+ memset (&config, 0, sizeof (config));
+
+// config.reset.mode=0xf05f;
+ config.reset.mode = ~CP_INPUT_PINS;
+ config.reset.low_power = 0;
+ config.reset.latch = CP_INPUT_PINS | CP_PIN_GPIO_1 | CP_PIN_GPIO_2;
+// config.suspend.mode=0xf05f;
+ config.suspend.mode = ~CP_INPUT_PINS;
+ config.suspend.low_power = 0;
+ config.suspend.latch = CP_INPUT_PINS | CP_PIN_GPIO_1 | CP_PIN_GPIO_2;
+// config.enhanced_fxn=0x30;
+ config.enhanced_fxn = CP_EFXN_ENABLE_WPU;
+
+
+ log_ioctl (fd, CPIOC_PORTCONFSET, &config);
+ log_ioctl (fd, CPIOC_PORTCONFGET, &config);
+
+ printf ("reset=(%x,%x,%x), suspend=(%x,%x,%x), enhanced_fxn=%x\n",
+ config.reset.mode,
+ config.reset.low_power,
+ config.reset.latch,
+ config.suspend.mode,
+ config.suspend.low_power,
+ config.suspend.latch, config.enhanced_fxn);
+
+ log_ioctl (fd, CPIOC_DEVICERESET, 0);
+
+}
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;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-11-04 00:25:54 +0000