Diff: main.cpp

Revision:

27:43bd0e444633

Parent:

26:62d9857eaecc

Child:

28:fc6c46c1db65

--- a/main.cpp	Mon Feb 15 09:28:48 2021 +0000
+++ b/main.cpp	Tue Feb 16 02:56:16 2021 +0000
@@ -9,31 +9,12 @@
 #define MPU6050_PWR_MGMT_1   0x6B
 #define MPU_ADDRESS  0x68
 #define pi 3.141562
-
-//加速度 Full-Scale Range
-//#define ACCEL_FSR MPU6050_ACCEL_FS_2
-//#define ACCEL_FSR MPU6050_ACCEL_FS_4
 #define ACCEL_FSR MPU6050_ACCEL_FS_8
-//#define ACCEL_FSR MPU6050_ACCEL_FS_16
-//加速度 Sensitivity Scale Factor
 #define ACCEL_SSF 36500.0
-//角速度 Full-Scale Range
 #define GYRO_FSR MPU6050_GYRO_FS_250
-//#define GYRO_FSR MPU6050_GYRO_FS_500
-//#define GYRO_FSR MPU6050_GYRO_FS_1000
-//#define GYRO_FSR MPU6050_GYRO_FS_2000
-//角速度 Sensitivity Scale Factor
 #define GYRO_SSF 131.0
-//#define GYRO_SSF 65.5
-//#define GYRO_SSF 32.8
-//#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
+
 MPU6050 accelgyro;
 Madgwick MadgwickFilter;
 SBUS sbus(PD_5, PD_6);
@@ -41,25 +22,17 @@
 Serial sd(PG_14,PG_9);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
+
 PwmOut servoRight(PE_9);
-//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(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;
+int loop_count = 0;
 float rc[16];
-//float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ
 float roll_ac;
 double pitch = 0.0;
 double roll = 0.0;
@@ -70,77 +43,42 @@
 double gyro_x,gyro_y,gyro_z;
 int out1, out2;
 float scaled_servo[3];
-int servo_out[3];
+int servo_out[3] = {0,0,0};
+int motor_out[1] = {0};
+int offsetVal[6] = {0,0,0,0,0,0};
 
 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);
     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);
+void calcServoOut(){
+        if(sbus.failSafe == false) {
+            // sbusデータの読み込み
+            for (int i =0 ; i < 16;i ++){
+                rc[i] = 0.65f * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - 0.65f) * rc[i]; // mapped input
+            }
+        }
+    if(loop_count > 10)
         pushto_sdcard();
         loop_count = 0;
+    }else{
+        loop_count +=1;
     }
 }
 
-int main()
-{
-    LoopTicker sdcard_loop;
-    LoopTicker PIDtick;
-    PIDtick.attach(PID_compute,PID_dt);
-    sdcard_loop.attach(pushto_sdcard,1.0);
-    pc.baud(115200);
-    sd.baud(115200);
-    accelgyro.initialize();
-    uint8_t offsetVal[6] = {0,0,0,0,0,0};
-    accelgyro.setXAccelOffset(offsetVal[0]);
-    accelgyro.setYAccelOffset(offsetVal[1]);
-    accelgyro.setZAccelOffset(offsetVal[2]);
-    accelgyro.setXGyroOffset(offsetVal[3]);
-    accelgyro.setYGyroOffset(offsetVal[4]);
-    accelgyro.setZGyroOffset(offsetVal[5]);
-    //加速度計のフルスケールレンジを設定
-    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
-    //■角速度計のフルスケールレンジを設定
-    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(-1, 1);
-    test_control.setBias(0.0);
-    t.start();
-    float LP_rc = 0.65f;
-    while(1) {
-        tstart = t.read();
-        if(sbus.failSafe == false) {
-            // sbusデータの読み込み
-            for (int i =0 ; i < 16;i ++){
-                rc[i] = LP_rc * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc[i]; // mapped input
-            }
-            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);
-
+void updateAttitude(){
         // gx gy gz ax ay az
         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
         // 加速度値を分解能で割って加速度(G)に変換する
@@ -152,17 +90,40 @@
         gyro_y = gy / GYRO_SSF;
         gyro_z = gz / GYRO_SSF;
         //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);
+}
+
+int main()
+{
+    LoopTicker sdcard_loop;
+    LoopTicker PIDtick;
+    PIDtick.attach(PID_compute,PID_dt);
+    pc.baud(115200);
+    sd.baud(115200);
+    accelgyro.initialize();
+    //加速度計のフルスケールレンジを設定
+    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
+    //■角速度計のフルスケールレンジを設定
+    accelgyro.setFullScaleGyroRange(GYRO_FSR);
+    //MPU6050のLPFを設定
+    accelgyro.setDLPFMode(MPU6050_LPF);
+
+    servoRight.period_us(20000);
+
+    MadgwickFilter.begin(100); //サンプリング周波数Hza
+    t.start();
+    while(1) {
+        tstart = t.read();
+        //姿勢角を更新
+        updateAttitude()
         
         pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
         roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
         
         //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;
+        servoOut = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac;
         int pwmMax = 1800;
         int pwmMin = 1200;
-        //out1 = map((int)（rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
         if(out1<pwmMin) {
             out1 = pwmMin;
         };