Diff: main.cpp

Revision:

6:43b59cb5dcd9

Parent:

5:c2e3d6e70d85

--- a/main.cpp	Thu Dec 14 06:55:10 2017 +0000
+++ b/main.cpp	Thu Dec 21 01:11:16 2017 +0000
@@ -4,36 +4,79 @@
 #include "rtos.h"
 #include "string.h"
 
+#define min(a, b) (a)<(b) ? (a) : (b)
+#define max(a, b) (a)>(b) ? (a) : (b)
+#define clamp(val, floor, ceil) min(max(val, floor), ceil)
+
 extern "C" void mbed_reset();
 
 Serial pc(USBTX, USBRX);
 //macOS: screen /dev/tty.usbmodem{num} {baud rate}
-//Windows: Realterm lol
-
-Timer y1_t;
-Timer x1_t;
-Timer y2_t;
-Timer x2_t;
+//Windows: Realterm
 
-InterruptIn y1(p16);
-InterruptIn x1(p15);
-InterruptIn y2(p18);
-InterruptIn x2(p17);
+class Axis {
+    public:
+        Axis(PinName pin) : _pin(pin) {
+            _t.reset();
+            _center = 0;
+            _scale = 1.0;
+            _pin.rise(this, &Axis::start);
+            _pin.fall(this, &Axis::stop);
+        }
+        
+        int8_t read() {
+            int val = _t.read_us();
+            return val < 1000 ? clamp((val - _center)*_scale, -128, 127) : -128;
+        }
+        
+        int16_t read_raw() {
+            return _t.read_us();
+        }
+        
+        void setup(int center, int range) {
+            _center = center;
+            _scale = 255.0/range;
+        }
+        
+        bool connected() {
+            int val = _t.read_us();
+            return val == clamp(val, 10, 1000);
+        }
+    
+    private:
+        InterruptIn _pin;
+        Timer _t;
+        int _center;
+        float _scale;
+        
+        void start() {
+            _t.reset();
+            _t.start();
+        }
 
-volatile int y1_pulse;
-volatile int x1_pulse;
-volatile int y2_pulse;
-volatile int x2_pulse;
+        void stop() {
+            _t.stop();
+        }
+        
+};
+
+Axis x1 = Axis(p15);
+Axis y1 = Axis(p16);
+Axis x2 = Axis(p17);
+Axis y2 = Axis(p18);
+
 volatile int buttons;
 
 DigitalOut trig(p19);
 
-BusIn buttons_raw(p21, p22, p23, p24);
-
-//first row: necessary garbage, size, mode, reserved x2
+//1st row: necessary garbage, size, mode, reserved, reserved
+//2nd row: x1 center, x1 range, y1 center, y1 range, reserved
+//3rd row: x2 center, x2 range, y2 center, y2 range, reserved
 //following rows: input, min, max, output, value
 int bindings[128][5];
-/*int bindings[128][5] = {{0xFFFFFFFF, 7, 0, 0, 0},
+/*int bindings[128][5] = {{0xFFFFFFFF, 9, 0, 0, 0},
+                      {128, 255, 128, 255, 0},
+                      {128, 255, 128, 255, 0},
                       {0, 0, 100, 4, (int)'a'},
                       {0, 400, 1000, 4, (int)'d'},
                       {1, 0, 100, 4, (int)'w'},
@@ -48,74 +91,9 @@
                       
 LocalFileSystem local("local");
 
-void start_y1() {
-    y1_t.reset();
-    y1_t.start();
-}
-
-void stop_y1() {
-    y1_t.stop();
-    y1_pulse = y1_t.read_us();
-}
-
-void start_x1() {
-    x1_t.reset();
-    x1_t.start();
-}
-
-void stop_x1() {
-    x1_t.stop();
-    x1_pulse = x1_t.read_us();
-}
-void start_y2() {
-    y2_t.reset();
-    y2_t.start();
-}
-
-void stop_y2() {
-    y2_t.stop();
-    y2_pulse = y2_t.read_us();
-}
-
-void start_x2() {
-    x2_t.reset();
-    x2_t.start();
-}
-
-void stop_x2() {
-    x2_t.stop();
-    x2_pulse = x2_t.read_us();
-}
-
-
-void analog_thread() {
-    
-    buttons_raw.mode(PullUp);
-    
+void analog_trig() {
+    trig = 0;
     trig = 1;
-    y1_pulse = 0;
-    x1_pulse = 0;
-    y2_pulse = 0;
-    x2_pulse = 0;
-    
-    y1.rise(&start_y1);
-    y1.fall(&stop_y1);
-    
-    x1.rise(&start_x1);
-    x1.fall(&stop_x1);
-    
-    y2.rise(&start_y2);
-    y2.fall(&stop_y2);
-    
-    x2.rise(&start_x2);
-    x2.fall(&stop_x2);
-
-    while(1) {
-        trig = 0;
-        trig = 1;
-        buttons = ~buttons_raw&0xF;
-        Thread::wait(1);
-    }
 }
 
 
@@ -124,8 +102,8 @@
     pc.printf("---------------------------------\r\n");
     
     while(1) {
-        pc.printf("Joystick 1 - %d, %d, %X \r\n", x1_pulse, y1_pulse, buttons&0x3);
-        pc.printf("Joystick 2 - %d, %d, %X \r\n", x2_pulse, y2_pulse, (buttons>>2)&0x3);
+        pc.printf("Joystick 1 - %d, %d, %X \r\n", x1.read_raw(), y1.read_raw(), buttons&0x3);
+        pc.printf("Joystick 2 - %d, %d, %X \r\n", x2.read_raw(), y2.read_raw(), (buttons>>2)&0x3);
         pc.printf("\r\n");
         Thread::wait(500);
     }
@@ -134,77 +112,87 @@
 
 void keys_mouse_output_thread() {
     USBMouseKeyboard keys_mouse;
+    BusIn buttons_raw(p21, p22, p23, p24);
+    buttons_raw.mode(PullUp);
+    
     int mouse[4];
     int value, trigval;
     
     while(true) {
         memset(mouse, 0, sizeof(mouse));
+        buttons = ~buttons_raw&0xF;
         
-        for (int i=1; i<bindings[0][1]; i++) {
+        for (int i=3; i<bindings[0][1]; i++) {
             //pc.printf("Checking %d: ", i);
             switch (bindings[i][0]) {
-                case 0: value = x1_pulse; trigval = x1_pulse; break;
-                case 1: value = y1_pulse; trigval = y1_pulse; break;
-                case 2: value = x2_pulse; trigval = x2_pulse; break;
-                case 3: value = y2_pulse; trigval = y2_pulse; break;
-                case 4: trigval = (buttons & 0x1)*255; value = bindings[i][4]; break;
-                case 5: trigval = ((buttons>>1) & 0x1)*255; value = bindings[i][4]; break;
-                case 6: trigval = ((buttons>>2) & 0x1)*255; value = bindings[i][4]; break;
-                case 7: trigval = ((buttons>>3) & 0x1)*255; value = bindings[i][4]; break;
+                case 0: trigval = x1.read(); break;
+                case 1: trigval = y1.read(); break;
+                case 2: trigval = x2.read(); break;
+                case 3: trigval = y2.read(); break;
+                case 4: trigval = (buttons & 0x1)*127; break;
+                case 5: trigval = ((buttons>>1) & 0x1)*127; break;
+                case 6: trigval = ((buttons>>2) & 0x1)*127; break;
+                case 7: trigval = ((buttons>>3) & 0x1)*127; break;
             }
+            value = bindings[i][4];
             
             if (trigval >= bindings[i][1] && trigval <= bindings[i][2]) {
-                pc.printf("Triggered : %d, %d, %d\r\n", i, trigval, value);
+                //pc.printf("Triggered : %d, %d, %d\r\n", i, trigval, value);
                 switch (bindings[i][3]) {
-                    case 0: mouse[0] = value/10; break; //Mouse X
-                    case 1: mouse[1] = value/10; break; //Mouse Y
-                    case 2: mouse[2] |= bindings[i][4]; break; //Mouse buttons
-                    case 3: mouse[3] = value/10; break; //Mouse scroll
-                    case 4: keys_mouse.keyCode(bindings[i][4]); break; //Keypress
+                    case 0: mouse[0] = trigval/value; break; //Mouse X
+                    case 1: mouse[1] = trigval/value; break; //Mouse Y
+                    case 2: mouse[2] |= value; break; //Mouse buttons
+                    case 3: mouse[3] = trigval/value; break; //Mouse scroll
+                    case 4: keys_mouse.keyCode(value); break; //Keypress
                 }  
             }
         }
         keys_mouse.update(mouse[0], mouse[1], mouse[2], mouse[3]);
-        Thread::wait(20);
+        Thread::wait(15);
     }
 }
 
 
 void joystick_output_thread() {
     USBJoystick joystick;
+    BusIn buttons_raw(p21, p22, p23, p24);
+    buttons_raw.mode(PullUp);
+    
     int joy[6];
     int value, trigval;
     
     while(true) {
         memset(joy, 0, sizeof(joy));
+        buttons = ~buttons_raw&0xF;
         
-        for (int i=1; i<bindings[0][1]; i++) {
+        for (int i=3; i<bindings[0][1]; i++) {
             //pc.printf("Checking %d: ", i);
             switch (bindings[i][0]) {
-                case 0: value = x1_pulse; trigval = x1_pulse; break;
-                case 1: value = y1_pulse; trigval = y1_pulse; break;
-                case 2: value = x2_pulse; trigval = x2_pulse; break;
-                case 3: value = y2_pulse; trigval = y2_pulse; break;
-                case 4: trigval = (buttons & 0x1)*255; value = bindings[i][4]; break;
-                case 5: trigval = ((buttons>>1) & 0x1)*255; value = bindings[i][4]; break;
-                case 6: trigval = ((buttons>>2) & 0x1)*255; value = bindings[i][4]; break;
-                case 7: trigval = ((buttons>>3) & 0x1)*255; value = bindings[i][4]; break;
+                case 0: trigval = x1.read(); break;
+                case 1: trigval = y1.read(); break;
+                case 2: trigval = x2.read(); break;
+                case 3: trigval = y2.read(); break;
+                case 4: trigval = (buttons & 0x1)*127; break;
+                case 5: trigval = ((buttons>>1) & 0x1)*127; break;
+                case 6: trigval = ((buttons>>2) & 0x1)*127; break;
+                case 7: trigval = ((buttons>>3) & 0x1)*127; break;
             }
+            value = bindings[i][4];
             
             if (trigval >= bindings[i][1] && trigval <= bindings[i][2]) {
                 //pc.printf("Triggered : %d, %d, %d\r\n", i, trigval, value);
                 switch (bindings[i][3]) {
-                    case 0: joy[2] = value; break;//Joy X
-                    case 1: joy[3] = value; break; //Joy Y
-                    case 2: joy[0] = value; break; //Joy throttle
-                    case 3: joy[1] = value; break; //Joy rudder
-                    case 4: joy[4] |= bindings[i][4]; break; //Joy buttons
-                    case 5: joy[5] |= bindings[i][4]; break; //Joy hat
+                    case 0: joy[2] = trigval/value; break;//Joy X
+                    case 1: joy[3] = trigval/value; break; //Joy Y
+                    case 2: joy[0] = trigval/value; break; //Joy throttle
+                    case 3: joy[1] = trigval/value; break; //Joy rudder
+                    case 4: joy[4] |= value; break; //Joy buttons
+                    case 5: joy[5] |= value; break; //Joy hat
                 }  
             }
         }
         joystick.update(joy[0], joy[1], joy[2], joy[3], joy[4], joy[5]);
-        Thread::wait(20);
+        Thread::wait(15);
     }
 }
 
@@ -217,18 +205,23 @@
     fread(bindings[1], 5*sizeof(int), bindings[0][1]-1, bf);
     fclose(bf);
     
-    Thread analogThread;
-    Thread outputThread;
+    x1.setup(bindings[1][0], bindings[1][1]);
+    y1.setup(bindings[1][2], bindings[1][3]);
+    x2.setup(bindings[2][0], bindings[2][1]);
+    y2.setup(bindings[2][2], bindings[2][3]);
     
-    analogThread.start(analog_thread);
-    Thread::wait(100);
+    //Thread analogThread(analog_thread);
+    Ticker analog_ticker;
+    analog_ticker.attach(&analog_trig, 0.05);
+    
+    Thread outputThread;
+    //Thread::wait(100);
     switch(bindings[0][2]) {
         case 0: outputThread.start(keys_mouse_output_thread); break;
         case 1: outputThread.start(joystick_output_thread); break;
     }
     
-    //Thread debugThread;
-    //debugThread.start(debug_thread);
+    Thread debugThread(debug_thread);
     
     while(1) Thread::yield();
 }