라인 팔로워 로봇 - PID 제어 - Android 설정

이 프로젝트 정보

이 프로젝트의 목적은 PID 제어가 가능한 라인 팔로워 로봇을 구축하는 것입니다. 우리는 또한 더 좋고 빠른 조정을 위해 주요 제어 매개변수를 쉽게 설정하기 위해 Android 장치를 사용할 것입니다.

이 프로젝트는 내 의도가 Line Follower Robots의 잠재력을 탐구하는 더 복잡한 2개 부분 중 첫 번째 프로젝트입니다. 2부:미로 해결 로봇, 아두이노와 인공 지능을 사용하는 로봇은 간단한 인공 지능 기술을 사용하여 미로를 탐색하고 해결합니다.

다음은 라인 회로를 따라가는 로봇을 보여주는 비디오입니다.

1단계:BOM

필요한 재료 목록은 매우 간단하고 최종 로봇은 매우 저렴합니다(약 $75.00):

본체(필요에 따라 조정 가능):

2단계:모터 설정

모터는 2개의 연속 서보(SM-S4303R)를 사용하였다. 사진에서 볼 수 있듯이 함께 "접착"되어 하나의 단단한 블록을 만듭니다(3M 명령 스트립, 접착제 또는 양면 테이프 사용). 이러한 서보는 데이터 입력에서 수신된 펄스 폭에 의해 정의된 주어진 속도로 실행됩니다. 이 특정 서보의 경우 펄스 폭은 1.0ms(1,000마이크로초)에서 2.0ms(2,000마이크로초)로 변경됩니다. 다른 서보는 다른 펄스 폭으로 작동할 수 있습니다.

자세히 살펴보기:

두 서보를 물리적으로 연결했으면 위의 드로잉 회로를 따라 소스(외부 5V 또는 6V)를 공급하고 Arduino의 신호를 공급합니다.

모두 연결되면 가장 먼저 해야 할 일은 1.5ms 펄스를 보내 모터가 "정지"(작동 중이 아님)되었는지 확인하는 것입니다. 그렇지 않은 경우 서보를 완전히 정지하도록 조정해야 합니다(서보 아래에 있는 노란색 볼트를 찾으십시오).

참고 :서보에 이 물리적 조정이 없으면 완전히 멈출 때까지 기능 내에서 매개변수 "1500" 마이크로초(위 또는 아래)를 변경하려고 시도합니다.

아래 Arduino 코드는 작업을 수행할 수 있습니다.

#include  // 서보 라이브러리 Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach(3); leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500);}무효 루프(){} 

3단계:움직임 테스트를 위해 본체와 모터 조립

서보가 장착되는 방식으로 인해 속도 범위는 다음과 같습니다.

신호화 및 테스트 목적으로 외부 LED가 핀 13에 추가됩니다(원하는 경우 외부 대신 내부 Arduino LED를 사용할 수 있지만 케이블 중간에서 보기 어려울 것이라는 점을 고려하십시오).

또한 버튼은 핀 9에 연결되어 있습니다. 이 버튼은 테스트 목적과 로봇 시작에 매우 유용합니다.

예:

while(digitalRead(buttonPin)) { }motorTurn(LEFT, 500);motorTurn(RIGHT, 500); 

로봇이 왼쪽으로 회전하고 500ms를 기다렸다가 오른쪽으로 회전하도록 명령하는 2개의 라인은 버튼을 누른 후에만 발생합니다(buttonPin =0). 그 전에 프로그램은 무한 루프에서 중지됩니다.

아래 코드는 완전한 모터 테스트(앞으로, 뒤로, 완전히 정지, 좌회전, 우회전)를 위한 기반으로 사용할 수 있습니다. 필요한 경우 모터에 따라 필요한 회전 각도에 대한 지연을 조정해야 합니다(또한 때때로 왼쪽 및 오른쪽 펄스 값은 모터의 균형 부족을 보상하기 위해 약간 달라야 합니다.

4단계:블루투스 모듈(옵션)

Bluetooth 모듈 HC-06은 그림과 같이 브레드보드에 설치해야 합니다. Arduino 라이브러리 SoftSerial이 사용됩니다.

HC-06 핀 연결 아래:

로봇은 블루투스와 함께 또는 블루투스 없이 작동합니다. 코드는 BT를 활성화하지 않으면 기본 매개변수가 로봇이 사용하는 방식으로 작성되었습니다. 따라서 HC-06 모듈을 설치하지 않으려면 걱정하지 마십시오. 코드는 여전히 잘 작동합니다. 이 튜토리얼의 마지막 부분에서는 로봇 매개변수의 더 나은 조정 및/또는 수동 모드에서 로봇을 이동하기 위해 데이터를 전송하기 위해 Android 앱을 사용하는 방법을 탐구할 것입니다. 예를 들어 경쟁에서 라인 팔로워 로봇을 더 많이 사용하고 싶은 사람이 있을 경우를 대비하여 블루투스와 앱의 사용은 선택 사항으로 남겨두겠습니다.

5단계:라인 센서 추가

다음과 같이 케이블을 Arduino 핀에 연결합니다.

제 경우에는 4개의 센서가 통합된 모듈과 1개의 추가 센서가 있는 모듈을 사용합니다. 모두 호환됩니다. 단순화를 위해 다이어그램에는 5개의 독립형 센서가 함께 연결되어 있습니다. 최종 결과는 두 구성 모두에서 동일합니다.

6단계:IR 센서 로직 구현

IR 센서는 개별 IR LED와 IR 포토다이오드로 구성됩니다. LED에서 방출된 IR 빛은 표면에 부딪혀 IR 포토다이오드로 다시 반사됩니다. 그러면 포토다이오드는 표면의 반사율 수준에 비례하는 출력 전압을 생성합니다("밝은 표면"의 경우 더 높은 값, "검은색/어두운 표면"의 경우 더 낮은 값).

사용된 센서의 경우 모듈의 집적 회로는 출력으로 간단한 디지털 신호(HIGH:어둡게, LOW:밝음)를 생성합니다. 모듈에 설치된 전위차계(사진 참조)는 "어두움" 또는 "밝음"으로 간주되도록 정확한 수준의 빛을 조정합니다. 반사광 색상이 검정/어두울 때 출력에서 ​​HIGH("1") 디지털 레벨이 생성되고 다른 밝은 색상에 대해 LOW("0")가 생성되는 방식으로 작동합니다. 여기서는 4개의 센서가 있는 통합 모듈과 단독 센서가 있는 추가 모듈(모양은 다르지만 논리는 동일)을 사용했습니다. 이 조합은 아래에 설명된 대로 훌륭하고 부드러운 제어에 좋은 5개의 센서 어레이입니다.

5개의 센서 어레이는 하나의 센서만 검은색 선과 관련하여 중앙에 배치되면 해당 특정 센서만 HIGH를 생성하는 방식으로 장착됩니다. 다른 한편으로는 2개의 센서가 검은색 선의 전체 너비를 동시에 덮을 수 있도록 센서 사이의 공간을 계산하여 두 센서 모두에서 HIGH를 생성해야 합니다(위 그림 참조).

라인을 따를 때 가능한 센서 어레이 출력은 다음과 같습니다.

5개의 센서가 있어 아래와 같이 라인 위의 로봇 위치를 제어하는 ​​데 도움이 되는 "오류 변수"를 생성할 수 있습니다.

최적의 조건은 "중간 센서"(센서 2) 바로 아래에 선이 있는 로봇이 중앙에 있을 때라고 가정해 보겠습니다. 배열의 출력은 0 0 1 0 0이며 이 상황에서 "오류"는 "0"이 됩니다. 로봇이 왼쪽으로 움직이기 시작하면(라인이 오른쪽으로 "움직이는 것 같습니다") 오류는 양의 신호와 함께 증가해야 합니다. 로봇이 오른쪽으로 움직이기 시작하면(라인이 왼쪽으로 "움직이는 것 같습니다"), 같은 방식으로 오류가 증가해야 하지만 이제 음의 신호가 나타납니다.

센서 상태와 관련된 오류 변수는 다음과 같습니다.

0 0 1 0 0 ==> 오류 =0

Arduino 코드를 보면 각 센서는 특정 이름으로 정의됩니다(왼쪽에 있는 Line Follow Sensor에는 레이블 "0"이 지정되어야 함을 고려하십시오).

const int lineFollowSensor0 =12;const int lineFollowSensor1 =18;const int lineFollowSensor2 =17;const int lineFollowSensor3 =16;const int lineFollowSensor4 =19; 

각 센서의 값을 저장하기 위해 어레이 변수가 생성됩니다:

int LFSensor[5]={0, 0, 0, 0, 0}; 

어레이의 각 위치는 각 센서의 출력으로 지속적으로 업데이트됩니다.

LFSensor[0] =digitalRead(lineFollowSensor0), LFSensor[1] =digitalRead(lineFollowSensor1), LFSensor[2] =digitalRead(lineFollowSensor2), LFSensor[3] =digitalRead(lineFollowSensor3), LFSensor[4] =digitalRead(lineFollowSensor4); 

각 센서의 값이 있으면 오류 변수를 생성하는 논리를 구현해야 합니다.

7단계:방향 제어(비례 제어 - P)

완벽한! 이 시점에서 로봇이 조립되어 작동합니다. 모터로 몇 가지 기본 테스트를 수행하고 센서의 출력을 읽고 라인을 통해 테스트해야 합니다. 빠진 것은 진짜 '두뇌', '인공지능'의 첫걸음이다. 로봇이 라인을 계속 따라갈 수 있도록 보장하는 제어 로직을 구현하여 이것을 얻을 것입니다.

단순 비례 제어:

로봇이 라인 위를 달리고 있고 센서 어레이 출력이 "0 0 1 0 0 "이라고 가정합니다. . 해당 오류는 "0"입니다. 이 상황에서 두 모터는 일정한 속도로 전진해야 합니다.

예:

변수 정의:iniMotorSpeed ​​=250 ; LEFT 서보는 1,250us, RIGHT 서보는 1,750us의 펄스를 수신한다는 의미입니다. 이러한 매개변수를 사용하여 로봇은 절반 속도로 전진합니다. 오른쪽 서보 정방향 속도의 범위는 펄스 길이가 1,500us(정지됨)에서 2,000us(최대 속도)이고 왼쪽 서보가 1,500us(중지됨)에서 1,000us(최고 속도)입니다.

rightServo.writeMicroseconds(1500 + iniMotorPower);leftServo.writeMicroseconds(1500 - iniMotorPower); 

이제 로봇이 왼쪽으로 이동하고("LINE이 오른쪽으로 이동"과 유사) 센서 3도 덮었다고 가정합니다. 어레이 출력은 "0 0 1 1 0 " 입니다. 및 오류 =1 . 이 상황에서 필요한 것은 로봇을 오른쪽으로 돌리는 것입니다. 그러려면 RIGHT 서보의 속도를 줄여야 합니다. 즉, 펄스 길이를 줄여야 합니다. 또한 LEFT 서보의 속도를 높여야 하므로 LEFT 서보 펄스의 길이를 줄여야 합니다. 그렇게 하려면 모터 제어 기능을 변경해야 합니다.

rightServo.writeMicroseconds(1500 + iniMotorPower - 오류); ==> 양수 오류:velocityleftServo.writeMicroseconds(1500 - iniMotorPower - 오류) 감소; ==> 양수 오류:속도 증가 

위의 논리는 정확하지만 펄스 길이에서 "1" 마이크로초를 더하거나 빼면 실제 시간에 필요한 수정이 생성되지 않는다는 것을 이해하기 쉽습니다. 더하거나 뺄 숫자가 더 커야 한다는 것은 직관적입니다. 예를 들어 50, 100 등입니다. 이를 얻으려면 "오류"에 상수를 곱해야 합니다(이를 "K"라고 합시다). 이 상수의 영향이 오류에 비례하면 이름을 "비례 상수:Kp"로 지정합니다. .

모터 기능은 다음과 같습니다.

int Kp =50;rightServo.writeMicroseconds(1500 + iniMotorPower - Kp*error);leftServo.writeMicroseconds(1500 - iniMotorPower - Kp*error); 

아래와 같이 모터에 어떤 일이 일어날지 재개할 수 있습니다.

상황이 반대이고 로봇이 오른쪽으로 구동되는 경우 오류는 "음수"이고 서보의 속도가 변경되어야 합니다.

이 시점에서 로봇이 한쪽으로 몰리는 만큼 오류가 커지고 중앙으로 더 빨리 돌아가야 한다는 것이 분명합니다. 로봇의 속도는 오류에 반응할 것이며 그에 비례할 것입니다. 이를 '비례 제어'라고 합니다. , 즉 더 복잡한 제어 네트워크인 PDI(Proportional, Derivative, Integral)의 "P" 구성요소입니다.

8단계:PID 제어(선택 사항)

이 부분에서 점프를 하고 싶을 때도 OK입니다. 마지막 단계에서 설명한 비례 제어를 유지하거나 로봇에 더 복잡한 제어 시스템을 구현하기 위해 두뇌를 태울 수 있습니다. 선택은 귀하의 선택입니다.

마음먹었다면 가자!

PID(비례, 미분 및 적분)는 가장 일반적인 제어 방식 중 하나입니다. 대부분의 산업용 제어 루프는 PID 제어의 일부를 사용합니다. 이 예에서 사용된 수동 기술을 포함하여 PID 루프를 조정하는 방법에는 여러 가지가 있습니다.

PID를 단순한 스프링으로 생각하십시오. 용수철은 원래 길이가 있는데 팽창이나 수축에 의해 방해를 받으면 가능한 한 가장 짧은 시간에 원래 길이로 회복되는 경향이 있습니다. 마찬가지로 시스템의 PID 알고리즘에는 'set point라고 하는 제어할 특정 물리량의 설정값이 있습니다. '는 어떤 이유로 변경되었을 때 시스템이 그 안에 있는 다른 필요한 기능을 제어하여 가능한 한 최단 시간에 원래 설정값으로 돌아갑니다. PID 제어기는 물리량을 제어하고 지정된 값과 같게 만들 필요가 있는 모든 곳에서 사용됩니다. 예, 크루즈 컨트롤러 자동차, 로봇, 온도 조절기, 전압 조절기 등

PID는 어떻게 작동합니까?

시스템은 'error를 계산합니다. ' 또는 '편차 '는 센서를 사용하여 해당 물리량의 현재 값을 측정하여 설정 포인트에서 물리량의. 설정값으로 돌아가려면 이 'error '는 최소화해야 하며 이상적으로는 0과 같아야 합니다. 또한 이 프로세스는 가능한 빨리 이루어져야 합니다. 이상적으로는 설정점 변경에 대한 시스템 응답에 지연이 없어야 합니다.

더 많은 정보는 여기를 포함한 많은 책과 웹사이트에서 찾을 수 있습니다.

PID 구현

i) 오류 용어 (e):

이것은 설정값과 제어되는 수량의 현재 값 사이의 차이와 같습니다. 오류 =set_point – 현재_값 (우리의 경우 오류 변수는 라인 위의 로봇 위치에서 가져옵니다.

ii) 비례 기간(P):

이 항은 오차에 비례합니다.

P =오류

이 값은 설정값을 달성하기 위해 물리량에 필요한 변화의 크기를 나타냅니다. 비율 항은 제어 루프 상승 시간 또는 설정점에 도달하는 속도를 결정합니다.

iii) 적분항(I):

이 항은 모든 이전 오류 값의 합계입니다.

나 =나 + 오류

이 값은 설정점으로부터의 변화에 ​​대한 시스템의 빠른 응답을 담당합니다. 적분항은 비례항에 필요한 정상 상태 오차를 제거하는 데 사용됩니다. 일반적으로 소형 로봇은 정상 상태 오류에 대해 걱정하지 않고 "루프 튜닝 ".

iv) 차등 또는 파생 용어(D):

이 항은 설정값의 순시 오차와 이전 순간의 오차의 차이입니다.

D =오류 - 이전 오류

이 값은 설정값에 가까워지면 물리량의 변화 속도를 늦추는 역할을 합니다. 파생 용어는 오버슈트 또는 시스템이 "과도하게 수정해야 하는 정도를 줄이는 데 사용됩니다. ".

방정식:

PID 값 =(Kp*P) + (Ki*I) + (Kd*D)

어디:

Kp 변경 정도를 변경하는 데 사용되는 상수입니다. 설정값을 달성하는 데 필요합니다. 기 변화율을 변경하는 데 사용되는 상수입니다. 세트 포인트를 달성하기 위해 물리적인 양으로 가져와야 합니다. Kd 안정성을 변경하는 데 사용되는 상수입니다. 시스템의.

루프를 조정하는 한 가지 접근 방식은 시도 오류 임시 방법일 수 있습니다. :

Kd 변수를 0으로 설정 먼저 Kp 항만 조정합니다. Kp 25 여기에서 우리의 경우 시작하기에 좋은 곳입니다. 마지막 단계에서 우리는 내 로봇과 잘 작동하는 50의 Kp를 사용했습니다. 로봇이 합리적으로 반응하면 제어 루프의 미분 부분(Kd ). 먼저 Kp 및 Kd 값을 각각 Kp 값의 1/2로 설정합니다. 예를 들어 로봇이 Kp =50으로 합리적으로 응답하면 Kp =25 및 Kd =25를 설정하여 시작합니다. 오버슈트를 줄이려면 Kd(미분) 게인을 늘리고 로봇이 불안정해지면 줄입니다.

고려해야 할 루프의 다른 구성요소 중 하나는 실제 샘플/루프 비율입니다. . 이 매개변수의 속도를 높이거나 낮추면 로봇의 성능이 크게 달라질 수 있습니다. 이것은 지연 에 의해 설정됩니다. 코드에 있는 명령문. 최적의 결과를 얻기 위한 시도-에러 임시 방법입니다.

위의 접근 방식을 기반으로 아래 기능이 구현되었습니다.

마지막 단계에서 사용된 단순 Kp 상수는 이 보다 완전한 PIDvalue로 대체됩니다. :

void motorPIDcontrol(){ int leftMotorSpeed ​​=1500 - iniMotorPower - PID값; int rightMotorSpeed ​​=1500 + iniMotorPower - PID값; leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed);} 

그러나 Kd 및 Ki =0이 있는 경우 , PID값 마지막 단계에서 사용한 Kp*오류입니다.

9단계:최종 코드

이 단계에서 로봇은 일정한 루프를 따를 수 있으며 멈추지 않고 수행합니다. 루프 프로그램은 다음과 같습니다.

void 루프(){ readLFSsensors(); // 센서 어레이에서 센서, 저장 값을 읽고 "오류"를 계산하십시오.calculatePID(); motorPIDcontrol();} 

그러나 보다 완전하고 실제적인 작업을 위해서는 최소한 몇 가지 기본 "commands를 추가하는 것이 중요합니다. " 완료 "라인 ". 예를 들어, 새 변수를 소개하겠습니다. "mode ". 이 변수에 대해 3가지 상태를 정의합니다.

모드:

모든 센서가 검은색 선을 찾으면 센서 어레이 출력은 다음과 같습니다. 1 1 1 1 1. 이 조건에서 모드를 "STOPPED"로 정의할 수 있고 로봇은 "완전 정지를 수행해야 합니다. ".

Follower Line Robots의 다른 일반적인 상황은 "줄 없음 ", 또는 센서 어레이 출력은 다음과 같습니다. 0 0 0 0 0. 이 경우 라인을 찾고 정상적인 라인 추적 조건이 재개될 때까지 180o 뒤로 돌리거나 작은 각도로 회전하도록 프로그래밍할 수 있습니다.

완전한 루프()

void 루프() { readLFSsensors(); 스위치(모드) { 경우 STOPPED:motorStop(); 부서지다; 케이스 NO_LINE:motorStop(); 모터턴(왼쪽, 180); 부서지다; 경우 FOLLOWING_LINE:계산PID(); 모터PID제어(); 부서지다; }} 

실제 최종 코드에는 몇 가지 추가 논리와 초기화해야 하는 변수 등이 통합됩니다. 위의 설명에서는 단순성을 위해 생략했지만 최종 코드를 보면 모든 것이 명확해야 합니다.

아래는 최종 Arduino 코드입니다:

10단계:Android 앱을 사용하여 PID 제어 조정

In the previous code, you can find at "robotDefines.h " tab the following definitions for the constants to be used with the PID control:

float Kp=50;float Ki=0;float Kd=0; 

As explained at previous step, the best way to define the correct constant to be used with a PID controller is using the "Try-error" methodology. The bad side of that is that you must re-compile the program each time that you must change it. One way to speed-up the process is to use the Android App to send the constants at the "Set-Up Phase" .

I developed an Android App exclusively for that. In short there are the traditional manual commands:

Also 3 sliders were included, one for each PID constants:

An extra button was included that will work exactly as the button connected on Arduino Pin9. You can use one or the other, it does not matter.

Below you can find the .aia file that can be modified at MIT AppInventor and the .apk file to be installed directly in your Android device.

Step 11:Changing the Code for PID remote tuning

During the Setup, we will introduce a loop where you can send the PID parameters to the Robot before you put him over the line:

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // BT 리모콘에서 명령이 수신되었는지 확인 manualCmd(); 명령 =""; } checkPIDvalues(); mode =STOPPED; 

The manual command function will be:

void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE; 부서지다; 케이스 's':motorStop(); // 두 모터를 모두 끕니다. break; 경우 'f':motorForward(); 부서지다; 케이스 'r':motorTurn(RIGHT, 30); 모터스톱(); 부서지다; 케이스 'l':motorTurn(LEFT, 30); 모터스톱(); 부서지다; 경우 'b':motorBackward(); 부서지다; 경우 'p':Kp =명령[2]; 부서지다; 케이스 'i':Ki =명령[2]; 부서지다; 케이스 'd':Kd =명령[2]; 부서지다; }} 

In the video, you can see some tests using the Android App. Below the final code including the PID setup via Android:

Step 12:Conclusion

This is the first part of a more complex project, exploring the potentiality of a line follower robot. In the next part, I will develop a Maze solve robot, based on this this project here. Hopefully I can contribute for others to learn more about electronics, robot, Arduino, etc.

The update files for this project can be found at GITHUB. For more tutorials, please visit my Blog:MJRoBot.org

Greetings from the south of the world!

Thanks

마르셀로

코드

코드 스니펫 #1일반 텍스트

#include  // Servo library Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach(3); leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500);}void loop(){}

코드 스니펫 #2일반 텍스트

while(digitalRead(buttonPin)) { }motorTurn (LEFT, 500);motorTurn (RIGHT, 500);

코드 스니펫 #3일반 텍스트

const int lineFollowSensor0 =12;const int lineFollowSensor1 =18;const int lineFollowSensor2 =17;const int lineFollowSensor3 =16;const int lineFollowSensor4 =19;

코드 스니펫 #5일반 텍스트

LFSensor[0] =digitalRead(lineFollowSensor0);LFSensor[1] =digitalRead(lineFollowSensor1);LFSensor[2] =digitalRead(lineFollowSensor2);LFSensor[3] =digitalRead(lineFollowSensor3);LFSensor[4] =digitalRead(lineFollowSensor4);

Code snippet #6Plain text

if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==1 )) error =4;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==1 )) error =3; else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =2;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =1;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =0;else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =- 1;else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-2;else if((LFSensor[0]==1 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-3;else if((LFSensor[0]==1 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-4;

Code snippet #10Plain text

void calculatePID(){ P =error; 나는 =나 + 오류; D =오류 이전 오류; PID값 =(Kp*P) + (Ki*I) + (Kd*D); previousError =error;}

코드 스니펫 #11일반 텍스트

void motorPIDcontrol(){ int leftMotorSpeed =1500 - iniMotorPower - PIDvalue; int rightMotorSpeed ​​=1500 + iniMotorPower - PID값; leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed);}

코드 스니펫 #12일반 텍스트

void loop (){ readLFSsensors(); // read sensors, storage values at Sensor Array and calculate "error" calculatePID(); motorPIDcontrol();}

Code snippet #15Plain text

void loop() { readLFSsensors(); switch (mode) { case STOPPED:motorStop(); 부서지다; case NO_LINE:motorStop(); motorTurn(LEFT, 180); 부서지다; case FOLLOWING_LINE:calculatePID(); 모터PID제어(); 부서지다; }}

Code snippet #17Plain text

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // BT 리모콘에서 명령이 수신되었는지 확인 manualCmd(); 명령 =""; } checkPIDvalues(); mode =STOPPED;

Code snippet #18Plain text

void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE; 부서지다; 케이스 's':motorStop(); // 두 모터를 모두 끕니다. break; 경우 'f':motorForward(); 부서지다; 케이스 'r':motorTurn(RIGHT, 30); 모터스톱(); 부서지다; 케이스 'l':motorTurn(LEFT, 30); 모터스톱(); 부서지다; 경우 'b':motorBackward(); 부서지다; 경우 'p':Kp =명령[2]; 부서지다; 케이스 'i':Ki =명령[2]; 부서지다; 케이스 'd':Kd =명령[2]; 부서지다; }}

깃허브

회로도