이번 포스팅은 밸런싱 로봇에 쓰이는 알고리즘에 대해 공부해본 내용을 담아보려고 합니다.
제가 현재 공부를 해야 하는 알고리즘은 "밸런싱"에 관한 알고리즘으로, 컴학도인 저에게 많은 도움이 되었으면 좋겠습니다.
시작하기 전, 저는 현재 백엔드 개발을 JAVA와 Spring Boot를 이용해 진행중이어서 이 전에 주 언어였던 C++을 떠난지 한참 오래여서 ... 네,. 좀 막힐 것으로 예상 됩니다만 이겨냅시다.
Inverted Pendulum Algorithm
본격적으로 시작하기 전에, 제가 최종적으로 녹여야하는 알고리즘을 정리하고자 합니다.
Inverted Pendulum Algorithm이란?
https://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum§ion=SystemModeling
The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to.
we will design a controller to restore the pendulum to a vertically upward position after it has experienced an impulsive "bump" to the cart.
inverted pendulum system is single-input, multi-output (SIMO).
즉, 넘어지지않게 하는 Balancing 기능을 수행하는 알고리즘이라고 생각하자.
Input : force
Output : 반대방향 혹은 다양한 방향으로 Bump를 주어, 넘어지지 않게 제어하는 것이다.
이제 이 프로젝트에서 쓰이는 라이브러리를 통해 알고리즘을 하나씩 뜯어보겠습니다.
1. 아두이노 PID제어 라이브러리
- For an ultra-detailed explanation of why the code is the way it is, please visit:
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/- For function documentation see: http://playground.arduino.cc/Code/PIDLibrary
/**********************************************************************************************
* Arduino PID Library - Version 1.2.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under the MIT License
**********************************************************************************************/
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <PID_v1.h> //헤더파일에는 인터페이스 처럼 함수 원형만 존재함
/*Constructor (...)*********************************************************
* The parameters specified here are those for for which we can't set up
* reliable defaults, so we need to have the user set them. : 유저가 값을 세팅할 수 있음
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int POn, int ControllerDirection) //PID제어 생성자_원하는 값 설정을 하기 위함
{
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
inAuto = false;
PID::SetOutputLimits(0, 255); //default output limit corresponds to
//the arduino pwm limits
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
PID::SetControllerDirection(ControllerDirection); //방향을 integer값으로 받는다.. 방향이 다양해질 수 있겠다.
PID::SetTunings(Kp, Ki, Kd, POn);
lastTime = millis()-SampleTime; //millis()는 아두이노 보드가 현재 프로그램을 돌리기 시작한 후 지난 밀리 초 숫자를 반환한다.
//즉, lastTime은 지속한 시간. 그러니까 프로그램을 돌린 이후에 컨트롤러가 개입한 이후의 시간을 의미한다. (맞나..?)
}
/*Constructor (...)*********************************************************
* To allow backwards compatability for v1.1, or for people that just want
* to use Proportional on Error without explicitly saying so : 커스텀 하라는 건듯
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
:PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection)
{
}
/* Compute() **********************************************************************
* This, as they say, is where the magic happens. this function should be called
* every time "void loop()" executes. the function will decide for itself whether a new
* pid Output needs to be computed. returns true when the output is computed,
* false when nothing has been done.
: 무한 루프 속 계산을 지속적으로 한다는 뜻인듯. 반환값은 boolean
return true : 결과 값이 계산 되었다면 : 새로운 PID 제어를 한다 : 방향이나 속도 및 위치 변경이 필요한 상황이라고 보면 되겠다.
return false : 결과 값이 계산 안되었다면
**********************************************************************************/
bool PID::Compute()
{
if(!inAuto) return false;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/ 오차값 변수들
double input = *myInput;
double error = *mySetpoint - input; // 오차값
double dInput = (input - lastInput);
outputSum+= (ki * error);
/*Add Proportional on Measurement, if P_ON_M is specified*/
if(!pOnE) outputSum-= kp * dInput;
if(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
/*Add Proportional on Error, if P_ON_E is specified*/
double output;
if(pOnE) output = kp * error;
else output = 0;
/*Compute Rest of PID Output*/
output += outputSum - kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
*myOutput = output;
/*Remember some variables for next time*/
lastInput = input;
lastTime = now;
return true;
}
else return false;
}
/* SetTunings(...)*************************************************************
* This function allows the controller's dynamic performance to be adjusted.
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
{
if (Kp<0 || Ki<0 || Kd<0) return;
pOn = POn;
pOnE = POn == P_ON_E;
dispKp = Kp; dispKi = Ki; dispKd = Kd;
double SampleTimeInSec = ((double)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if(controllerDirection ==REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
/* SetTunings(...)*************************************************************
* Set Tunings using the last-rembered POn setting
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd){
SetTunings(Kp, Ki, Kd, pOn);
}
/* SetSampleTime(...) *********************************************************
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (double)NewSampleTime
/ (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
/* SetOutputLimits(...)****************************************************
* This function will be used far more often than SetInputLimits. while
* the input to the controller will generally be in the 0-1023 range (which is
* the default already,) the output will be a little different. maybe they'll
* be doing a time window and will need 0-8000 or something. or maybe they'll
* want to clamp it from 0-125. who knows. at any rate, that can all be done
* here.
**************************************************************************/
void PID::SetOutputLimits(double Min, double Max)
{
if(Min >= Max) return;
outMin = Min;
outMax = Max;
if(inAuto)
{
if(*myOutput > outMax) *myOutput = outMax;
else if(*myOutput < outMin) *myOutput = outMin;
if(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
}
}
/* SetMode(...)****************************************************************
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
* when the transition from manual to auto occurs, the controller is
* automatically initialized
******************************************************************************/
void PID::SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if(newAuto && !inAuto)
{ /*we just went from manual to auto*/
PID::Initialize();
}
inAuto = newAuto;
}
/* Initialize()****************************************************************
* does all the things that need to happen to ensure a bumpless transfer
* from manual to automatic mode.
******************************************************************************/
void PID::Initialize()
{
outputSum = *myOutput;
lastInput = *myInput;
if(outputSum > outMax) outputSum = outMax;
else if(outputSum < outMin) outputSum = outMin;
}
/* SetControllerDirection(...)*************************************************
* The PID will either be connected to a DIRECT acting process (+Output leads
* to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
* know which one, because otherwise we may increase the output when we should
* be decreasing. This is called from the constructor.
******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
if(inAuto && Direction !=controllerDirection)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
}
/* Status Funcions*************************************************************
* Just because you set the Kp=-1 doesn't mean it actually happened. these
* functions query the internal state of the PID. they're here for display
* purposes. this are the functions the PID Front-end uses for example
******************************************************************************/
double PID::GetKp(){ return dispKp; }
double PID::GetKi(){ return dispKi;}
double PID::GetKd(){ return dispKd;}
int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
int PID::GetDirection(){ return controllerDirection;}
2. I2Cdev 라이브러리
3. MPU6050 라이브러리
IT기획/운영