請問有關ARDUINO四軸PID的問題(程式碼)

隨風大俠

我最近在做四軸，想請問大家

我參照論壇上找到的互補濾波和PID演算自行修改，但是都無法平衡!!

互補濾波：

angleA= atan2(compass.a.y , compass.a.z) * 180 / 3.14159;
   omega= 0.068 * (gyro.g.x );
   
   angleA2= atan2(compass.a.x , compass.a.z) * 180 / 3.14159;
   omega= 0.068 * (gyro.g.y );
   
   unsigned long now = millis();                           // 当前时间(ms)
   float dt = (now - preTime)/* / 1000.0*/;                    // 微分时间(ms)
   preTime = now;  
   float K = 0.8;                    
   float A = K / (K + dt);                    
   f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;
   f_angle2=A * (f_angle2 + omega2 * dt) + (1-A) * angleA2;

PID:

    now = millis(); // 當前時間(ms)
     float TimeCh = (now - lastTime) / 1000.0; // 採樣時間(s) 這裡怪怪的，採樣是S，因為已經是
                                             millis()，應該是要乘1000吧??
      // 比例係數、積分係數、微分係數
    //float SampleTime = 0.1; // 採樣時間(s)
     float Setpoint = 0; // 設定目標值(degree)
     float outMin = -42.5, outMax = 42.5; // 輸出上限、輸出下限
     
   
   // if(TimeCh >= SampleTime) // 到達預定採樣時間時
     //{
        float Input = f_angle, Input2 = f_angle2; // 輸入賦值
         float error = Setpoint - Input, error2 = Setpoint - Input2; // 偏差值
         ITerm+= (error * TimeCh); // 計算積分項
         ITerm = constrain(ITerm, outMin, outMax); // 限定值域
         float DTerm = (error - lastError) / TimeCh; // 計算微分項
         Output = Kp * error + Ki * ITerm + Kd *  DTerm; // 計算輸出值
         Output = constrain(Output, outMin, outMax); // 限定值域
         
      
          // 控制左驅
       //if(lastError>Input)  
         B = 117.5 - Output;   
         B = B+Bin;     
         B = constrain(B, 95, 180);
         myservo2.write(B);  
               
         // 控制右驅
      D = 119.5 + Output;
         D = D+Din;
         D = constrain(D, 95, 180);
         myservo4.write(D);

馬達控制是使用Servo.h函式，能夠反應的範圍是95~180，但是我看很多人好像不用Servo.h函式控制電調，而是用analogWrite，請問這兩者有差別嗎??

謝謝

学慧放弃

我也是前者，后者我反正不会用，我做六轴呢，算法还在研究中