Diff: main.cpp

Revision:

5:9332620c08c6

Parent:

2:fd71ffefefe5

Child:

6:f4a6cc918833

--- a/main.cpp	Mon Aug 27 06:15:31 2018 +0000
+++ b/main.cpp	Tue Dec 18 19:00:19 2018 +0900
@@ -1,21 +1,30 @@
 #include "mbed.h"
 #include "measuring_wheel.h"
 
-Serial pc(USBTX, USBRX,115200);
-MeasuringWheel measuring(PA_8,PA_9,PA_6,PA_7,PA_0,PA_1);
-
-int main()
-{
-    measuring.wheelDiameter(48,48,48);
-    while(true) {
-
+Serial pc(USBTX, USBRX, 115200);
+MeasuringWheel measuring(PA_9, PA_8, PA_0, PA_1, PB_7, PB_6);
+// MeasuringWheel measuring(PB_6,PB_7,PA_8,PA_9,PA_6,PA_7);
 
-
-//        pc.printf("fsjflsz\r\n");
-//        pc.printf("%d,%d,\n",(int)measuring.getOutX(),(int)measuring.getOutY());
-//        pc.printf("w1 = %f, w2 = %f, w3 = %f\r\n",measuring.getWheel1(),measuring.getWheel2(),measuring.getWheel3());
-            pc.printf("yaw = %f\r\n",measuring.getjyroAngle());
-//        wait(0.1);
+int main() {
+  measuring.wheelDiameter(48, 48, 48);
+  while (true) {
+    // pc.printf("%d,%d,\n",(int)measuring.getOutX(),(int)measuring.getOutY());
+    //        pc.printf("w1 = %f, w2 = %f, w3 =
+    //        %f\r\n",measuring.getWheel1(),measuring.getWheel2(),measuring.getWheel3());
+    // pc.printf("yaw = %f\r\n",measuring.getjyroAngle());
+    /// Thread::wait(100);
+    // measuring.debugLed[1]->write(!measuring.debugLed[1]->read());
+    if (abs((int)measuring.getjyroAngle()) % 6 > 3) {
+      measuring.debugLed[1]->write(true);
+    } else {
+      measuring.debugLed[1]->write(false);
     }
-
+    if (abs((int)measuring.getOutX()) % 6 > 3) {
+      measuring.debugLed[2]->write(true);
+    } else {
+      measuring.debugLed[2]->write(false);
+    }
+    // kwait(fabs(measuring.getjyroAngle()) / 1.0);
+    wait(0.1);
+  }
 }