PID제어로 서보 모터 제어
좀.. 어이가 없죠 서보 모터 자체가 PID제어로해서 목표각을 넣어주면 그정도로 각도로 제어가 되는 Servo.h를 쓰면서
그 위에 pid제어기를 또 사용한다니.... 뭐 pid제어기를 사용하는 이유는 물체까지 거리를 알수없어 각도를 못구하기때문에
pid제어기로 물체가 화면의 가운데로 오도록 모터를 제어하게됩니다.
PID 제어기
pid제어기에 대한 이론적인 내용은 스킵스킵
pid 제어기에서 error=물체와 영상 화면 중간 좌표와의 거리, process=액츄에이터가 될거같은데 Setpoint랑 output은 뭐지??
음....대충 ouput= angle로보고 Setpoint는 그냥 error로 보아야될거같네요
수업시간에 배운 내용에 따르면 Kp, Ki, Kd파라미터 값 설정이 문제가 되겠네용
PID제어기 구현
1.라즈베리에서 먼저 x축 각도를 0~200도 까지 10도 단위로 증가 시키고 200도 이상이면 80도로 바꿔주고 y축 값은 30도
고정으로 publish
#include "ros/ros.h"
#include "std_msgs/Float32.h"
int main(int argc,char **argv)
{
ros::init(argc,argv,"talker");
ros::NodeHandle n;
ros::Publisher pub_xx=n.advertise<std_msgs::Float32>("x_axis",1);
ros::Publisher pub_yy=n.advertise<std_msgs::Float32>("y_axis",1);
ros::Rate loop_rate(10);
float count=0;
while(ros::ok())
{
std_msgs::Float32 msg_x;
std_msgs::Float32 msg_y;
msg_x.data=count;
msg_y.data=30;
pub_xx.publish(msg_x);
pub_yy.publish(msg_y);
if (count<200){
count+=10;
}else{
count=80;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}2.PID 클래스를 생성해서 아두이노 라이브러리로 생성
(a) pid_control.h
상속개념 함써보고싶었습니다 읍읍
#ifndef pid_control_h
#define pid_control_h
#include "Arduino.h"
#include <Servo.h>
class PID
{
public:
PID( int angle, float kp, float kd, float ki);
float pid_control(float distance);
protected:
float error;
float Kp, Ki, Kd;
float error_previous;
float desired_angle;
float P_control, I_control, D_control;
float Time;
float PID_control;
float current_angle;
};
class control : public PID
{
public://PID pid=PID(90,1.3,1.4,4.5);
control(bool mode,int angle, float Kp, float Ki, float Kd);
void controling(bool flag);
float get_result();
void put_distance(float dis);
private:
float result;
bool mode_;
float distance;
};
#endif음... 이때까지만해도 c++이 익숙치 않아 this를 남발했네용 맨날 파이썬에서 self.쓰다보니....
(b). pid_control.cpp
#include "Arduino.h"
#include "pid_control.h"
PID::PID(int angle_x, float kp, float kd, float ki)
{
this->current_angle = angle_x; // axis of x
this->Time = 0.004;
this->Kp = kp; //1.3
this->Kd = kd; //1.4
this->Ki = ki; //4.5
}
control::control(bool mode,int angle, float Kp, float Ki, float Kd) : PID(angle, Kp, Ki, Kd)
{
this->result = angle; //setting angle,Kp,Ki,Kd
mode_=mode;
}
float PID::pid_control(float distance)//
{
this->desired_angle = distance;
this->error = this->desired_angle - this->current_angle; //이부분 수정 error는 sub으로부터 온 distance
// this->error=distance;
this->P_control = this->Kp * this->error;
this->I_control = this->Ki * this->error * this->Time;
this->D_control = this->Kd * (this->error - this->error_previous) / this->Time;
this->PID_control = this->P_control + this->I_control;
this->current_angle = this->current_angle + this->PID_control;
this->error_previous = this->error;
return this->current_angle;
}
void control::controling(bool flag)
{
if (flag == false) {//switch off시 pid계산안하고 종료
return;
}
float angle;
int max,min;
//모드 설정
if (mode_==true){
max=180;
min=0;
}else{
max=150;
min=20;
}
angle = pid_control(this->distance);//pid 결과값 반환
//pid계산값 min보다 작으면 result=min, max보다 result가 크면 result=max로 설정
if (angle <= max && angle >= min) {
this->result= angle;
}
else if (angle>max) {
this->result=max;
}
else if (angle<min){
this->result=min;
}
}
//물체와의 거리 distance값을 put
void control::put_distance(float dis){
this->distance=dis;
}
//result값 리턴
float control::get_result(){
return result;
}
3.아두이노 코드
#include <pid_control.h>
#include <ros.h>
#include <ArduinoHardware.h>
#include <Servo.h>
#include <std_msgs/Bool.h>
#include "std_msgs/Float32.h"
#define pin_num 2 //switch pin number
#define x_pin 6 // x axis servo motor pin number
#define y_pin 7 // y axis servo motor pin number
//control(bool mode,int angle,float Kp,float Ki,float Kd)
//1.3,1.4,4.5
Servo servo_x;
Servo servo_y;
control control_x(true,90, 1.3, 1.4, 4.5);//0~180 , setting angle=90
control control_y(false,90, 1.3, 1.4, 4.5);//20~150 , setting angle=90
/*******************rosserial part***************************************/
ros::NodeHandle nh;
std_msgs::Bool bool_msg;
std_msgs::Float32 degree_msg;
ros::Publisher switching("trigger", &bool_msg);
ros::Publisher degree("degree", °ree_msg);
bool flag = false;
void pub_degree(){ //x axis degree publish
degree_msg.data = control_x.get_result();
degree.publish(°ree_msg);
}
void trigger(){ //switch state publish
bool_msg.data = flag;
switching.publish( &bool_msg );
}
void sub_axis_x(const std_msgs::Float32& msg) { //x axis desired value subscribe
control_x.put_distance(msg.data);
}
void sub_axis_y(const std_msgs::Float32& msg) {//y axis desired value subscribe
control_y.put_distance(msg.data);
}
ros::Subscriber<std_msgs::Float32> sub_x("/x_axis", sub_axis_x);
ros::Subscriber<std_msgs::Float32> sub_y("/y_axis", sub_axis_y);
void changing() {//interrupt function , changing flag value
if (digitalRead(pin_num)==1){
flag=true;
}else{ flag = false; }
}
void setup() {
Serial.begin(57600);
servo_x.attach(x_pin);
servo_y.attach(y_pin);
servo_x.write(control_x.get_result());
servo_y.write(control_y.get_result());
delay(5);
pinMode(pin_num, INPUT_PULLUP);//switch pull up mode
attachInterrupt(0, changing,CHANGE);//switch value interrupt
bool_msg.data = flag;
degree_msg.data = control_x.get_result();
nh.initNode();
nh.advertise(switching);
nh.advertise(degree);
nh.subscribe(sub_x);
nh.subscribe(sub_y);
}
void loop() {
trigger();
control_x.controling(flag);
servo_x.write(control_x.get_result());
pub_degree();
control_y.controling(flag);
servo_y.write(control_y.get_result());
delay(10);
nh.spinOnce();
}자 이제 실행시키고 rqt_graph로 실행 노드 확인
라즈베리파이에서 publish한 y_axis,x_axis를 받아서 ouput degree를 publish하게 됩니다.
아주 잘되네요 ㅋㅋㅋㅋ
동영상 서비스가 종료되어 해당 콘텐츠를 재생할 수 없습니다.
다음으로 rqt_plot /degree결과를 보죵
90->0도로 가는데 어느정도 값이 오차가 발생하네용
근데 문제없이 동작하믄 장땡아닌가....
이제 영상처리 파트로 ~~~~~
'대외활동 > irc src 씨름로봇' 카테고리의 다른 글
| [씨름로봇] 영상처리- color tracking(2) (0) | 2022.10.10 |
|---|---|
| [씨름로봇] 영상처리- color tracking(1) (0) | 2022.10.10 |
| [씨름로봇 ] 색 기반 물체 추적기 (티스토리 초보) (1) | 2022.10.03 |
