Diff: main.cpp

Revision:

25:5aca9b224226

Parent:

24:6ab61527855d

Child:

26:62d9857eaecc

--- a/main.cpp	Mon Feb 15 07:14:44 2021 +0000
+++ b/main.cpp	Mon Feb 15 08:30:16 2021 +0000
@@ -38,7 +38,7 @@
 Madgwick MadgwickFilter;
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
-// Serial sd(PE_8,PE_7);
+Serial sd(PE_8,PE_7);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 PwmOut servoRight(PE_9);
@@ -60,6 +60,7 @@
 int ch1, ch2;
 float rc[16];
 //float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ
+float roll_ac;
 double pitch = 0.0;
 double roll = 0.0;
 double yaw = 0.0;
@@ -68,7 +69,7 @@
 double acc_x,acc_y,acc_z;
 double gyro_x,gyro_y,gyro_z;
 int out1, out2;
-int scaler_servo[3];
+float scaled_servo[3];
 int servo_out[3];
 
 const double pitch_align = 0.0;
@@ -80,23 +81,22 @@
 }
 void pushto_sdcard()
 {
-    // sd.printf("tstart: %f \n",tstart);
+    sd.printf("tstart: %f \n",tstart);
 }
 // 割り込まれた時点での出力(computeの結果）を返す関数
-/*
-float PID_compute(){
-    
+
+void PID_compute(){
+    scaled_servo[0] = test_control.compute();
 }
-*/
 
 int main()
 {
-    //LoopTicker sdcard_loop;
+    LoopTicker sdcard_loop;
     LoopTicker PIDtick;
-    //PIDtick.attach(test_control,PID_dt);
-    //sdcard_loop.attach(pushto_sdcard,0.01);
+    PIDtick.attach(PID_compute,PID_dt);
+    sdcard_loop.attach(pushto_sdcard,1.0);
     pc.baud(115200);
-    //sd.baud(115200);
+    sd.baud(115200);
     accelgyro.initialize();
     uint8_t offsetVal[6] = {0,0,0,0,0,0};
     accelgyro.setXAccelOffset(offsetVal[0]);
@@ -117,22 +117,23 @@
     MadgwickFilter.begin(100); //サンプリング周波数Hza
     // のセットアップ
     test_control.setInputLimits(-60.0,60.0); // 60°が限界
-    test_control.setOutputLimits(1200, 1800);
-    test_control.setBias((1200+1800)/2); // out range の真ん中に設定する（out rangeの中央が安定点のため）
+    test_control.setOutputLimits(-1, 1);
+    test_control.setBias(0.0);
     t.start();
     float LP_rc = 0.65f;
     while(1) {
         tstart = t.read();
-        //sdcard_loop.loop();
+        sdcard_loop.loop();
         if(sbus.failSafe == false) {
             // sbusデータの読み込み
             for (int i =0 ; i < 16;i ++){
-                int data;
-                float mapped_data;
                 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",rc[1]);
+            float rc_setpoint = map((int)(rc[1]*1000.0f),-1000,1000,-30,30); // sbus用
+            pc.printf("data: %f",rc_setpoint);
+            test_control.setSetPoint(rc_setpoint);
         }
+        else test_control.setSetPoint(0.0);
 
         // gx gy gz ax ay az
         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
@@ -153,12 +154,9 @@
         
         //float LP_rc3 = 0.15f;
         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)（de _left*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
+        //out1 = map((int)（rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
         if(out1<pwmMin) {
             out1 = pwmMin;
         };
@@ -172,8 +170,7 @@
         if(out2>pwmMax) {
             out2 = pwmMax;
         };
-        //test_control.setSetPoint(rc[1]);
-        test_control.setSetPoint(0.0); // 仮（実際にはsbusの読み込みなど）
+        //test_control.setSetPoint(0.0); // 仮（実際にはsbusの読み込みなど）
         pc.printf("roll%f ", roll*180.0/pi);
         //pc.printf("out1:%d\n ", out1);
         //pc.printf("out2:%d ", out2);
@@ -182,28 +179,14 @@
         MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
         pc.printf("time%f \r\n", (tend-tstart));
         test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう
-        scaled_servo[0] = test_control.compute(); // -1 ~ 1
-        servo_out[0] = map((int)(scaler_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
+        PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
+        printf("scaled_servo[0]: %f",scaled_servo[0]);
+        servo_out[0] = map((int)(scaled_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] = max(servo_out[i],pwmMin);
             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]); 
+        pc.printf("out1:%d ",servo_out[0]);
+        servoRight.pulsewidth_us(servo_out[0]); 
     }
 }
\ No newline at end of file