Diff: main.cpp

Revision:

24:6ab61527855d

Parent:

23:4a490fa8bf4a

Child:

25:5aca9b224226

--- a/main.cpp	Mon Feb 15 06:57:00 2021 +0000
+++ b/main.cpp	Mon Feb 15 07:14:44 2021 +0000
@@ -68,6 +68,8 @@
 double acc_x,acc_y,acc_z;
 double gyro_x,gyro_y,gyro_z;
 int out1, out2;
+int scaler_servo[3];
+int servo_out[3];
 
 const double pitch_align = 0.0;
 const double roll_align = -0.0;
@@ -91,7 +93,7 @@
 {
     //LoopTicker sdcard_loop;
     LoopTicker PIDtick;
-    //PIDtick.attach(test_control,RATE);
+    //PIDtick.attach(test_control,PID_dt);
     //sdcard_loop.attach(pushto_sdcard,0.01);
     pc.baud(115200);
     //sd.baud(115200);
@@ -115,7 +117,8 @@
     MadgwickFilter.begin(100); //サンプリング周波数Hza
     // のセットアップ
     test_control.setInputLimits(-60.0,60.0); // 60°が限界
-    test_control.setOutputLimits(-1,1);
+    test_control.setOutputLimits(1200, 1800);
+    test_control.setBias((1200+1800)/2); // out range の真ん中に設定する（out rangeの中央が安定点のため）
     t.start();
     float LP_rc = 0.65f;
     while(1) {
@@ -149,13 +152,13 @@
         roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
         
         //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;
+        roll_ac = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac;
         //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)(rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax);
+        out1 = map((int)（de _left*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
         if(out1<pwmMin) {
             out1 = pwmMin;
         };
@@ -179,8 +182,28 @@
         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("out1:%d ",out1);
-        servoRight.pulsewidth_us(out1); 
+        scaled_servo[0] = test_control.compute(); // -1 ~ 1
+        servo_out[0] = map((int)(scaler_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
+        for (int i = 0; i < 3 ; i ++){
+            servo_out[i] = max(servo_out[i],pwemMin);
+            servo_out[i] = min(servo_out[i], pwmMax);
+        }
+        /*
+        if(out1<pwmMin) {
+            out1 = pwmMin;
+        };
+        if(out1>pwmMax) {
+            out1 = pwmMax;
+        };
+        out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
+        if(out2<pwmMin) {
+            out2 = pwmMin;
+        };
+        if(out2>pwmMax) {
+            out2 = pwmMax;
+        };
+        */
+        pc.printf("out1:%d ",servo_out[i]);
+        servoRight.pulsewidth_us(servo_out[i]); 
     }
 }
\ No newline at end of file