Diff: main.cpp

Revision:

23:4a490fa8bf4a

Parent:

22:da9893b71f24

Child:

24:6ab61527855d

--- a/main.cpp	Mon Feb 15 06:49:37 2021 +0000
+++ b/main.cpp	Mon Feb 15 06:57:00 2021 +0000
@@ -58,8 +58,8 @@
 Timer t;
 
 int ch1, ch2;
-int channels[16];
-//float rc1, rc2; // channels[16]の1,2要素めにそれぞれ移行ずみ
+float rc[16];
+//float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ
 double pitch = 0.0;
 double roll = 0.0;
 double yaw = 0.0;
@@ -126,11 +126,9 @@
             for (int i =0 ; i < 16;i ++){
                 int data;
                 float mapped_data;
-                data = int(sbus.getData(i));
-                mapped_data = LP_rc * float(map(data,368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * channels[i]; // mapped input
-                channels[i] = int(mapped_data); // channelの値をmapしたものを格納
+                rc[i] = LP_rc * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc[i]; // mapped input
             }
-            pc.printf("data: %d",sbus.getData(1));
+            pc.printf("data: %d",rc[1]);
         }
 
         // gx gy gz ax ay az
@@ -157,21 +155,21 @@
         //rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
         int pwmMax = 1800;
         int pwmMin = 1200;
-        out1 = map((int)(channels[1]*1000.0f),-1000,1000,pwmMin,pwmMax);
+        out1 = map((int)(rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax);
         if(out1<pwmMin) {
             out1 = pwmMin;
         };
         if(out1>pwmMax) {
             out1 = pwmMax;
         };
-        out2 = map((int)(channels[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
+        out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
         if(out2<pwmMin) {
             out2 = pwmMin;
         };
         if(out2>pwmMax) {
             out2 = pwmMax;
         };
-        //test_control.setSetPoint(channels[1]);
+        //test_control.setSetPoint(rc[1]);
         test_control.setSetPoint(0.0); // 仮（実際にはsbusの読み込みなど）
         pc.printf("roll%f ", roll*180.0/pi);
         //pc.printf("out1:%d\n ", out1);
@@ -181,7 +179,7 @@
         MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
         pc.printf("time%f \r\n", (tend-tstart));
         test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう
-        out1 = test_control.compute();
+        //out1 = test_control.compute();
         pc.printf("out1:%d ",out1);
         servoRight.pulsewidth_us(out1); 
     }