Diff: bot/controller/controller.cpp

Revision:

1:269914e0aa07

Parent:

0:757e9800c103

Child:

2:ea151e05033a

--- a/bot/controller/controller.cpp	Mon Aug 21 12:20:14 2017 +0000
+++ b/bot/controller/controller.cpp	Tue Aug 22 05:07:57 2017 +0000
@@ -2,4 +2,50 @@
 
 Controller::Controller() : FEP(controllerTX, controllerRX, ADDR)
 {
+    for(int i = 0; i < 7; i++) Button1[i] = 0;
+    for(int i = 0; i < 6; i++) Button2[i] = 0;
+    for(int i = 0; i < 6; i++) data[i] = 0;
+    for(int i = 0; i < 4; i++) stick[i] = 0;
+    for(int i = 0; i < 2; i++) degree[i] = 0;
+    for(int i = 0; i < 2; i++) norm[i] = 0;
+    for(int i = 0; i < 2; i++) vector[i] = 0;
+    fepTemp = 0;
+}
+
+void Controller::receiveState()
+{
+    fepTemp = read(data, 6);
+    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);
+        }
+        getStickData();
+    } else if(fepTemp == FEP_NO_RESPONSE) {
+    } else {
+    }
+}
+
+void Controller::getStickData()
+{
+    degree[0] = (atan2(stick[1], -stick[0])) * (180 / M_PI);
+    degree[1] = (atan2(stick[3], -stick[2])) * (180 / M_PI);
+
+    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;
+
+    vector[0] = degree[0];
+    vector[1] = norm[0];
 }
\ No newline at end of file