PID제어로 공의 균형을 잡기 프로젝트(3. 만들어 보기)

2015-03-19 11:18:10

이번 글에서는 간단하게 공의 균형을 맞추는 기계를 만들어 보겠습니다.^____^

제가 생각하기에는 PID제어를 하는 프로젝트 중 막대 위에서 균형을 잡는 프로젝트가 제일 만들기 쉬워보이면서 재료도 간단히 드는 프로젝트였습니다.

사실상 선 위(1차원)에서 움직이면서 균형을 잡는것이 전부이기 때문에 쿼드 콥터처럼 주위 환경이나, 공간적으로 신경 쓰는것보다는 덜 한거 같습니다.

제가 만든 것은 당연히 아래의 영상보다는 약간 부실(?)하겠지만ㅜㅜ 기능만 제대로 구현되면 되니까요!

이번 프로젝트를 제작하기 위해 필요한 부품은 정말 간단합니다.

사실상 들어가는 부품이 아두이노, 서보모터 1개, 초음파 센서가 전부이기 때문에 한번쯤은 만들어봐도 좋을만한 프로젝트라고 생각합니다.

그럼 만들면서 필요한 부품을 알아보겠습니다.

부품 목록

No	부품 이름	수량	비고
1	오렌지 보드	1개	아두이노
2	초음파 센서	1개	HC-SR04
3	서보 모터(표준형)	1개
4	5v 어댑터	1개
5	브래드보드	1개
6	점퍼 케이블	10개

부품 이름	오렌지 보드	초음파 센서	서보모터(표준형)	5v 어댑터	브래드 보드	점퍼 케이블
부품 사진

만드는 방법

일단 규격은 아래와 같이 맞춰주어야 합니다.

너무 작거나 큰 사이즈가 아니기 때문에 쉽게 제작이 가능합니다.

저는 일단 단단한 나무 막대기나 플라스틱보다 가벼우면서 나름 질긴 우드락재질의 보드 종이를 사용했습니다.

제가 만들어본 모형입니다. 최대한 위의 규격에 맞춰서 제작하였습니다.

재료는 그냥 다른 프로젝트하다가 남은걸로 후다닥 공수해와서 사용했습니다;

초음파센서를 설치한 모습. 나사를 통해 고정시킨 모습도 볼 수 있습니다.

서보모터를 글루건으로 고정하고 나사로 두 막대를 고정하여 자유롭게 움직이게 하였습니다.

나사 뒤쪽은 너트로 꽉 조여주면 빠지지 않으면서 부드럽게 잘 돌아갑니다.

소스 코드


#include
#include

const int servoPin = 9;                          //서보의 핀번호

float Kp = 2.5;                //P게인 값
float Ki = 0;                  //I게인 값 
float Kd = 1;                  //D게인 값
double Setpoint, Input, Output, ServoOutput;                                       

float duration, distance;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);   //PID객체 생성

Servo myServo;                                     //서보 객체 생성, 초기화


int echoPin = 6;                 //초음파 핀번호 설정
int trigPin = 7;

void setup() {
  Serial.begin(9600);                  //시리얼 통신 초기화
  myServo.attach(servoPin);            //서보모터 핀번호 설정

  Input = readPosition();             //막대 위의 공의 위치를 측정값 함수 호출
  
  //초음파 센서 설정
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);                                                                  

  myPID.SetMode(AUTOMATIC);               //PID모드를 AUTOMATIC으로 설정
  myPID.SetOutputLimits(-80,80);          //PID의 값을 최소 -80부터 최대 80까지 설정
}

void loop() {
  Setpoint = 15;                         //막대 중앙 위치(Set Point를 15cm로 설정)
  Input = readPosition();                //공의 위치 측정                            

  myPID.Compute();                       //PID계산 

  ServoOutput=100+Output;                //서보모터의 각도 설정(100도는 서보모터가 수평을 이루었을 때 각도) 
  myServo.write(ServoOutput);            //서보모터에게 값 전달
}


float readPosition() {
  delay(40);                             //딜레이 설정 

  //초음파 센서 거리 측정 부분
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(2);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH); 
  distance = ((float)(340 * duration) / 10000) / 2;  


  if(distance > 30) {                   //공의 측정거리가 30cm 이상일 경우 최대 30으로 설정
    distance=30;
  }

  Serial.println(distance);             //시리얼 모니터로 공의 거리 출력

  return distance;                      //측정값 반환
}

PID제어를 위해서는 PID_v1 라이브러리가 필요합니다.

PID_v1 라이브러리 받기

PID라이브러리를 받았으면 PID를 아두이노에서 사용할 수 있습니다.

위 소스에서 중요한 부분 몇가지를 보면

1. PID각각의 게인값의 설정이 중요합니다.

위 소스에서는 P게인(Kp)을 2.5, I게인(Ki)을 0, D게인(Kd)을 1로 주었습니다.

(위에 적은 규격과 동일 하게 만든다면 게인값을 똑같이 주어도 됩니다.)

2. Set point설정을 해야 하는데 Set point는 어느 지점을 중심으로 잡을지에 대한 값입니다.

여기서는 초음파센서가 공의 위치를 15cm로 측정할 때 중심에 있는 것으로 판단하고 중앙위치로 지정하였습니다.

3. 마지막으로 서보모터의 수평값 또한 필요한데 처음 서보모터를 조립할때 서보모터가 수평을 이루는 각도를 알고 있어야 합니다.

(여기서는 100도를 서보모터의 수평값으로 설정했지만 이 값은 제작하는 사람마다 다를 수 있습니다.)

또한 PID계산을 통해 나온값을 수평을 이루는 각도에 더했을 때 0도 보다 작거나 180도 보다 크면 안됩니다.(그럴 경우 서보모터에 무리가 가거나 멈출 수 있음)