Initial import

1 files changed, 187 insertions, 0 deletions

diff --git a/src/joy.c b/src/joy.c
new file mode 100644
index 0000000..99ade75
--- /dev/null
+++ b/src/joy.c
@@ -0,0 +1,187 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <pc.h>
+#include <dos.h>
+
+#include "ubox_vga.h"
+#include "ubox_keyb.h"
+#include "ubox_control.h"
+
+#include "ubox_joy.h"
+
+#define PORT 0x201
+
+#define BUTTON1 0x10
+#define BUTTON2 0x20
+#define AXISX   1
+#define AXISY   2
+
+#define TIMEOUT 9999
+#define CALIBRATING_LOOPS 10
+
+static uint16_t j_up, j_down, j_left, j_right;
+
+static uint16_t read_axis(uint16_t *x, uint16_t *y)
+{
+    uint16_t i, value;
+    uint16_t mask = AXISX | AXISY;
+
+    *x = *y = 0;
+
+    disable();
+    outportb(PORT, 0);
+    for (i = 1; mask && i < TIMEOUT; i++)
+    {
+        value = inportb(PORT) ^ mask;
+        if (value & AXISX)
+        {
+            *x = i;
+            mask ^= AXISX;
+        }
+        if (value & AXISY)
+        {
+            *y = i;
+            mask ^= AXISY;
+        }
+    }
+    enable();
+
+    return i;
+}
+
+static uint8_t calibrate_release(uint16_t *x, uint16_t *y)
+{
+    uint8_t i;
+    uint16_t xacc, yacc;
+
+    while (1)
+    {
+        if (ubox_keys[UBOX_KEY_ESC])
+        {
+            while (ubox_keys[UBOX_KEY_ESC])
+                ubox_wait_vsync();
+            return 0;
+        }
+
+        xacc = yacc = 0;
+        for (i = 0; i < CALIBRATING_LOOPS; i++)
+        {
+            read_axis(x, y);
+            xacc += *x;
+            yacc += *y;
+        }
+
+        if (!(inportb(PORT) & BUTTON1))
+        {
+            while (!(inportb(PORT) & BUTTON1))
+                ubox_wait_vsync();
+
+            *x = xacc / CALIBRATING_LOOPS;
+            *y = yacc / CALIBRATING_LOOPS;
+
+            return 1;
+        }
+        ubox_wait_vsync();
+    }
+}
+
+static uint8_t calibrate_circle(uint16_t joy_values[4])
+{
+    uint8_t i;
+    uint16_t x, y;
+
+    while (1)
+    {
+        if (ubox_keys[UBOX_KEY_ESC])
+        {
+            while (ubox_keys[UBOX_KEY_ESC])
+                ubox_wait_vsync();
+            return 0;
+        }
+
+        for (i = 0; i < CALIBRATING_LOOPS; i++)
+        {
+            read_axis(&x, &y);
+            // up
+            if (y < joy_values[0])
+                joy_values[0] = y;
+            // down
+            if (y > joy_values[1])
+                joy_values[1] = y;
+            // left
+            if (x < joy_values[2])
+                joy_values[2] = x;
+            // right
+            if (x > joy_values[3])
+                joy_values[3] = x;
+        }
+
+        if (!(inportb(PORT) & BUTTON1))
+        {
+            while (!(inportb(PORT) & BUTTON1))
+                ubox_wait_vsync();
+
+            return 1;
+        }
+        ubox_wait_vsync();
+    }
+}
+
+uint8_t ubox_joy_detect()
+{
+    uint16_t xcenter, ycenter;
+
+    // this read doesn't matter
+    if (read_axis(&xcenter, &ycenter) == TIMEOUT)
+        return 0;
+
+    printf("Joystick detected!\n");
+
+    printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
+    if (!calibrate_release(&xcenter, &ycenter))
+        return 0;
+
+    // up, down, left, right
+    uint16_t joy_values[4] = { ycenter, ycenter, xcenter, xcenter };
+
+    printf("Please move the joystick in circles and then press BUTTON 1 (ESC to abort)\n");
+    if (!calibrate_circle(joy_values))
+        return 0;
+
+    printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
+    if (!calibrate_release(&xcenter, &ycenter))
+        return 0;
+
+    j_up = ycenter - ((ycenter - joy_values[0]) / 2);
+    j_down = ycenter + ((joy_values[1] - ycenter) / 2);
+    j_left = xcenter - ((xcenter - joy_values[2]) / 2);
+    j_right = xcenter + ((joy_values[3] - xcenter) / 2);
+
+    return 1;
+}
+
+uint8_t ubox_joy_read()
+{
+    uint8_t r = UBOX_CTL_NONE;
+    uint8_t b;
+
+    b = inportb(PORT);
+    if (!(b & BUTTON1))
+        r |= UBOX_CTL_FIRE1;
+    if (!(b & BUTTON2))
+        r |= UBOX_CTL_FIRE2;
+
+    uint16_t x, y;
+
+    read_axis(&x, &y);
+    if (y < j_up)
+        r |= UBOX_CTL_UP;
+    if (y > j_down)
+        r |= UBOX_CTL_DOWN;
+    if (x < j_left)
+        r |= UBOX_CTL_LEFT;
+    if (x > j_right)
+        r |= UBOX_CTL_RIGHT;
+
+    return r;
+}