Diff: PS_PAD.cpp

Revision:

1:840370e1dcce

Parent:

0:6eeefcf5a37a

--- a/PS_PAD.cpp	Wed Dec 11 04:42:44 2013 +0000
+++ b/PS_PAD.cpp	Thu Dec 12 09:04:45 2013 +0000
@@ -11,6 +11,7 @@
     _cs = 1;
     _vib1 = 0;
     _vib2 = 0;
+    _connected = false;
 }
 
 PS_PAD::PS_PAD (SPI &spi, PinName cs) : _spi(spi), _cs(cs) {
@@ -19,6 +20,7 @@
     _cs = 1;
     _vib1 = 0;
     _vib2 = 0;
+    _connected = false;
 }
 
 int PS_PAD::init () {
@@ -51,17 +53,26 @@
     cmd[3] = _vib1;
     cmd[4] = _vib2;
     send(cmd, 9, buf);
-    if (buf[2] == 0xff) {
+    if (buf[2] != 0x5a) {
         return -1;
     }
 
     for (i = 0; i < 6; i ++) {
         _pad[i] = buf[3 + i];
     }
+    _connected = true;
     return 0;
 }
 
 int PS_PAD::read (TYPE t) {
+    if (!_connected) {
+        if (t <= BUTTONS) {
+            return 0;
+        } else {
+            return 0x80;
+        }
+    }
+
     switch (t) {
     case PAD_LEFT:
         return _pad[0] & 0x80 ? 0 : 1;
@@ -95,30 +106,16 @@
         return _pad[1] & 0x02 ? 0 : 1;
     case PAD_L2:
         return _pad[1] & 0x01 ? 0 : 1;
+    case BUTTONS:
+        return ~((_pad[1] << 8) | _pad[0]) & 0xffff;
     case ANALOG_RX:
-        if (_pad[2] < 0x80) {
-            return (_pad[2] - 0x7f) * 100 / 0x7f;
-        } else {
-            return (_pad[2] - 0x7f) * 100 / 0x7f;
-        }
+        return _pad[2] - 0x80;
     case ANALOG_RY:
-        if (_pad[3] < 0x80) {
-            return (_pad[3] - 0x7f) * 100 / 0x7f;
-        } else {
-            return (_pad[3] - 0x7f) * 100 / 0x7f;
-        }
+        return _pad[3] - 0x80;
     case ANALOG_LX:
-        if (_pad[4] < 0x80) {
-            return (_pad[4] - 0x7f) * 100 / 0x7f;
-        } else {
-            return (_pad[4] - 0x7f) * 100 / 0x7f;
-        }
+        return _pad[4] - 0x80;
     case ANALOG_LY:
-        if (_pad[5] < 0x80) {
-            return (_pad[5] - 0x7f) * 100 / 0x7f;
-        } else {
-            return (_pad[5] - 0x7f) * 100 / 0x7f;
-        }
+        return _pad[5] - 0x80;
     }
     return 0;
 }