Diff: Joystick.cpp

Revision:

0:f76f52dc57f7

Child:

3:cd36fdbb23f8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Joystick.cpp	Thu Feb 09 19:13:02 2017 +0000
@@ -0,0 +1,69 @@
+#include "Joystick.h"
+
+int mapInto(int x, int in_min, int in_max, int out_min, int out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+int constrain(int x, int x_min, int x_max)
+{
+    return x >= x_min && x <= x_max ? x : (x < x_min ? x_min : x_max);
+}
+
+void Joystick::process()
+{
+    JoystickValue newValue(_xIn.read_u16(), _yIn.read_u16());
+    if (_calibrated)
+    {
+        if (newValue.x < _center.x - JOYSTICK_CENTER_DEADZONE)
+        {
+            newValue.x = newValue.x - (_center.x - JOYSTICK_CENTER_DEADZONE);
+        }
+        else if (newValue.x > _center.x + JOYSTICK_CENTER_DEADZONE)
+        {
+            newValue.x = newValue.x - (_center.x + JOYSTICK_CENTER_DEADZONE);
+        }
+        else
+        {
+            newValue.x = 0;
+        }
+        if (newValue.y < _center.y - JOYSTICK_CENTER_DEADZONE)
+        {
+            newValue.y = newValue.y - (_center.y - JOYSTICK_CENTER_DEADZONE);
+        }
+        else if (newValue.y > _center.y + JOYSTICK_CENTER_DEADZONE)
+        {
+            newValue.y = newValue.y - (_center.y + JOYSTICK_CENTER_DEADZONE);
+        }
+        else
+        {
+            newValue.y = 0;
+        }
+        newValue.x = constrain(mapInto(newValue.x, -(JOYSTICK_ADC_MAX - JOYSTICK_CENTER_DEADZONE * 2) / 2, (JOYSTICK_ADC_MAX - JOYSTICK_CENTER_DEADZONE * 2) / 2, -_range, _range), -_range, _range);
+        newValue.y = constrain(mapInto(newValue.y, -(JOYSTICK_ADC_MAX - JOYSTICK_CENTER_DEADZONE * 2) / 2, (JOYSTICK_ADC_MAX - JOYSTICK_CENTER_DEADZONE * 2) / 2, -_range, _range), -_range, _range);
+        if (_swapXY)
+        {
+            int t = newValue.x;
+            newValue.x = newValue.y;
+            newValue.y = t;
+        }
+        JoystickValue joyPrev(_joyValue);
+        _joyValue.x = _flipX ? -1 * newValue.x : newValue.x;
+        _joyValue.y = _flipY ? -1 * newValue.y : newValue.y;
+        if (_onChange != NULL && (_delta == 0 || abs(_joyValue.x - joyPrev.x) >= _delta || abs(_joyValue.y - joyPrev.y) >= _delta))
+        {
+            _onChange(_joyValue, joyPrev);
+        }
+    }
+    else
+    {
+        _center.x = (_center.x + newValue.x) / 2;
+        _center.y = (_center.y + newValue.y) / 2;
+        if (++_calibrationCounter >= JOYSTICK_CALIBRATION_CYCLES)
+		{
+			_calibrated = true;
+		}
+    }
+}
+
+