Diff: main.cpp

Revision:

22:da9893b71f24

Parent:

21:df4e4e857a3e

Child:

23:4a490fa8bf4a

--- a/main.cpp	Tue Feb 09 09:03:00 2021 +0000
+++ b/main.cpp	Mon Feb 15 06:49:37 2021 +0000
@@ -9,7 +9,6 @@
 #define MPU6050_PWR_MGMT_1   0x6B
 #define MPU_ADDRESS  0x68
 #define pi 3.141562
-#define RATE 0.1 // for 
 
 //加速度 Full-Scale Range
 //#define ACCEL_FSR MPU6050_ACCEL_FS_2
@@ -52,10 +51,15 @@
 // (float Kp, float Ki, float Kd, float tSample);
 const double PID_dt = 0.01;
 PID test_control(1.2, 0.0, 0.0, PID_dt);
+PID pitchPID(1.0, 0,0,PID_dt); //rad
+PID pitchratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
+PID rollPID(1.0,0.0,0.0,PID_dt);
+PID rollratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
 Timer t;
 
 int ch1, ch2;
-float rc1, rc2;
+int channels[16];
+//float rc1, rc2; // channels[16]の1,2要素めにそれぞれ移行ずみ
 double pitch = 0.0;
 double roll = 0.0;
 double yaw = 0.0;
@@ -76,10 +80,18 @@
 {
     // sd.printf("tstart: %f \n",tstart);
 }
+// 割り込まれた時点での出力(computeの結果）を返す関数
+/*
+float PID_compute(){
+    
+}
+*/
 
 int main()
 {
     //LoopTicker sdcard_loop;
+    LoopTicker PIDtick;
+    //PIDtick.attach(test_control,RATE);
     //sdcard_loop.attach(pushto_sdcard,0.01);
     pc.baud(115200);
     //sd.baud(115200);
@@ -103,19 +115,22 @@
     MadgwickFilter.begin(100); //サンプリング周波数Hza
     // のセットアップ
     test_control.setInputLimits(-60.0,60.0); // 60°が限界
-    test_control.setOutputLimits(1200, 1800);
-    test_control.setSetPoint(0.0); // 仮（実際にはsbusの読み込みなど）
-    test_control.setBias((1200+1800)/2); // out range の真ん中に設定する（out rangeの中央が安定点のため）
+    test_control.setOutputLimits(-1,1);
     t.start();
+    float LP_rc = 0.65f;
     while(1) {
         tstart = t.read();
         //sdcard_loop.loop();
         if(sbus.failSafe == false) {
-            ch1 = int(sbus.getData(1));
-            ch2 = int(sbus.getData(2));
-            pc.printf("ch1 :%d ", ch1);
-
-            //pc.printf("ch2 :%d ", ch2);
+            // sbusデータの読み込み
+            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したものを格納
+            }
+            pc.printf("data: %d",sbus.getData(1));
         }
 
         // gx gy gz ax ay az
@@ -134,40 +149,40 @@
         
         pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
         roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
-        /*
-        float LP_rc = 0.65f;
+        
         //float LP_rc3 = 0.15f;
-        rc1 = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
-        rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
+        //rc1 = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
+        //rc1 = LP_rc * float(map(ch1,368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc1; // mapped input
+        
+        //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)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax);
+        out1 = map((int)(channels[1]*1000.0f),-1000,1000,pwmMin,pwmMax);
         if(out1<pwmMin) {
             out1 = pwmMin;
         };
         if(out1>pwmMax) {
             out1 = pwmMax;
         };
-        out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
+        out2 = map((int)(channels[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(0.0); // 仮（実際にはsbusの読み込みなど）
         pc.printf("roll%f ", roll*180.0/pi);
-        //pc.printf("out1:%d ", out1);
+        //pc.printf("out1:%d\n ", out1);
         //pc.printf("out2:%d ", out2);
-        // pc.printf("%f",PID_dt - t.read() + tstart);
         wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節（割り込みにするべき）
         float tend = t.read();
         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();
-        pc.printf("co:%d ",out1);
+        pc.printf("out1:%d ",out1);
         servoRight.pulsewidth_us(out1); 
     }
 }
\ No newline at end of file