전체 구성도는 위와 같은 형태로 작동된다. 사용자는 Desired Pos 즉, 원하는 특정 위치를 입력하게 된다. 입력된 값은 센서 로부터 받아오는 질점의 좌표값을 통하여 비교하게 된다.
센서에서 받는 값은 질점이 카메라 센서에서 Detecting되는 상대적 위치 값을 픽셀 값으로 반환한다. 이에 따라, 본 장치의 좌표계와 일치시키고, 축척값을 조정하기 위하여 Transform 행렬을 거치게 된다. Transform 행렬에 연산되기 위해서는 Noise가 없는 값으로 연산해야하므로 Low-pass filter를 마이크로 컨트롤러의 코딩으로서 구현하여 Noise가 없는 값을 추정하여 연산에 활용하였다.
Desired Position 과 sensing Position 과의 차이값은 1th PID controller의 error항으로 연산에 이용된다. 1th PID controller에서 반환되는 값은 Desired Position 에 가기 위한 Desired speed 에 해당하는 값으로 계산된다.
반환된 Desired speed는 Transform 행렬을 통해 얻은 위치 값을 통한 이산적인 현재 추정 속도와 비교된다. Desired speed 와 sensing speed 와의 차이 값은 2th PID controller의 error항으로 연산에 이용된다.2th PID controller에서 반환되는 값은 Desired speed 에 가기 위한 Servo에 인가하는 PWM의 Duty[ms]에 해당하는 값으로 계산된다.
이때, PID controller의 gain에 대한 연산은 humanity methods를 활용하였다. 이에 따라 완벽한 PID controller의 gain을 구할 수 없었기에 최종적인 steady state 상에서 약간의 Hysteresis가 관측되었다. 이에 따라 ON - OFF controller의 일종인 Bang-Bang controller를 도입하여 일정 steady state 에 수렴하였다고 판단되었을 때, Duty[ms]를 0으로 만들어 수렴되도록 설정하였다.
https://www.youtube.com/watch?v=IyMxYsGJcrU
// 파란선은 끝AREF 다음다음칸 초록은 바로 다음칸 주황은 VCC 노랑은 GND
//HUSKYLENS green line >> SDA; blue line >> SCL
//헤더파일 선언
#include "HUSKYLENS.h"
#include "SoftwareSerial.h"
#include <Servo.h>
//상수 선언
#define I_max 300 //수정해야함
#define I_min 0
#define bangbang_control_range 1 //수정해야함
#define X_SERVO 9
#define Y_SERVO 10
//엑추에이터 선언
Servo x_servo;
Servo y_servo;
HUSKYLENS ball_tracker;
float x, y, x_f, y_f;
//PID gain
float Kp = 3.4;
float Ki = 0.0;
float Kd = 3.0;
float r_x = 130;
float u_x = 0;
float r_y = 155;
float u_y = 0;
float x_past = 0;
float y_past = 0;
float millisTime_i;
float millisTime_f;
float dt = 0;
float I_past = 0;
float data_past = 0;
float lowpassfilter(float filter, float data, float lowpass_constant)
{
filter = filter * (1 - lowpass_constant) + data * lowpass_constant;
return filter;
}
float computePID(float r, float data, float dt, float u)
{
float error = r - data;
float P = Kp * error;
float I = I_past + Ki * error * dt;
float D = Kd * (-data + data_past) / dt;
I = constrain(I , I_min , I_max);
data_past = data;
I_past = I;
u = P + I + D;
if ( abs(error)<= bangbang_control_range)
{
u = 0;
}
u = constrain(u, -500, 500);
return u;
}
void printResult(HUSKYLENSResult result)
{
x = result.xCenter;
y = result.yCenter;
}
void Coordinate_Transform(float x, float y)
{
x = x - 120;
y = y - 155;
//여기는 상무가 수정
}
void Control_Servo()
{
x_servo.writeMicroseconds(1350.4 + u_x);
y_servo.writeMicroseconds(1423.4 + u_y);
}
void setup()
{
Serial.begin(115200);
Wire.begin();
ball_tracker.begin(Wire);
x_servo.attach(X_SERVO);
y_servo.attach(Y_SERVO);
}
void loop()
{
millisTime_i = millis();
ball_tracker.request();
ball_tracker.isLearned();
while (ball_tracker.available())
{
HUSKYLENSResult result = ball_tracker.read();
printResult(result);
//Coordinate_Transform(x,y);
x_f = lowpassfilter(x_f, x, 0.1);
y_f = lowpassfilter(y_f, y, 0.1);
x_past = x;
y_past = y;
u_x = computePID(r_x, x_f, dt, u_x);
u_y = computePID(r_y, y_f, dt, u_y);
Control_Servo();
Serial.print(x_f);
Serial.print(",");
Serial.println(y_f);
}
millisTime_f = millis();
dt = millisTime_f - millisTime_i;
}
C// 파란선은 끝AREF 다음다음칸 초록은 바로 다음칸 주황은 VCC 노랑은 GND
//HUSKYLENS green line >> SDA; blue line >> SCL
//헤더파일 선언
#include "HUSKYLENS.h"
#include "SoftwareSerial.h"
#include <Servo.h>
//상수 선언
#define I_max 300 //수정해야함
#define I_min 0
#define X_SERVO 9
#define Y_SERVO 10
//엑추에이터 선언
Servo x_servo;
Servo y_servo;
HUSKYLENS ball_tracker;
float x, y, x_f, y_f;
//원점
float r_x = 200;
float r_y = 180;
float u_x = 0;
float u_y = 0;
float V_x = 0;
float V_x_past = 0;
float x_past = 0;
float V_f = 0;
float u_x_v = 0;
float u_y_v = 0;
float V_y = 0;
float V_y_past = 0;
float y_past = 0;
float V_f_y = 0;
float millisTime_i;
float millisTime_f;
float dt = 0;
float I_past = 0;
float data_past = 0;
float buf =0;
float lowpassfilter(float filter, float data, float lowpass_constant)
{
filter = filter * (1 - lowpass_constant) + data * lowpass_constant;
return filter;
}
float computePID(float r, float data, float dt, float u, float Kp, float Ki, float Kd, float bangbang_control_range)
{
float error = r - data;
float P = Kp * error;
float I = Ki * error * dt;
float D = Kd * (-data + data_past) / dt;
I = constrain(I , I_min , I_max);
data_past = data;
I_past = I;
u = P + I + D;
if ( abs(error)<= bangbang_control_range)
{
u = 0;
}
u = constrain(u, -500, 500);
return u;
}
void printResult(HUSKYLENSResult result)
{
x = result.xCenter;
y = result.yCenter;
}
void Control_Servo()
{
x_servo.writeMicroseconds(1350 + u_x);
y_servo.writeMicroseconds(1520 + u_y);
}
void setup()
{
Serial.begin(115200);
Wire.begin();
ball_tracker.begin(Wire);
x_servo.attach(X_SERVO);
y_servo.attach(Y_SERVO);
}
void loop()
{
millisTime_i = millis();
ball_tracker.request();
ball_tracker.isLearned();
while (ball_tracker.available())
{
HUSKYLENSResult result = ball_tracker.read();
printResult(result);
x_f = lowpassfilter(x_f, x, 0.1);
y_f = lowpassfilter(y_f, y, 0.1);
V_x = x - x_past;
V_y = y - y_past;
if (V_x == 0)
{
V_x = V_x_past;
}
if (V_y == 0)
{
V_y = V_y_past;
}
V_f = lowpassfilter(V_f, V_x, 0.1);
V_f_y = lowpassfilter(V_f_y, V_y, 0.1);
x_past = x;
V_x_past = V_x;
y_past = y;
V_y_past = V_y;
u_x_v = computePID(r_x, x_f, dt, u_x_v, 0.0412, 0, 0.091, 2.5);
u_y_v = computePID(r_y, y_f, dt, u_y_v, 0.0412, 0, 0.091, 2.5);
u_x = computePID(u_x_v, V_f, dt, u_x, 10, 0, 0.09, 1);
u_y = computePID(u_y_v, V_f_y, dt, u_y, 10.5, 0, 0.09, 1);
Control_Servo();
millisTime_f = millis();
dt = millisTime_f - millisTime_i;
}