summaryrefslogtreecommitdiffstats
path: root/userland/libradiator.c
diff options
context:
space:
mode:
Diffstat (limited to 'userland/libradiator.c')
-rw-r--r--userland/libradiator.c364
1 files changed, 364 insertions, 0 deletions
diff --git a/userland/libradiator.c b/userland/libradiator.c
new file mode 100644
index 0000000..15d2e21
--- /dev/null
+++ b/userland/libradiator.c
@@ -0,0 +1,364 @@
+#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 <unistd.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(Radiator *r)
+{
+ int i=BRAKE;
+ printf (" Motor braking\n");
+ return cp210x_set_gpio(r->cp210x,i);
+}
+
+static int
+drive_motor (Radiator*r, 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 cp210x_set_gpio(r->cp210x,i);
+}
+
+
+static int
+sensor_led_power (Radiator *r, int on)
+{
+
+unsigned int control;
+
+printf (" Rotation sensor power %s\n", on ? "on" : "off");
+
+return cp210x_set_dtr(r->cp210x,on);
+
+}
+
+
+static int count_steps (Radiator *r, int count)
+{
+ int counted = 0;
+ struct timeval tv;
+ char buf[128];
+ int i;
+ fd_set rfds;
+
+ printf (" Counting %d steps", count);
+ fflush(stdout);
+
+
+ while (count)
+ {
+ i=cp210x_read(r->cp210x,buf,sizeof(buf),TIMEOUT);
+
+ if (i==0) break;
+
+ 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;
+}
+
+
+
+
+static int
+drive_motor_count (Radiator * r, int dir, int steps)
+{
+ int s1, s2, ret;
+
+ sensor_led_power (r, 1);
+ drive_motor (r, dir);
+ s1 = count_steps (r, steps);
+ if (s1 != steps) {
+ printf (" Hit end stop\n");
+ drive_motor (r, 0);
+ } else {
+ brake_motor(r);
+ }
+ s2 = count_steps (r, -1);
+ sensor_led_power (r, 0);
+ drive_motor (r, 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;
+}
+
+void
+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;
+}
+len+=2;
+
+return len;
+}
+
+
+static int make_usb_descriptor(uint8_t *dst,char *src)
+{
+int len;
+len=utf16(dst+2,(uint8_t *) src);
+//memcpy(dst+2,src,len);
+dst[0]=len+2;
+dst[1]=0x3;
+return len+2;
+}
+
+
+int
+radiator_program (Radiator * r)
+{
+ uint8_t buf[256];
+ struct cp210x_port_config config;
+ memset (&config, 0, sizeof (config));
+ uint16_t vid,pid;
+ int len;
+
+
+ cp210x_get_portconf(r->cp210x,&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;
+
+
+ cp210x_set_portconf(r->cp210x,&config);
+
+ memset (&config, 0, sizeof (config));
+ cp210x_get_portconf(r->cp210x,&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);
+
+ memset(buf,0,sizeof(buf));
+
+vid=0x413c;
+
+ cp210x_set_vid(r->cp210x,vid);
+
+pid=0x9500;
+ cp210x_set_pid(r->cp210x,pid);
+
+ len=make_usb_descriptor(buf,"USB Radiator Valve");
+ cp210x_set_product(r->cp210x, buf,len);
+
+ len=make_usb_descriptor(buf,"000001");
+ cp210x_set_serial (r->cp210x,buf,len);
+
+ len=make_usb_descriptor(buf,"Global Panaceas");
+ cp210x_set_mfg (r->cp210x,buf,len);
+
+ cp210x_reset(r->cp210x);
+
+ return 0;
+}
+
+Radiator *
+radiator_open (char *path, int quiet)
+{
+ Radiator *r = malloc (sizeof (Radiator));
+
+
+ r->cp210x=cp210x_open();
+
+ if (!r->cp210x) {
+ free(r);
+ return NULL;
+ }
+
+
+ if (!quiet)
+ radiator_calibrate (r);
+
+ return r;
+}
+
+
+void
+radiator_close (Radiator * r)
+{
+ cp210x_close(r->cp210x);
+ free (r);
+}
+
+
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-17 09:16:46 +0000