Diff: ds3_si.cpp

Revision:

0:4ac4c7635035

Child:

1:3fbf08dc60b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ds3_si.cpp	Thu Sep 07 02:22:00 2017 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "ds3_si.h"
+
+ds3_si::ds3_si(PinName tx, PinName rx):serial(tx, rx), pc(USBTX, USBRX)
+{
+    in[0] = 128;
+    in[1] = 0;
+    in[2] = 0;
+    in[3] = 64;
+    in[4] = 64;
+    in[5] = 64;
+    in[6] = 64;
+    in[7] = 0;
+    
+    serial.baud(2400);
+    serial.attach(this, &ds3_si::serialin, Serial::RxIrq);
+}
+
+ds3_si::ds3_si(PinName tx, PinName rx, int baudrate):serial(tx, rx), pc(USBTX, USBRX)
+{
+    in[0] = 128;
+    in[1] = 0;
+    in[2] = 0;
+    in[3] = 64;
+    in[4] = 64;
+    in[5] = 64;
+    in[6] = 64;
+    in[7] = 0;
+    
+    serial.baud(baudrate);
+    serial.attach(this, &ds3_si::serialin, Serial::RxIrq);
+}
+
+void ds3_si::serialin()
+{
+    if(serial.readable())
+    {
+        while(serial.getc() != 128) {}
+        for(short i = 1; i < 8; i++)
+        {
+            *(in + i) = serial.getc();
+        }
+    }
+}
+
+bool ds3_si::buttonstate(short order)
+{
+    return in[(order >> 7) + 1] & order;
+}
+
+int ds3_si::analogstate(short order)
+{
+    return (order % 2) ? in[order] - 64 : 64 - in[order];
+}
+
+int ds3_si::getinputdata(short order)
+{
+    return in[order];
+}
+
+double ds3_si::getangle(short order)
+{
+    int x = analogstate(order * 2 + 1);
+    int y = analogstate(order * 2 + 2);
+    if(x || y)  return atan2(double(y), double(x));
+    else        return 10.0;//NULL
+}
+
+void ds3_si::usb_debug()
+{
+    for(short i = 0; i < 8; i++)
+        pc.printf("%4d", in[i]);
+    pc.printf("  %4.1lf  %4.1lf", getangle(L) * 180 / PI, getangle(R) * 180 / PI);
+    pc.printf("\n");
+}
\ No newline at end of file