Diff: main.cpp

Revision:

23:29b2722bd753

Parent:

22:a19be3b505b7

Child:

24:0ad1725c7849

--- a/main.cpp	Sun Feb 17 12:45:35 2019 +0000
+++ b/main.cpp	Mon Feb 18 06:08:24 2019 +0000
@@ -102,10 +102,10 @@
 
 /*超音波はRaspberryPiに積む*/
 
-DigitalOut led1(PA_0);  //黄色のコネクタ
-DigitalOut led2(PA_1);
-DigitalOut led3(PB_5); 
-DigitalOut led4(PB_4);
+DigitalOut led1(PA_0);  //tanakaOK kimuraは動作不良
+DigitalOut led2(PA_1);  //tanakaOK kimuraOK
+DigitalOut led3(PB_5);  //使ってないよ
+DigitalOut led4(PB_4);  //使ってないよ
 
 
 //外付けコンパス
@@ -504,6 +504,11 @@
 
 
 void setup(){
+    
+    led1 = 1;
+    led2 = 1;
+    led3 = 1;
+    led4 = 1;
      
     SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
                &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
@@ -541,6 +546,10 @@
         for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
         pc.printf("\t%3.2f\t",nowAngle_HMC);
         pc.printf("\r\n");
+        led1 = !led1;
+        led2 = !led2;
+        led3 = !led3;
+        led4 = !led4;
     }
     
     FirstROLL = nowAngle[ROLL];
@@ -554,6 +563,11 @@
     nowAngle_HMC -=g_FirstYAW_HMC;
     if(nowAngle_HMC<0)nowAngle_HMC+=360;
     
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+        
     wait(0.2);
     
     pc.printf("All initialized\r\n");