Diff: main.cpp

Revision:

28:fc6c46c1db65

Parent:

27:43bd0e444633

Child:

29:34ee662f454e

--- a/main.cpp	Tue Feb 16 02:56:16 2021 +0000
+++ b/main.cpp	Tue Feb 16 03:24:20 2021 +0000
@@ -42,9 +42,14 @@
 double acc_x,acc_y,acc_z;
 double gyro_x,gyro_y,gyro_z;
 int out1, out2;
-float scaled_servo[3];
-int servo_out[3] = {0,0,0};
-int motor_out[1] = {0};
+float scaledServoOut[3]= {0,0,0};
+float scaledMotorOut[1]= {-1};
+int servoOut[3] = {1500,1500,1500};
+int motorOut[1] = {1000};
+int servoPwmMax = 1800;
+int servoPwmMin = 1200;
+int motorPwmMax = 1800;
+int motorPwmMin = 1200;
 int offsetVal[6] = {0,0,0,0,0,0};
 
 const double pitch_align = 0.0;
@@ -64,12 +69,49 @@
 
 // 割り込まれた時点での出力(computeの結果）を返す関数
 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(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
         }
+    }
+    
+    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 dT = rc[2];
+    
+    scaledServoOut[0]=de+da;
+    scaledServoOut[1]=de-da;
+    scaledMotorOut[0]= dT;
+    for(int i = 0;i<sizeof(servoOut);i++){
+        servoOut[i] = int(map(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))
+        if(servoOut[i]<servoPwmMin) {
+            servoOut[i] = servoPwmMin;
+        };
+        if(servoOut[i]>servoPwmMax) {
+            servoOut[i] = servoPwmMax;
+        };
+    }
+    
+    for(int i = 0;i<sizeof(motorOut);i++){
+        motorOut[i] = int(map(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))
+        if(motorOut[i]<motorPwmMin) {
+            motorOut[i] = motorPwmMin;
+        };
+        if(motorOut[i]>motorPwmMax) {
+            motorOut[i] = motorPwmMax;
+        };
+    }
+    
+    servoRight.pulsewidth_us(servoOut[0]);
+    //servoLeft.pulsewidth_us(servoOut[1]); 
+        
     if(loop_count > 10)
         pushto_sdcard();
         loop_count = 0;
@@ -97,7 +139,7 @@
 {
     LoopTicker sdcard_loop;
     LoopTicker PIDtick;
-    PIDtick.attach(PID_compute,PID_dt);
+    PIDtick.attach(calcServoOut,PID_dt);
     pc.baud(115200);
     sd.baud(115200);
     accelgyro.initialize();
@@ -107,53 +149,35 @@
     accelgyro.setFullScaleGyroRange(GYRO_FSR);
     //MPU6050のLPFを設定
     accelgyro.setDLPFMode(MPU6050_LPF);
-
+    
+    pitchPID.setSetPoint(0.0);
+    pitchratePID.setSetPoint(0.0); 
+    rollPID.setSetPoint(0.0); 
+    rollratePID.setSetPoint(0.0); 
+    pitchPID.setBias(0.0);
+    pitchratePID.setBias(0.0); 
+    rollPID.setBias(0.0); 
+    rollratePID.setBias(0.0); 
+    pitchPID.setOutputLimits(-1.0,1.0);
+    pitchratePID.setOutputLimits(-1.0,1.0);
+    rollPID.setOutputLimits(-1.0,1.0); 
+    rollratePID.setOutputLimits(-pi,pi);
+    pitchPID.setInputLimits(-pi,pi);
+    pitchratePID.setInputLimits(-pi,pi);
+    rollPID.setInputLimits(-pi,pi); 
+    rollratePID.setInputLimits(-pi,pi);
     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;
+        PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
         
-        //float LP_rc3 = 0.15f;
-        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;
-        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("roll%lf ", gyro_x);
-        //pc.printf("out1:%d\n ", out1);
-        //pc.printf("out2:%d ", out2);
-        wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節（割り込みにするべき）
         float tend = t.read();
-        MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
         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 ++){
-            servo_out[i] = max(servo_out[i],pwmMin);
-            servo_out[i] = min(servo_out[i], pwmMax);
-        }
-        pc.printf("out1:%d ",servo_out[0]);
-        servoRight.pulsewidth_us(servo_out[0]); 
+        MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz
+        
     }
 }
\ No newline at end of file