Diff: main.cpp

Revision:

36:e68ce293306e

Parent:

35:4535af88f7bf

Child:

37:dad55cf05e3d

--- a/main.cpp	Tue Mar 02 05:01:20 2021 +0000
+++ b/main.cpp	Wed Mar 03 03:34:11 2021 +0000
@@ -1,11 +1,14 @@
 #include "mbed.h"
 #include "PIDcontroller.h"
 #include "SBUS.hpp"
-#include <MadgwickAHRS.h>
+//#include <MadgwickAHRS.h>
 #include "LoopTicker.hpp"
 #include "MPU6050.h"
 #include <I2Cdev.h>
-
+#include "FastPWM.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
+#include <math.h>
 
 #define MPU6050_PWR_MGMT_1   0x6B
 #define MPU_ADDRESS  0x68
@@ -17,24 +20,30 @@
 #define MPU6050_LPF MPU6050_DLPF_BW_256
 
 MPU6050 accelgyro;
-Madgwick MadgwickFilter;
+//Madgwick MadgwickFilter;
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
 Serial sd(PG_14,PG_9);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 
-PwmOut servoRight(PE_9);
-PwmOut servoLeft(PE_11);
-PwmOut servoThrust(PA_0);
-const double PID_dt = 1.0/200;
-PID pitchPID(3.0, 0,0,PID_dt); //rad
+FastPWM servoRight(PE_9);
+FastPWM servoLeft(PE_11);
+FastPWM servoThrust(PA_0);
+const double PID_dt = 0.015;
+PID pitchPID(5.0, 0,0,PID_dt); //rad
 PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
-PID rollPID(3.0,0.0,0.0,PID_dt);
+PID rollPID(5.0,0.0,0.0,PID_dt);
 PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
 Timer t;
 
-int loop_count = 0;
+Matrix qhat(4,1);
+Matrix Phat(4,4);
+Matrix Racc(3,3);
+
+
+int loop_count = 1;
+float att_dt = 0.01;
 float rc[16];
 float roll_ac;
 double pitch = 0.0;
@@ -45,15 +54,15 @@
 double acc_x,acc_y,acc_z;
 double gyro_x,gyro_y,gyro_z;
 int out1, out2;
-float scaledServoOut[3]= {0,0,0};
+float scaledServoOut[3]= {0,0};
 float scaledMotorOut[1]= {-1};
-float servoOut[3] = {0.0015,0.0015,0.0015};
-float motorOut[1] = {0.00095};
-float servoPwmMax = 0.002;
-float servoPwmMin = 0.0012;
-float motorPwmMax = 0.002;
-float motorPwmMin = 0.00095;
-int offsetVal[6] = {0,0,0,0,0,0};
+float servoOut[3] = {1500.0,1500.0};
+float motorOut[1] = {1100.0};
+float servoPwmMax = 1800.0;
+float servoPwmMin = 1200.0;
+float motorPwmMax = 2000.0;
+float motorPwmMin = 1100.0;
+int offsetVal[6] = {0,0,0,-600,570,265};
 
 const double pitch_align = 0.0;
 const double roll_align = -0.0;
@@ -66,7 +75,7 @@
 
 void writeSdcard()
 {
-    pc.printf("motor: %f, servo1: %f, servo2: %f \r\n",motorOut[0],servoOut[0],servoOut[1]);
+    pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f);
 }
 
 // 割り込まれた時点での出力(computeの結果）を返す関数
@@ -78,22 +87,25 @@
         }
     }
     
-    pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
-    roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
+    
+    //pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
+    //roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
     pitchPID.setProcessValue(pitch);
     pitchratePID.setProcessValue(gyro_y);
     rollPID.setProcessValue(roll);
     rollratePID.setProcessValue(gyro_x);
-    float de = pitchPID.compute()+pitchratePID.compute()+rc[1];
-    float da = rollPID.compute()+rollratePID.compute()+rc[0];
+    float de = -pitchPID.compute()-pitchratePID.compute()-rc[1]+rc[0];
+    float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1];
     float dT = rc[2];
     
     scaledServoOut[0]=de+da;
-    scaledServoOut[1]=de-da;
+    scaledServoOut[1]=-de+da;
     scaledMotorOut[0]= dT;
     
-    for(int i = 0;i<4;i++){
-        servoOut[i] = float(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax));
+    double LP_servo = 0.2;
+    double LP_motor = 0.2;
+    for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
+        servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
         if(servoOut[i]<servoPwmMin) {
             servoOut[i] = servoPwmMin;
         };
@@ -102,8 +114,8 @@
         };
     }
     
-    for(int i = 0;i<1;i++){
-        motorOut[i] = float(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax));
+    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
+        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
         if(motorOut[i]<motorPwmMin) {
             motorOut[i] = motorPwmMin;
         };
@@ -111,14 +123,13 @@
             motorOut[i] = motorPwmMax;
         };
     }
-    
-    servoRight.pulsewidth(servoOut[0]);
-    servoLeft.pulsewidth(servoOut[1]); 
-    servoThrust.pulsewidth(motorOut[0]);
+    servoRight.pulsewidth_us(servoOut[0]);
+    servoLeft.pulsewidth_us(servoOut[1]); 
+    servoThrust.pulsewidth_us(motorOut[0]);
      
-    if(loop_count > int(1/PID_dt/10.0)){
+    if(loop_count > 10){
         writeSdcard();
-        loop_count = 0;
+        loop_count = 1;
     }else{
         loop_count +=1;
     }
@@ -136,27 +147,53 @@
         gyro_y = gy / GYRO_SSF;
         gyro_z = gz / GYRO_SSF;
         //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
-        MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
+        //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
 }
 
 void updateBetweenMeasures(){
-    
+        // gx gy gz ax ay az
+        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        ax = ax - offsetVal[0];
+        ay = ay - offsetVal[1];
+        az = az - offsetVal[2];
+        gx = gx - offsetVal[3];
+        gy = gy - offsetVal[4];
+        gz = gz - offsetVal[5];
+        // 加速度値を分解能で割って加速度(G)に変換する
+        acc_x = ax / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
+        acc_y = ay / ACCEL_SSF;
+        acc_z = az / ACCEL_SSF;
+        // 角速度値を分解能で割って角速度(rad per sec)に変換する
+        gyro_x = float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
+        gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
+        gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
+        
+
+        Matrix phi(4,4);
+        phi <<  1.0               << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
+            <<  gyro_x*0.5*att_dt << 1.0                << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
+            <<  gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0               << gyro_x*0.5*att_dt
+            <<  gyro_z*0.5*att_dt <<  gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
+        qhat = phi*qhat;
+        
+        float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+        qhat *= (1.0f/ qnorm);
+        
+}
+
+void computeAngles()
+{
+    float q0 = qhat.getNumber( 1, 1 );
+    float q1 = qhat.getNumber( 2, 1 ); 
+    float q2 = qhat.getNumber( 3, 1 ); 
+    float q3 = qhat.getNumber( 4, 1 ); 
+    roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
+    pitch = asinf(-2.0f * (q1*q3 - q0*q2));
+    yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
 }
 
 int main()
-{
-    LoopTicker PIDtick;
-    PIDtick.attach(calcServoOut,PID_dt);
-    //pc.baud(115200);
-    //sd.baud(115200);
-    accelgyro.initialize();
-    //加速度計のフルスケールレンジを設定
-    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
-    //■角速度計のフルスケールレンジを設定
-    accelgyro.setFullScaleGyroRange(GYRO_FSR);
-    //MPU6050のLPFを設定
-    accelgyro.setDLPFMode(MPU6050_LPF);
-    
+{    
     pitchPID.setSetPoint(0.0);
     pitchratePID.setSetPoint(0.0); 
     rollPID.setSetPoint(0.0); 
@@ -174,19 +211,50 @@
     rollPID.setInputLimits(-pi,pi); 
     rollratePID.setInputLimits(-pi,pi);
     
-    servoRight.period(0.02f);
-    servoLeft.period(0.02f);
-    servoThrust.period(0.02f);
-    MadgwickFilter.begin(100); //サンプリング周波数Hza
+    servoRight.period_us(15000.0);
+    servoLeft.period_us(15000.0);
+    servoThrust.period_us(15000.0);
+    servoRight.pulsewidth_us(1500.0);
+    servoLeft.pulsewidth_us(1500.0); 
+    servoThrust.pulsewidth_us(1100.0);
+    
+    qhat << 1 << 0 << 0 << 0;
+    
+    Phat << 0.0001 << 0 <<0 <<0
+         << 0 << 0.0001 <<0 <<0
+         << 0 << 0 <<0.0001 <<0
+         << 0 << 0 << 0 << 0.0001;
+         
+    Racc << 0.0001 << 0 <<0
+         << 0 << 0.0001 <<0
+         << 0 << 0 <<0.0001;
+    
+    
+    LoopTicker PIDtick;
+    PIDtick.attach(calcServoOut,PID_dt);
+    //pc.baud(115200);
+    //sd.baud(115200);
+    accelgyro.initialize();
+    //加速度計のフルスケールレンジを設定
+    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
+    //■角速度計のフルスケールレンジを設定
+    accelgyro.setFullScaleGyroRange(GYRO_FSR);
+    //MPU6050のLPFを設定
+    accelgyro.setDLPFMode(MPU6050_LPF);
+    //MadgwickFilter.begin(100); //サンプリング周波数Hza
     t.start();
+    //servoRight.write(0.0f);
     while(1) {
         float tstart = t.read();
         //姿勢角を更新
-        updateAttitude();
+        //updateAttitude();
+        updateBetweenMeasures();
+        computeAngles();
         PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
         
         float tend = t.read();
-        MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz
+        att_dt = (tend-tstart);
+        //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz
         
     }
 }
\ No newline at end of file