Diff: main.cpp

Revision:

20:2c3f113a8e8f

Parent:

19:61d7e9b4b6c2

Child:

21:df4e4e857a3e

--- a/main.cpp	Fri Feb 05 04:58:07 2021 +0000
+++ b/main.cpp	Tue Feb 09 03:31:18 2021 +0000
@@ -1,14 +1,15 @@
 #include "mbed.h"
+#include "PIDcontroller.h"
 #include "SBUS.hpp"
 #include <MadgwickAHRS.h>
 #include "LoopTicker.hpp"
 #include "MPU6050.h"
 #include <I2Cdev.h>
-//#include <Servo.h>
 
 #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
@@ -29,23 +30,28 @@
 //#define GYRO_SSF 16.4
 #define MPU6050_LPF MPU6050_DLPF_BW_256
 //#define MPU6050_LPF MPU6050_DLPF_BW_188
-//#define MPU6050_LPF MPU6050_DLPF_BW_98 
-//#define MPU6050_LPF MPU6050_DLPF_BW_42  
-//#define MPU6050_LPF MPU6050_DLPF_BW_20 
-//#define MPU6050_LPF MPU6050_DLPF_BW_10   
-//#define MPU6050_LPF MPU6050_DLPF_BW_5  
+//#define MPU6050_LPF MPU6050_DLPF_BW_98
+//#define MPU6050_LPF MPU6050_DLPF_BW_42
+//#define MPU6050_LPF MPU6050_DLPF_BW_20
+//#define MPU6050_LPF MPU6050_DLPF_BW_10
+//#define MPU6050_LPF MPU6050_DLPF_BW_5
 MPU6050 accelgyro;
 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);
-//Servo servoRight(PB_4); //右のサーボ
+//PwmOut co(PE_9); // サーボのoutput
+//Servo servoRight(PE_); //右のサーボ
 //Servo servoLeft(PB_5); //左のサーボ
 //Servo thrServo(PB_6); //スロットルサーボ
 
+// のセットアップ
+// (float Kp, float Ki, float Kd, float tSample);
+const double PID_dt = 0.01;
+PID test_control(0.1, 0.1, 0.1, PID_dt);
 Timer t;
 
 int ch1, ch2;
@@ -61,17 +67,22 @@
 
 const double pitch_align = 0.0;
 const double roll_align = -0.0;
-
+float tstart;
 long map(long x, long in_min, long in_max, long out_min, long out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
+void pushto_sdcard()
+{
+    // sd.printf("tstart: %f \n",tstart);
+}
 
 int main()
 {
-    int count = 0;
+    //LoopTicker sdcard_loop;
+    //sdcard_loop.attach(pushto_sdcard,0.01);
     pc.baud(115200);
-    sd.baud(9600);
+    //sd.baud(115200);
     accelgyro.initialize();
     uint8_t offsetVal[6] = {0,0,0,0,0,0};
     accelgyro.setXAccelOffset(offsetVal[0]);
@@ -86,40 +97,26 @@
     accelgyro.setFullScaleGyroRange(GYRO_FSR);
     //MPU6050のLPFを設定
     accelgyro.setDLPFMode(MPU6050_LPF);
-    
+
     servoRight.period_us(20000);
-    
+
     MadgwickFilter.begin(100); //サンプリング周波数Hza
+    // のセットアップ
+    test_control.setInputLimits(-60.0,60.0); // 60°が限界
+    test_control.setOutputLimits(1200, 1800);
+    test_control.setSetPoint(0); // 仮（実際にはsbusの読み込みなど）
     t.start();
     while(1) {
-        if(count == 50){
-            sd.printf("test");
-            count = 0;  
-        }
-        count ++;
-        float tstart = t.read();
-        if(sbus.failSafe == false)
-        {
+        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("ch1 :%d ", ch1);
+
             //pc.printf("ch2 :%d ", ch2);
-            float LP_rc = 0.65f;
-            float LP_rc3 = 0.15f;
-            rc1 = LP_rc*float(map(ch1,368,1680,-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;
-            //pc.printf("rc1 :%f ", rc1);
-            //pc.printf("rc2 :%f ", rc1);
-            int pwmMax = 1800;
-            int pwmMin = 1200;
-            out1 = map((int)(rc1*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);
-            if(out2<pwmMin){out2 = pwmMin;};
-            if(out2>pwmMax){out2 = pwmMax;};
         }
-        
+
         // gx gy gz ax ay az
         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
         // 加速度値を分解能で割って加速度(G)に変換する
@@ -130,24 +127,46 @@
         gyro_x = gx / GYRO_SSF;  // (deg/s)
         gyro_y = gy / GYRO_SSF;
         gyro_z = gz / GYRO_SSF;
-      //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
+        //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
         //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z);
         MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
         
         pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
         roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
-
-        servoRight.pulsewidth_us(out1);
+        /*
+        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;
+        int pwmMax = 1800;
+        int pwmMin = 1200;
+        out1 = map((int)(rc1*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);
+        if(out2<pwmMin) {
+            out2 = pwmMin;
+        };
+        if(out2>pwmMax) {
+            out2 = pwmMax;
+        };
+        */
 
-        pc.printf("out1:%d ", out1);
-        //pc.printf("roll%f ", roll*180.0/pi);
+        pc.printf("roll%f ", roll*180.0/pi);
         //pc.printf("out1:%d ", 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();
-        wait(0.005);
         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);
+        servoRight.pulsewidth_us(out1); 
+    }
 }
\ No newline at end of file