PID제어기에의한 모터 제어
PID제어기에의한 모터 제어
서보모터를 이용한 볼 제어 ( PID library)
초음파센서 (TS601-01)모듈을 이용하여 센서로부터 떨어진 볼의 거리를 측정하여 원하는 거리의 위치에서 볼을 정지시키는 동작으로 거리에 따라서 서보모터의 회전각이 조절되도록 한다.
볼 제어 회로의 동작을 그림으로 Kp=2.5, Ki= 0, Kd=0.5의 값은 볼 제어 회로에서 볼이 이동할 수 있는 거리 및 구조에 따라 달라 질수 있다.
#include<PID_v1.h> //1
#include<Servo.h> //2
Servo myServo;
double Setpoint, Input, Output , ServoOutput; //3
float duration, cm ; //4
const int pingPin = 7; //5
const int servoPin = 9 ; //6
float Kp = 2.5; //7
float Ki = 0 ; //8
float Kd = 0.5 ; //9
PID myPID (&Input, &Output, &Setpoint, Kp, Ki, Kd ,DIRECT ); //10
void setup() {
Serial.begin(9600); //11
myServo.attach(servoPin); //12
Input = readPosition() ; //13
myPID.SetMode(AUTOATIC) ; //14
myPID.SetOutputLimits( -5 , 5 ) ; //15
}
void loop() {
Setpoint = 14 ; //16
Input = readPosition() ; //17
myPID.Copute(); //18
ServoOutput = 96+Output ; //19
myServo.write(ServoOutput) ; //20
Serial.println(ServoOutput) ;
Serial.println(cm); //21
}
float readPosition() { //22
delay(20);
pinMode(pinPin, OUTPUT) ;
digitalWrite( pinPin, LOW) ;
delayMicroseconds (2);
digitalWrite( pinPin, HIGH ) ;
delayMicroseconds(5);
digitalWrite( pinPin, LOW ) ;
pinMode(pinPin , INPUT) ;
duration = pulseIn(pinPin, high ) ;
cm = microsecondsToCentimeters(duration);
if(cm>28) cm= 28; //23
if(cm<2) cm =2; //24
return cm ; //25
}
long microsecondsToCentimeters( long microseconds) //26
{
return microseconds/29/2 ;
}
소스코드 분석
1) PID제어기에 의한 모터 제어에 필요한 함수의 프로토 타입(protype) 및 구조체등의 자료구조가 저의 되어있는 파일
2)서보모터 관련 함수의 포로토타입(protype) 및 구조체등의 자료 구조가 정의 되어있는 파일 3)PID제어기의 변수 선언
3)PID제어기의 변수 선언
4)초음차 센서 거리측정값을 얻기위한 변수 선언
5)초음파센서의 신호핀을 설정한다.
6)서보모터의 PWM 신호핀을 설정한다.
7)비레제어 게인값 8) 적분 제어게인값 9) 미분제어 게인값
10) PID제어기의 객체를 생성한다.
11)시리얼 모니터로 통신하기위한 속도를 설정한다.
12)아두이노의 9번핕을 서보모터와 연결
13)공으 l 위치를 측정하기위한 사용자 정의 함수
14)PID제어기의 모드를 AUTOMATIC으로 설정한다.
15)PID의 값을 최소 -5부터 최대 5까지 설정( 서보모터의 회전각의 범위)
16)공의 위치를 설정한다 ( Set point를 14cm로설정)
17)공의 위치를 측정한다
18)PID제어기의 연산을 한다
19)서버모터의 수평을 이루었을 때 으 각도를 설정한다
(제작 과정에 따라서 각도는 다를수 있음)
20)PID제어기에 의해 계산된 값을 아두이노 9번 핀에서 서보모터로 출력
21)시리얼 모니터로 공의 거리를 출력한다.
22)초음파 센서 모듈로부터 거리정뽀를 얻는다.
23)공의 측정거리가 28cm 이상일 경우에 치대 28cm로 설정한다.
24)공의 측정거리가 2cm 이하일 경우 최소 2cm로 설정
25)공의 거리를 측정한 값을 반환한다.
26)공의 거리를 측정한 결과를 센티메타 값으로 반환해 주는 함수이다.