Diff: bot/controller/controller.cpp

Revision:

1:845af5425eec

Child:

15:9aa11febe517

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bot/controller/controller.cpp	Tue Sep 05 16:11:20 2017 +0900
@@ -0,0 +1,92 @@
+#include "controller.h"
+
+Controller::Controller() :
+    FEP(XBee1TX, XBee1RX, ADDR),
+    data(),
+    fepTemp(0),
+    button1(),
+    button2(),
+    stick(),
+    radian(),
+    norm()
+{
+}
+
+Controller::Controller(PinName FEPtx, PinName FEPrx, int addr) :
+    FEP(FEPtx, FEPrx, addr),
+    data(),
+    fepTemp(0),
+    button1(),
+    button2(),
+    stick(),
+    radian(),
+    norm()
+{
+}
+
+bool Controller::receiveState()
+{
+    fepTemp = FEP::read(data, DATA_SIZE);
+    if(fepTemp == FEP_SUCCESS) {
+        for(int i = 0; i < 7; i++) {
+            button1[i] = data[4] % 2;
+            data[4] /= 2;
+        }
+        for(int i=0; i<6; i++) {
+            button2[i] = data[5] % 2;
+            data[5] /= 2;
+        }
+        for(int i = 0; i < 4; i++) {
+            stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0);
+        }
+        setStick();
+    } else if(fepTemp == FEP_NO_RESPONSE) {
+        return 0;
+    } else {
+        return 0;
+    }
+    return 1;
+}
+
+void Controller::setStick()
+{
+    for(int i = 0; i < 4; i++) {
+        if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0;
+    }
+
+    radian[0] = atan2(stick[1], -stick[0]);
+    radian[1] = atan2(stick[3], -stick[2]);
+
+    norm[0] = hypot(stick[0], stick[1]);
+    norm[1] = hypot(stick[2], stick[3]);
+
+    if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
+    if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
+    if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
+    if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;
+}
+
+bool Controller::getButton1(int number) const
+{
+    return button1[number];
+}
+
+bool Controller::getButton2(int number) const
+{
+    return button2[number];
+}
+
+float Controller::getStick(int number) const
+{
+    return stick[number];
+}
+
+float Controller::getRadian(int number) const
+{
+    return radian[number];
+}
+
+float Controller::getNorm(int number) const
+{
+    return norm[number];
+}