第6堂 PID演算法

硬體組裝

接線

舵機pin8

超音波trig 3, echo 4

PID參考程式

#include <Servo.h>
Servo servo8;
#include <HCSR04.h>
HCSR04 hc(3,4);

int angle;
float time, distance;
float setPoint=11; //中心距離
float error, previous_error;
float kp=10; //10
float ki=0.05; //0.05
float kd=200; //200
int dt=50; 
float P,I,D,PID;


void setup() {
  Serial.begin(9600);
  servo8.attach(8);
  time = millis();
}


void loop() {
  if (millis() > time + dt){
    time = millis();
    distance = hc.dist();
    error = distance-setPoint;
    
    P = kp*error;  //**********************************P
    
    if(-4 < error && error < 4){
      I=I+ki*error;  //********************************I
    }else{
    I = 0;
    }

    D = kd*((error - previous_error)/dt); //***********D

    PID=P + I + D ; //*********************************PID

    if (PID>200){
      PID=200;
    }
    if (PID<-200){
      PID=-200;
    }
    
    previous_error=error; //下一個迴圈用，一定要寫在下面這邊

  Serial.print("Distance: "); Serial.print(distance);
  Serial.print("Error: "); Serial.print(error); 
  Serial.print(" P: "); Serial.print(P);
  Serial.print(" D: "); Serial.print(D);
  Serial.print(" I: "); Serial.print(I);
  Serial.print(" PID: "); Serial.println(PID);

  angle = map(PID, -200,200,120,90);
  servo8.write(angle);
  
  }
}

舵機固定在90度代碼

#include <Servo.h> 
Servo myservo8;

void setup() {
  myservo8.attach(8); 
}

void loop() {
  myservo8.write(90);
}