PID 제어 활용하기.

PID 제어 

 

 INPUT값은 thruster 의 PWM, servo의 각도이고, ERROR는 목표속도쌍과 현재속도쌍의 차이

그 에러를 INPUT 에 조작해줘서 좋은 OUTPUT이 나오게 

 

그러면 내 배의 속도를 측정할 수 있어야 하고 각속도를 측정할 수 있어야 해 

v, yaw_rate 두개. (무슨센서의 어떤값으로 측정해야하는지)

이것과 DWA에서 원하는 v, yaw_rate를 일치시켜줘야하고 이 차이가 error지

 

그러면 일단 pwm 신호 1100~ 1500 ~ 1900

서보 값 0~300도

 

(예시)

P제어

pwm (1650, 1650)

서보 150 (중립)

고고 ..

optimal_v (bestU)

optimal_yaw_rate (bestU)

cur_v

cur_yaw_rate

 

error_v = optimal_v - cur_v

error_yaw_rate = optimal_yaw_rate - cur_yaw_rate

pwm + P_gain * error_v

servo + P_gain * error_yaw_rate

 

의문 : pwm servo 두개 따로 pid 코스트 줘야할지 같이 줘야할지.?

>> 그런거 같음 P제어만 봐도 pwm 범위가 1100 ~1900이고 , 서보는 0~300인데 같은 P * error를 넣어 버리면 하나는 감당이 안되거나 미미하거나 이렇게 되버려서... 그러면 다른 의문이 생기지

다른 의문 : 서보만 PID 따로해주면 서보모터만 돈다고 각속도가 변하는게 아닌데(스러스터가 정지라거나,,) 제어가 될까?

>>ex) 제자리 회전을 해야된다라는 상황 : 목표 U(v:0, yaw_rate:30) 일때 , pwm 1500이상 서보 (150+90) 이정도는 쏴줘야함. 만약 직진 전진하고있어 그러면 error가 -속도 , +각속도가 나오고 그걸 처리해줘야해 그렇게 되면 결과가 1500, 서보만 돌아가있는 이렇게 될수 있는거네 ? 그러면 기본 pwm을 1500, 1500 쌍 넣지말고 1600, 1600 이런식으로 넣으면 해결될수도?! 그리고 서보에 제한 걸어놓자. +-90으로?!

 

(그러면 test를 1600,1600 서보 90꺾었을때 각속도 체크해서 맥스 각속도에 넣거나 해야할듯.) 

 

헐 BUT !! 근데?! 그래도 1600 1600이면 v가 0이 아니라 오차가 -가 뜨니깐 1500으로 맞춰버리겠네 ;;; 그러면 목표U( 0 , 30) 이런 쌍은 제어 할 수 가 없는거네?!?!

 

그냥 아지무스 말고 출력 양쪽 제어로 하면 안되나 혹은 다른 해결책이 있나 내가 모르는.?

 

1안 ) 속도 0인 쌍을 예외처리 하자 

 

2안) 모르겠네 아지무쓰포기?