Arduinoでライントレースロボット：TCRT5000センサーとPID

1// 5 sensörün ham değerlerini okur / reads the raw values of 5 sensors
2const int N = 5;
3const int SENSOR[N] = {A0, A1, A2, A3, A4};   // A0 en solda / A0 is the leftmost
4
5void setup() {
6  Serial.begin(115200);
7}
8
9void loop() {
10  for (int i = 0; i < N; i++) {
11    Serial.print(analogRead(SENSOR[i]));
12    Serial.print(i < N - 1 ? '\t' : '\n');    // Serial Plotter için sekme / tabs for the Serial Plotter
13  }
14  delay(50);
15}

1// 3 sensörlü basit çizgi izleyen / simple 3-sensor line follower
2const int ENA = 9, IN1 = 8, IN2 = 7;      // sol motor / left motor
3const int ENB = 10, IN3 = 5, IN4 = 4;     // sağ motor / right motor
4const int S_LEFT = A0, S_MID = A1, S_RIGHT = A2;   // sensörlerin DO çıkışları / sensor DO outputs
5const bool WHITE_LINE = false;            // siyah zemin + beyaz çizgi ise true / true for a white line on black
6
7const int SPEED = 150;                    // düz gidiş hızı / straight-line speed
8const int SOFT = 80;                      // hafif dönüşte iç teker / inner wheel on a soft turn
9const int BACK = -90;                     // sert dönüşte iç teker geri / inner wheel reverses on a sharp turn
10int lastSide = 0;                         // -1 sol, 1 sağ / -1 left, 1 right
11
12void drive(int left, int right) {         // her biri -255 … 255 / each -255 … 255
13  left = constrain(left, -255, 255);
14  right = constrain(right, -255, 255);
15  digitalWrite(IN1, left >= 0);  digitalWrite(IN2, left < 0);
16  digitalWrite(IN3, right >= 0); digitalWrite(IN4, right < 0);
17  analogWrite(ENA, abs(left));
18  analogWrite(ENB, abs(right));
19}
20
21void setup() {
22  int outputs[] = {ENA, IN1, IN2, ENB, IN3, IN4};
23  for (int p : outputs) pinMode(p, OUTPUT);
24  pinMode(S_LEFT, INPUT);
25  pinMode(S_MID, INPUT);
26  pinMode(S_RIGHT, INPUT);
27}
28
29void loop() {
30  bool l = digitalRead(S_LEFT) != WHITE_LINE;    // true = sensör çizgide / true = sensor on the line
31  bool m = digitalRead(S_MID) != WHITE_LINE;
32  bool r = digitalRead(S_RIGHT) != WHITE_LINE;
33
34  if (m && !l && !r) {                    // tam ortada: düz git / centered: go straight
35    drive(SPEED, SPEED);
36  } else if (l && r) {                    // kavşak ya da bitiş çizgisi: düz geç / junction or finish line: go straight
37    drive(SPEED, SPEED);
38  } else if (l && m) {                    // çizgi biraz solda / line slightly to the left
39    drive(SOFT, SPEED);  lastSide = -1;
40  } else if (r && m) {                    // çizgi biraz sağda / line slightly to the right
41    drive(SPEED, SOFT);  lastSide = 1;
42  } else if (l) {                         // çizgi çok solda / line far to the left
43    drive(BACK, SPEED);  lastSide = -1;
44  } else if (r) {                         // çizgi çok sağda / line far to the right
45    drive(SPEED, BACK);  lastSide = 1;
46  } else {                                // çizgi kayıp: son görülen tarafa dön / line lost: turn to the last seen side
47    if (lastSide < 0)      drive(BACK, SPEED);
48    else if (lastSide > 0) drive(SPEED, BACK);
49    else                   drive(SOFT, SOFT);
50  }
51}

1// PID çizgi izleyen: Arduino Uno + 5 analog sensör + L298N
2// PID line follower: Arduino Uno + 5 analog sensors + L298N
3const int ENA = 9, IN1 = 8, IN2 = 7;      // sol motor / left motor
4const int ENB = 10, IN3 = 5, IN4 = 4;     // sağ motor / right motor
5const int BUTTON = 2;                     // diğer bacağı GND'ye / other leg to GND
6const int N = 5;
7const int SENSOR[N] = {A0, A1, A2, A3, A4};   // A0 en solda / A0 is the leftmost
8const bool WHITE_LINE = false;            // siyah zemin + beyaz çizgi ise true / true for a white line on black
9const int CENTER = (N - 1) * 500;         // orta konum: 2000 / center position: 2000
10
11float Kp = 0.06, Ki = 0.0, Kd = 0.6;      // başlangıç değerleri / starting values
12int baseSpeed = 140;                      // düzlük hızı (0-255) / straight-line speed (0-255)
13const unsigned long PERIOD_MS = 5;        // sabit döngü süresi / fixed loop period
14
15int minVal[N], maxVal[N];
16int lastPosition = CENTER;
17float integral = 0, lastError = 0;
18unsigned long lastTime = 0;
19
20void drive(int left, int right) {         // her biri -255 … 255 / each -255 … 255
21  left = constrain(left, -255, 255);
22  right = constrain(right, -255, 255);
23  digitalWrite(IN1, left >= 0);  digitalWrite(IN2, left < 0);
24  digitalWrite(IN3, right >= 0); digitalWrite(IN4, right < 0);
25  analogWrite(ENA, abs(left));
26  analogWrite(ENB, abs(right));
27}
28
29void waitButton() {
30  while (digitalRead(BUTTON) == HIGH) {}  // basılmasını bekle / wait for a press
31  delay(30);
32  while (digitalRead(BUTTON) == LOW) {}   // bırakılmasını bekle / wait for the release
33  delay(30);
34}
35
36void calibrate() {                        // 5 sn: robotu çizginin üstünde sağa sola gezdir / 5 s: sweep the robot across the line
37  for (int i = 0; i < N; i++) { minVal[i] = 1023; maxVal[i] = 0; }
38  digitalWrite(LED_BUILTIN, HIGH);
39  unsigned long start = millis();
40  while (millis() - start < 5000) {
41    for (int i = 0; i < N; i++) {
42      int v = analogRead(SENSOR[i]);
43      if (v < minVal[i]) minVal[i] = v;
44      if (v > maxVal[i]) maxVal[i] = v;
45    }
46  }
47  digitalWrite(LED_BUILTIN, LOW);
48}
49
50int readPosition() {                      // 0 (en sol) … 4000 (en sağ) / 0 (leftmost) … 4000 (rightmost)
51  long sum = 0, weighted = 0;
52  bool onLine = false;
53  for (int i = 0; i < N; i++) {
54    int range = max(maxVal[i] - minVal[i], 1);
55    long v = (long)(analogRead(SENSOR[i]) - minVal[i]) * 1000 / range;
56    v = constrain(v, 0L, 1000L);          // 0 = beyaz, 1000 = siyah / 0 = white, 1000 = black
57    if (WHITE_LINE) v = 1000 - v;
58    if (v > 500) onLine = true;           // eşik: kalibrasyonun ortası / threshold: middle of the calibration
59    if (v > 50) {                         // küçük gürültüyü yok say / ignore small noise
60      weighted += v * (i * 1000L);
61      sum += v;
62    }
63  }
64  if (!onLine) {                          // çizgi kayıp: son görülen tarafa dön / line lost: turn to the last seen side
65    return (lastPosition < CENTER) ? 0 : (N - 1) * 1000;
66  }
67  lastPosition = weighted / sum;
68  return lastPosition;
69}
70
71void setup() {
72  int outputs[] = {ENA, IN1, IN2, ENB, IN3, IN4, LED_BUILTIN};
73  for (int p : outputs) pinMode(p, OUTPUT);
74  pinMode(BUTTON, INPUT_PULLUP);
75  drive(0, 0);
76  waitButton();                           // 1. basış: kalibrasyon / 1st press: calibration
77  calibrate();
78  waitButton();                           // 2. basış: yarış / 2nd press: race
79  lastTime = millis();
80}
81
82void loop() {
83  if (millis() - lastTime < PERIOD_MS) return;
84  lastTime += PERIOD_MS;
85
86  float error = readPosition() - CENTER;  // artı: çizgi sağda / positive: line is to the right
87  integral = constrain(integral + error, -20000.0f, 20000.0f);   // birikmeyi sınırla / anti-windup
88  float derivative = error - lastError;
89  lastError = error;
90
91  int correction = (int)(Kp * error + Ki * integral + Kd * derivative);
92  drive(baseSpeed + correction, baseSpeed - correction);
93}

1// ESP32 PID çizgi izleyen + Bluetooth ile canlı ayar
2// ESP32 PID line follower + live tuning over Bluetooth
3#include "BluetoothSerial.h"
4
5BluetoothSerial SerialBT;
6
7const int ENA = 25, IN1 = 26, IN2 = 27;   // sol motor / left motor
8const int ENB = 33, IN3 = 18, IN4 = 19;   // sağ motor / right motor
9const int BUTTON = 23;                    // diğer bacağı GND'ye / other leg to GND
10const int LED_PIN = 2;                    // DevKit üzerindeki LED / onboard LED on the DevKit
11const int N = 5;
12const int SENSOR[N] = {36, 39, 34, 35, 32};   // yalnız ADC1 pinleri / ADC1 pins only
13const bool WHITE_LINE = false;            // siyah zemin + beyaz çizgi ise true / true for a white line on black
14const int CENTER = (N - 1) * 500;
15
16float Kp = 0.06, Ki = 0.0, Kd = 0.6;
17int baseSpeed = 140;
18const unsigned long PERIOD_MS = 5;
19
20int minVal[N], maxVal[N];
21int lastPosition = CENTER;
22float integral = 0, lastError = 0;
23unsigned long lastTime = 0;
24
25void drive(int left, int right) {         // her biri -255 … 255 / each -255 … 255
26  left = constrain(left, -255, 255);
27  right = constrain(right, -255, 255);
28  digitalWrite(IN1, left >= 0);  digitalWrite(IN2, left < 0);
29  digitalWrite(IN3, right >= 0); digitalWrite(IN4, right < 0);
30  analogWrite(ENA, abs(left));
31  analogWrite(ENB, abs(right));
32}
33
34void waitButton() {
35  while (digitalRead(BUTTON) == HIGH) {}
36  delay(30);
37  while (digitalRead(BUTTON) == LOW) {}
38  delay(30);
39}
40
41void calibrate() {                        // ESP32 ADC 12 bit: 0-4095 / ESP32 ADC is 12-bit: 0-4095
42  for (int i = 0; i < N; i++) { minVal[i] = 4095; maxVal[i] = 0; }
43  digitalWrite(LED_PIN, HIGH);
44  unsigned long start = millis();
45  while (millis() - start < 5000) {
46    for (int i = 0; i < N; i++) {
47      int v = analogRead(SENSOR[i]);
48      if (v < minVal[i]) minVal[i] = v;
49      if (v > maxVal[i]) maxVal[i] = v;
50    }
51  }
52  digitalWrite(LED_PIN, LOW);
53}
54
55int readPosition() {
56  long sum = 0, weighted = 0;
57  bool onLine = false;
58  for (int i = 0; i < N; i++) {
59    int range = max(maxVal[i] - minVal[i], 1);
60    long v = (long)(analogRead(SENSOR[i]) - minVal[i]) * 1000 / range;
61    v = constrain(v, 0L, 1000L);
62    if (WHITE_LINE) v = 1000 - v;
63    if (v > 500) onLine = true;
64    if (v > 50) {
65      weighted += v * (i * 1000L);
66      sum += v;
67    }
68  }
69  if (!onLine) {
70    return (lastPosition < CENTER) ? 0 : (N - 1) * 1000;
71  }
72  lastPosition = weighted / sum;
73  return lastPosition;
74}
75
76void readTuning() {                       // telefondan "p0.07", "d0.9", "s160" gibi komutlar / commands like "p0.07", "d0.9", "s160" from the phone
77  if (!SerialBT.available()) return;
78  char cmd = SerialBT.read();
79  if (cmd != 'p' && cmd != 'i' && cmd != 'd' && cmd != 's') return;   // satır sonlarını atla / skip line endings
80  float value = SerialBT.parseFloat();
81  if (cmd == 'p') Kp = value;
82  if (cmd == 'i') { Ki = value; integral = 0; }
83  if (cmd == 'd') Kd = value;
84  if (cmd == 's') baseSpeed = (int)value;
85  SerialBT.printf("Kp=%.3f Ki=%.4f Kd=%.3f speed=%d\n", Kp, Ki, Kd, baseSpeed);
86}
87
88void setup() {
89  int outputs[] = {ENA, IN1, IN2, ENB, IN3, IN4, LED_PIN};
90  for (int p : outputs) pinMode(p, OUTPUT);
91  pinMode(BUTTON, INPUT_PULLUP);
92  drive(0, 0);
93  SerialBT.begin("CizgiRobot");           // telefonda görünecek ad / name shown on the phone
94  SerialBT.setTimeout(20);                // parseFloat robotu bekletmesin / keep parseFloat from blocking
95  waitButton();
96  calibrate();
97  waitButton();
98  lastTime = millis();
99}
100
101void loop() {
102  readTuning();
103  if (millis() - lastTime < PERIOD_MS) return;
104  lastTime += PERIOD_MS;
105
106  float error = readPosition() - CENTER;
107  integral = constrain(integral + error, -20000.0f, 20000.0f);
108  float derivative = error - lastError;
109  lastError = error;
110
111  int correction = (int)(Kp * error + Ki * integral + Kd * derivative);
112  drive(baseSpeed + correction, baseSpeed - correction);
113}

1from machine import Pin, PWM, ADC
2import time
3
4ena = PWM(Pin(15)); ena.freq(1000)        # sol motor hızı / left motor speed
5in1 = Pin(14, Pin.OUT); in2 = Pin(13, Pin.OUT)
6enb = PWM(Pin(10)); enb.freq(1000)        # sağ motor hızı / right motor speed
7in3 = Pin(11, Pin.OUT); in4 = Pin(12, Pin.OUT)
8button = Pin(16, Pin.IN, Pin.PULL_UP)     # diğer bacağı GND'ye / other leg to GND
9led = Pin("LED", Pin.OUT)
10sensors = [ADC(26), ADC(27), ADC(28)]     # sol, orta, sağ / left, middle, right
11WHITE_LINE = False                        # siyah zemin + beyaz çizgi ise True / True for a white line on black
12
13N = len(sensors)
14CENTER = (N - 1) * 500                    # orta konum: 1000 / center position: 1000
15Kp, Ki, Kd = 0.05, 0.0, 0.4
16BASE = 45                                 # taban hız (%) / base speed (%)
17min_val = [65535] * N
18max_val = [0] * N
19last_pos = CENTER
20
21def drive(left, right):                   # her biri -100 … 100 / each -100 … 100
22    left = max(-100, min(100, left))
23    right = max(-100, min(100, right))
24    in1.value(left >= 0); in2.value(left < 0)
25    in3.value(right >= 0); in4.value(right < 0)
26    ena.duty_u16(int(abs(left) * 655.35))
27    enb.duty_u16(int(abs(right) * 655.35))
28
29def wait_button():
30    while button.value():                 # basılmasını bekle / wait for a press
31        time.sleep_ms(10)
32    time.sleep_ms(30)
33    while not button.value():             # bırakılmasını bekle / wait for the release
34        time.sleep_ms(10)
35    time.sleep_ms(30)
36
37def calibrate():                          # 5 sn sağa sola gezdir / sweep for 5 s
38    led.on()
39    start = time.ticks_ms()
40    while time.ticks_diff(time.ticks_ms(), start) < 5000:
41        for i, s in enumerate(sensors):
42            v = s.read_u16()
43            min_val[i] = min(min_val[i], v)
44            max_val[i] = max(max_val[i], v)
45    led.off()
46
47def read_position():                      # 0 (sol) … 2000 (sağ) / 0 (left) … 2000 (right)
48    global last_pos
49    total = weighted = 0
50    on_line = False
51    for i, s in enumerate(sensors):
52        rng = max(1, max_val[i] - min_val[i])
53        v = max(0, min(1000, (s.read_u16() - min_val[i]) * 1000 // rng))
54        if WHITE_LINE:
55            v = 1000 - v
56        if v > 500:
57            on_line = True
58        if v > 50:
59            weighted += v * i * 1000
60            total += v
61    if not on_line:                       # çizgi kayıp / line lost
62        return 0 if last_pos < CENTER else (N - 1) * 1000
63    last_pos = weighted // total
64    return last_pos
65
66drive(0, 0)
67wait_button()                             # 1. basış: kalibrasyon / 1st press: calibration
68calibrate()
69wait_button()                             # 2. basış: yarış / 2nd press: race
70integral = 0
71last_error = 0
72while True:
73    t0 = time.ticks_ms()
74    error = read_position() - CENTER
75    integral = max(-10000, min(10000, integral + error))
76    correction = Kp * error + Ki * integral + Kd * (error - last_error)
77    last_error = error
78    drive(BASE + correction, BASE - correction)
79    while time.ticks_diff(time.ticks_ms(), t0) < 5:   # 5 ms sabit döngü / fixed 5 ms loop
80        pass