Diff: main.cpp

Revision:

1:99a635cd2f19

Parent:

0:038c12ba8a60

Child:

2:22aa8109e2f0

--- a/main.cpp	Fri Oct 13 23:01:29 2017 +0000
+++ b/main.cpp	Tue Oct 24 15:37:15 2017 +0000
@@ -6,6 +6,7 @@
 #define ELBOW 1
 #define WRIST 2
 #define ARMPIT 3
+DigitalOut nReset(dp26);
 #if 0
 DigitalOut debugLED1(dp24);
 DigitalOut debugLED2(dp18);
@@ -41,7 +42,13 @@
 };
 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax);
 AnalogIn ArmSense[4] = {AnalogIn(dp10),AnalogIn(dp9),AnalogIn(dp11),AnalogIn(dp13)};
-
+void waitTime(float ti){
+    Timer t;
+    t.start();
+    while(ti > t.read());
+    t.stop();
+    return;
+}
 //AnalogInLPF ArmSense[4] = {AnalogInLPF(dp10),AnalogInLPF(dp9),AnalogInLPF(dp11),AnalogInLPF(dp13)};
 Nunchuck ctrl(dp5,dp27);
 Serial dev(dp16,dp15);
@@ -57,20 +64,27 @@
         timer.reset();
     }
 }
+void reset(){
+    nReset = 0;
+    waitTime(0.001);
+    nReset = 1;
+}
 int main() {
+    nReset = true;
     tmp = new char[dataNum];
     debugLED1 = 1;
     debugLED2 = 0;
     for(int i = 0 ; i < 50; i++){
         debugLED2 = !debugLED2;
-        wait(0.1);
+        waitTime(0.1);
     }
+    ctrl.offset_();
     dev.baud(115200);
     dev.attach(RX,Serial::RxIrq);
     timer.start();
     while(1) {
         //送信データ格納
-            
+        ctrl.getdata();
         debugLED1 = 1;
         tmp[0] = '0';
         tmp[1] = ctrl.analogx();//ヌンチャクアナログX
@@ -88,14 +102,27 @@
         for(int i = 0 ; i < dataNum ; i++){
             dev.putc(SerialData[i]);
         }
-        while(timer.read_ms() > 300){
+        while(timer.read_ms() > 1000){
+            double x = (int8_t)ctrl.analogx();//ヌンチャクアナログX
+            double y = (int8_t)ctrl.analogy();//ヌンチャクアナログy
+            bool b1 = ctrl.buttonc();
+            bool b2 = ctrl.buttonz();//ヌンチャクzボタンとCボタン
+            double sieta = atan2(x,y);
             debugLED2 = true;
-            wait(0.5);
+            waitTime(0.5);
             debugLED2 = false;
-            wait(0.5);
+            waitTime(0.5);
+            if(sieta < 3.14/3.0 && sieta > -3.14/3.0 && b1 == true && b2 == true){
+                reset();
+                break;
+            }
+            if(timer.read_ms() > 1500){
+                reset();
+                break;
+            }
         }
         debugLED1 = 0;
-        wait_ms(1);
+        wait_ms(15);
         //delete SerialData;
     }