Diff: main.cpp

Revision:

26:62d9857eaecc

Parent:

25:5aca9b224226

Child:

27:43bd0e444633

--- a/main.cpp	Mon Feb 15 08:30:16 2021 +0000
+++ b/main.cpp	Mon Feb 15 09:28:48 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(PG_14,PG_9);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 PwmOut servoRight(PE_9);
@@ -81,12 +81,19 @@
 }
 void pushto_sdcard()
 {
-    sd.printf("tstart: %f \n",tstart);
+    //sd.printf("tstart: %f \n",tstart);
+    sd.printf("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll);
 }
 // 割り込まれた時点での出力(computeの結果）を返す関数
-
+int loop_count = 0;
 void PID_compute(){
     scaled_servo[0] = test_control.compute();
+    loop_count +=1;
+    if(loop_count == 10)    {
+        pc.printf("log:%d",loop_count);
+        pushto_sdcard();
+        loop_count = 0;
+    }
 }
 
 int main()
@@ -123,7 +130,6 @@
     float LP_rc = 0.65f;
     while(1) {
         tstart = t.read();
-        sdcard_loop.loop();
         if(sbus.failSafe == false) {
             // sbusデータの読み込み
             for (int i =0 ; i < 16;i ++){
@@ -170,8 +176,7 @@
         if(out2>pwmMax) {
             out2 = pwmMax;
         };
-        //test_control.setSetPoint(0.0); // 仮（実際にはsbusの読み込みなど）
-        pc.printf("roll%f ", roll*180.0/pi);
+        pc.printf("roll%lf ", gyro_x);
         //pc.printf("out1:%d\n ", out1);
         //pc.printf("out2:%d ", out2);
         wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節（割り込みにするべき）
@@ -180,6 +185,7 @@
         pc.printf("time%f \r\n", (tend-tstart));
         test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう
         PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
+        //sdcard_loop.loop();
         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 ++){