From 250fb9aa02749277c9a7510195c6c81972256148 Mon Sep 17 00:00:00 2001
From: Bobby Dilley <bobby@dilley.uk>
Date: Sun, 9 Oct 2022 23:57:55 +0100
Subject: [PATCH] Turn JVS res to 10 and implement drive board

---
 src/lindbergh/driveboard.c | 59 ++++++++++++++++++++++++++++++++++++--
 src/lindbergh/graphics.c   |  4 +--
 src/lindbergh/jvs.c        |  2 +-
 3 files changed, 60 insertions(+), 5 deletions(-)

diff --git a/src/lindbergh/driveboard.c b/src/lindbergh/driveboard.c
index 9359f19..df2aaf4 100644
--- a/src/lindbergh/driveboard.c
+++ b/src/lindbergh/driveboard.c
@@ -1,6 +1,7 @@
 #include "string.h"
 
 #include "driveboard.h"
+#include "jvs.h"
 
 #define DRIVEBOARD_READY 0x00
 #define DRIVEBOARD_NOT_INIT 0x11
@@ -9,11 +10,16 @@
 int wheelTest = 0;
 unsigned char response = DRIVEBOARD_READY;
 
+double steerValue = 0.5;
+double force = 0.00;
+
 int initDriveboard()
 {
     wheelTest = 0;
     response = DRIVEBOARD_NOT_INIT;
 
+    setAnalogue(ANALOGUE_1, steerValue);
+
     return 0;
 }
 
@@ -57,18 +63,67 @@ ssize_t driveboardWrite(int fd, const void *buf, size_t count)
     }
     break;
 
+    case 0x80:
+    {
+        if (buffer[1] == 0 && buffer[2] == 0)
+        {
+            force = 0;
+        }
+
+        if (buffer[1] == 1 && buffer[2] == 1)
+        {
+
+            if (steerValue >= 0.9 && force > 0)
+                break;
+
+            if (steerValue <= 0.1 && force < 0)
+                break;
+
+            steerValue += force;
+            setAnalogue(ANALOGUE_1, (int)(steerValue * 1024));
+        }
+
+        printf("Driveboard move %f %f\n", steerValue, force);
+    }
+    break;
+
+    case 0x9e:
+    case 0x84:
+    {
+        response = DRIVEBOARD_READY;
+
+        if (buffer[1] == 1)
+            force = ((-1 * ((double)buffer[2] / 128.0)) * 2) / 100;
+
+        if (buffer[1] == 0)
+            force = ((1 - ((double)buffer[2] / 128.0)) * 2) / 100;
+
+        printf("Driveboard set force%f %f\n", steerValue, force);
+    }
+    break;
+
+    case 0xFA:
     case 0xFD:
     {
         printf("Driveboard: auto turn wheel mode\n");
+
         if (wheelTest)
         {
-            printf("Increment wheel until 0.9\n");
+            printf("Increment wheel until 0.9 -> %d\n", (int)(steerValue * 255));
+            steerValue += 0.09;
+            setAnalogue(ANALOGUE_1, (int)(steerValue * 1024));
+            response = DRIVEBOARD_BUSY;
+            if (steerValue >= 0.9)
+            {
+                wheelTest = 0;
+                response = DRIVEBOARD_READY;
+            }
         }
     }
     break;
 
     default:
-        printf("Driveboard: Unknown command received\n");
+        printf("Driveboard: Unknown command received %X\n", buffer[0]);
         break;
     }
 
diff --git a/src/lindbergh/graphics.c b/src/lindbergh/graphics.c
index b729b2b..9a2e966 100644
--- a/src/lindbergh/graphics.c
+++ b/src/lindbergh/graphics.c
@@ -197,8 +197,8 @@ int XNextEvent(Display *display, XEvent *event_return)
 
   case MotionNotify:
   {
-    setAnalogue(ANALOGUE_1, ((double)event_return->xmotion.x / (double)getConfig()->width) * 255.0);
-    setAnalogue(ANALOGUE_2, ((double)event_return->xmotion.y / (double)getConfig()->height) * 255.0);
+    setAnalogue(ANALOGUE_1, ((double)event_return->xmotion.x / (double)getConfig()->width) * 1024.0);
+    setAnalogue(ANALOGUE_2, ((double)event_return->xmotion.y / (double)getConfig()->height) * 1024.0);
   }
   break;
 
diff --git a/src/lindbergh/jvs.c b/src/lindbergh/jvs.c
index 988a105..cf32551 100644
--- a/src/lindbergh/jvs.c
+++ b/src/lindbergh/jvs.c
@@ -30,7 +30,7 @@ int initJVS()
     io.capabilities.switches = 14;
     io.capabilities.coins = 2;
     io.capabilities.players = 2;
-    io.capabilities.analogueInBits = 8;
+    io.capabilities.analogueInBits = 10;
     io.capabilities.rightAlignBits = 0;
     io.capabilities.analogueInChannels = 20;
     io.capabilities.generalPurposeOutputs = 20;