Если робот отклонился влево → левый мотор замедляется, правый ускоряется
Если робот отклонился вправо → правый мотор замедляется, левый ускоряется
💻 Базовая реализация P-регулятора
🔧 Структура программы
1// Глобальные параметры регулятора
2floatKp=1.0;// Коэффициент пропорциональности
3intbaseSpeed=150;// Базовая скорость (0-255)
4intmaxCorrection=100;// Максимальная коррекция
5 6// Пины датчиков и моторов
7constintLEFT_SENSOR=A0; 8constintRIGHT_SENSOR=A1; 9constintLEFT_MOTOR_PWM=5;10constintLEFT_MOTOR_DIR=4;11constintRIGHT_MOTOR_PWM=6;12constintRIGHT_MOTOR_DIR=7;1314voidsetup(){15Serial.begin(9600);1617// Инициализация пинов
18pinMode(LEFT_MOTOR_DIR,OUTPUT);19pinMode(RIGHT_MOTOR_DIR,OUTPUT);2021// Направление моторов (вперед)
22digitalWrite(LEFT_MOTOR_DIR,HIGH);23digitalWrite(RIGHT_MOTOR_DIR,HIGH);2425Serial.println("P-Controller Line Follower Ready!");26delay(2000);// Время на размещение робота
27}2829voidloop(){30followLineWithPController();31delay(20);// 50 Hz update rate
32}
📏 Чтение и обработка датчиков
1structSensorData{ 2intleftRaw;// Сырые данные левого датчика
3intrightRaw;// Сырые данные правого датчика
4floatleftNorm;// Нормализованные данные (0.0-1.0)
5floatrightNorm;// Нормализованные данные (0.0-1.0)
6floaterror;// Ошибка регулирования
7}; 8 9SensorDatareadSensors(){10SensorDatadata;1112// Читаем сырые значения
13data.leftRaw=analogRead(LEFT_SENSOR);14data.rightRaw=analogRead(RIGHT_SENSOR);1516// Нормализация (черная линия = 0, белый фон = 1)
17data.leftNorm=map(data.leftRaw,0,1023,0.0,1.0);18data.rightNorm=map(data.rightRaw,0,1023,0.0,1.0);1920// Инвертируем для удобства (линия = высокое значение)
21data.leftNorm=1.0-data.leftNorm;22data.rightNorm=1.0-data.rightNorm;2324// Вычисляем ошибку
25data.error=data.leftNorm-data.rightNorm;2627returndata;28}
⚙️ Основной алгоритм регулирования
1voidfollowLineWithPController(){ 2// Читаем датчики
3SensorDatasensors=readSensors(); 4 5// Пропорциональное регулирование
6floatcorrection=Kp*sensors.error; 7 8// Ограничиваем коррекцию
9correction=constrain(correction,-maxCorrection,maxCorrection);1011// Вычисляем скорости моторов
12intleftSpeed=baseSpeed+correction;13intrightSpeed=baseSpeed-correction;1415// Ограничиваем скорости
16leftSpeed=constrain(leftSpeed,0,255);17rightSpeed=constrain(rightSpeed,0,255);1819// Применяем к моторам
20analogWrite(LEFT_MOTOR_PWM,leftSpeed);21analogWrite(RIGHT_MOTOR_PWM,rightSpeed);2223// Отладочная информация
24if(Serial.available()>0&&Serial.read()=='d'){25printDebugInfo(sensors,correction,leftSpeed,rightSpeed);26}27}2829voidprintDebugInfo(SensorDatasensors,floatcorrection,intleftSpeed,intrightSpeed){30Serial.print("L:");Serial.print(sensors.leftNorm,2);31Serial.print(" R:");Serial.print(sensors.rightNorm,2);32Serial.print(" E:");Serial.print(sensors.error,2);33Serial.print(" C:");Serial.print(correction,1);34Serial.print(" LS:");Serial.print(leftSpeed);35Serial.print(" RS:");Serial.println(rightSpeed);36}
⭐ Для любознательных: Продвинутая версия
1classAdvancedPController{ 2private: 3floatKp; 4intbaseSpeed; 5intmaxCorrection; 6 7// Калибровочные данные
8intsensorMin[2]={0,0}; 9intsensorMax[2]={1023,1023};1011// Фильтрация шумов
12floatsensorHistory[2][5]={{0}};// История для сглаживания
13inthistoryIndex=0;1415public:16AdvancedPController(floatkp,intspeed,intmaxCorr)17:Kp(kp),baseSpeed(speed),maxCorrection(maxCorr){}1819voidcalibrateSensors(){20Serial.println("Calibrating... Move robot over line for 5 seconds");2122unsignedlongstartTime=millis();23while(millis()-startTime<5000){24intleft=analogRead(LEFT_SENSOR);25intright=analogRead(RIGHT_SENSOR);2627// Обновляем минимумы и максимумы
28sensorMin[0]=min(sensorMin[0],left);29sensorMin[1]=min(sensorMin[1],right);30sensorMax[0]=max(sensorMax[0],left);31sensorMax[1]=max(sensorMax[1],right);3233delay(10);34}3536Serial.println("Calibration complete!");37Serial.print("Left range: ");Serial.print(sensorMin[0]);38Serial.print("-");Serial.println(sensorMax[0]);39Serial.print("Right range: ");Serial.print(sensorMin[1]);40Serial.print("-");Serial.println(sensorMax[1]);41}4243floatgetFilteredSensorValue(intsensorIndex){44intrawValue=analogRead(sensorIndex==0?LEFT_SENSOR:RIGHT_SENSOR);4546// Нормализация с калибровкой
47floatnormalized=map(rawValue,sensorMin[sensorIndex],sensorMax[sensorIndex],0.0,1.0);48normalized=constrain(normalized,0.0,1.0);4950// Добавляем в историю
51sensorHistory[sensorIndex][historyIndex]=normalized;5253// Вычисляем скользящее среднее
54floatsum=0;55for(inti=0;i<5;i++){56sum+=sensorHistory[sensorIndex][i];57}5859returnsum/5.0;60}6162voidupdate(){63// Обновляем историю
64historyIndex=(historyIndex+1)%5;6566// Читаем отфильтрованные значения
67floatleftValue=getFilteredSensorValue(0);68floatrightValue=getFilteredSensorValue(1);6970// Инвертируем (линия = высокое значение)
71leftValue=1.0-leftValue;72rightValue=1.0-rightValue;7374// Вычисляем ошибку
75floaterror=leftValue-rightValue;7677// P-регулирование
78floatcorrection=Kp*error;79correction=constrain(correction,-maxCorrection,maxCorrection);8081// Применяем к моторам
82intleftSpeed=constrain(baseSpeed+correction,0,255);83intrightSpeed=constrain(baseSpeed-correction,0,255);8485analogWrite(LEFT_MOTOR_PWM,leftSpeed);86analogWrite(RIGHT_MOTOR_PWM,rightSpeed);87}88};
🧪 Экспериментальная настройка коэффициентов
📋 Методика настройки
Пошаговый алгоритм:
Начальная настройка:
Kp = 0.5 (осторожное начало)
baseSpeed = 100 (медленно и надежно)
maxCorrection = 50
Постепенное увеличение:
Увеличиваем Kp на 0.2-0.5
Тестируем на прямой линии
Проверяем на поворотах
Поиск оптимума:
Слишком малый Kp → медленная реакция, робот съезжает
Слишком большой Kp → колебания, неустойчивость
Оптимальный Kp → быстрая реакция без колебаний
📊 Таблица экспериментов
1voidrunTuningExperiments(){ 2floattestValues[]={0.5,1.0,1.5,2.0,2.5,3.0,3.5,4.0}; 3intnumTests=sizeof(testValues)/sizeof(testValues[0]); 4 5Serial.println("Starting automatic tuning..."); 6Serial.println("Kp\tTime\tOscillations\tOffTrack"); 7 8for(inti=0;i<numTests;i++){ 9Kp=testValues[i];1011Serial.print("Testing Kp = ");12Serial.println(Kp);1314TestResultsresults=runSingleTest(10000);// 10 секунд
1516Serial.print(Kp);Serial.print("\t");17Serial.print(results.completionTime);Serial.print("\t");18Serial.print(results.oscillationCount);Serial.print("\t");19Serial.println(results.offTrackCount);2021delay(2000);// Пауза между тестами
22}23}2425structTestResults{26unsignedlongcompletionTime;27intoscillationCount;28intoffTrackCount;29floataverageError;30};3132TestResultsrunSingleTest(unsignedlongduration){33TestResultsresults={0};34unsignedlongstartTime=millis();3536floatlastError=0;37interrorDirection=0;// Для подсчета колебаний
3839while(millis()-startTime<duration){40SensorDatasensors=readSensors();4142// Подсчет колебаний
43if((sensors.error>0&&lastError<0)||44(sensors.error<0&&lastError>0)){45results.oscillationCount++;46}4748// Подсчет съездов с линии
49if(abs(sensors.error)>0.8){50results.offTrackCount++;51}5253// Средняя ошибка
54results.averageError+=abs(sensors.error);5556followLineWithPController();57lastError=sensors.error;58delay(20);59}6061results.completionTime=millis()-startTime;62results.averageError/=(duration/20);// Количество итераций
6364returnresults;65}
📈 Анализ поведения системы
Признаки разных настроек:
Kp слишком мал (< 1.0):
1Поведение: ___/‾‾‾\___/‾‾‾\___
2Проблемы:
3- Медленная реакция на повороты
4- Большие отклонения от линии
5- Может потерять линию на острых углах
Kp оптимальный (1.5-2.5):
1Поведение: ~~~~~~~~~~~~~~~~~~~
2Достоинства:
3- Быстрая реакция
4- Минимальные отклонения
5- Плавное движение
6- Стабильность на поворотах
1classLiveTuner{ 2private: 3floatkpStep=0.1; 4 5public: 6voidsetup(){ 7Serial.println("Live Tuning Mode"); 8Serial.println("Commands:"); 9Serial.println(" + : Increase Kp by 0.1");10Serial.println(" - : Decrease Kp by 0.1");11Serial.println(" s : Show current settings");12Serial.println(" d : Toggle debug mode");13Serial.println(" r : Reset to default");14}1516voidprocessCommands(){17if(Serial.available()>0){18charcommand=Serial.read();1920switch(command){21case'+':22Kp+=kpStep;23Serial.print("Kp increased to: ");Serial.println(Kp);24break;2526case'-':27Kp=max(0.1,Kp-kpStep);28Serial.print("Kp decreased to: ");Serial.println(Kp);29break;3031case's':32showSettings();33break;3435case'd':36debugMode=!debugMode;37Serial.print("Debug mode: ");38Serial.println(debugMode?"ON":"OFF");39break;4041case'r':42Kp=1.0;43baseSpeed=150;44Serial.println("Settings reset to default");45break;4647case'f':// Fast mode
48baseSpeed=min(255,baseSpeed+20);49Serial.print("Speed increased to: ");Serial.println(baseSpeed);50break;5152case'w':// sloW mode
53baseSpeed=max(50,baseSpeed-20);54Serial.print("Speed decreased to: ");Serial.println(baseSpeed);55break;56}57}58}5960private:61booldebugMode=false;6263voidshowSettings(){64Serial.println("=== Current Settings ===");65Serial.print("Kp: ");Serial.println(Kp);66Serial.print("Base Speed: ");Serial.println(baseSpeed);67Serial.print("Max Correction: ");Serial.println(maxCorrection);68Serial.print("Debug Mode: ");Serial.println(debugMode?"ON":"OFF");69Serial.println("=======================");70}71};7273LiveTunertuner;7475voidsetup(){76// ... основная инициализация ...
77tuner.setup();78}7980voidloop(){81tuner.processCommands();82followLineWithPController();83delay(20);84}
🏁 Тестовые трассы для испытаний
🛣️ Уровни сложности
Уровень 1: Новичок 🟢
1Трасса: ═══════════════════════════
2 Прямая линия 2 метра
34Критерии:
5- Без отклонений > 1 см
6- Время прохождения < 10 секунд
7- Плавность движения
Уровень 2: Любитель 🟡
1Трасса: ═══╗ ╔═══╗ ╔═══
2 ╚═════╝ ╚═════╝
3 Плавные повороты 90°
45Критерии:
6- Проходит все повороты
7- Скорость поворота адаптивная
8- Не теряет линию
Уровень 3: Эксперт 🔴
1Трасса: ═══╗ ╔╗ ╔═══════╗ ╔═╗ ╔═══
2 ╚═╝╚═╝ ╚═╝ ╚═╝
3 Острые углы, зигзаги, S-повороты
45Критерии:
6- Все углы 90° и острее
7- Минимальное время
8- Максимальная точность
Робот должен пройти прямую линию без единого отклонения
Измеряется максимальное отклонение от центра
Победитель: минимальное отклонение
Этап 2: “Скорость и точность”
Комбинированная трасса с поворотами
Балл = (Точность × 1000) / Время
Нужен баланс между скоростью и качеством
Этап 3: “Адаптация”
Трасса меняется во время движения
Робот должен быстро адаптироваться
Оценивается время реакции на изменения
🏅 Система оценки
1classCompetitionJudge{ 2public: 3structScore{ 4floataccuracy;// 0-100 баллов
5floatspeed;// 0-100 баллов
6floatsmoothness;// 0-100 баллов
7floatadaptability;// 0-100 баллов
8floattotal;// Общий балл
9};1011ScoreevaluateRun(TestResultsresults,unsignedlongtime){12Scorescore;1314// Точность (чем меньше ошибка, тем больше баллов)
15score.accuracy=max(0.0,100.0-results.averageError*100);1617// Скорость (бонус за быстрое прохождение)
18floattimeBonus=max(0.0,100.0-time/100.0);19score.speed=timeBonus;2021// Плавность (штраф за колебания)
22floatoscillationPenalty=min(100.0,results.oscillationCount*5);23score.smoothness=max(0.0,100.0-oscillationPenalty);2425// Адаптивность (штраф за съезды)
26floattrackPenalty=min(100.0,results.offTrackCount*10);27score.adaptability=max(0.0,100.0-trackPenalty);2829// Общий балл (взвешенная сумма)
30score.total=score.accuracy*0.4+31score.speed*0.2+32score.smoothness*0.3+33score.adaptability*0.1;3435returnscore;36}3738voidprintScore(Scorescore){39Serial.println("=== COMPETITION RESULTS ===");40Serial.print("Accuracy: ");Serial.println(score.accuracy);41Serial.print("Speed: ");Serial.println(score.speed);42Serial.print("Smoothness: ");Serial.println(score.smoothness);43Serial.print("Adaptability: ");Serial.println(score.adaptability);44Serial.print("TOTAL SCORE: ");Serial.println(score.total);45Serial.println("==========================");46}47};
🏃 Физкультминутка: Человеческий P-регулятор
🎮 Упражнение “Живой регулятор”
Игра 1: “Балансир-регулятор”
Один ученик стоит на одной ноге (система)
Другой дает команды коррекции (регулятор)
Цель: минимальные колебания
Настройки P-регулятора:
Kp мал: “Слегка наклонись влево”
Kp велик: “РЕЗКО влево!”
Оптимальный: “Умеренно скорректируй положение”
Игра 2: “Робот-оператор”
“Робот” с завязанными глазами
“Регулятор” видит линию на полу
Команды только “левее/правее” с разной интенсивностью
Наблюдения:
При слишком слабых командах робот теряет линию
При слишком сильных - начинает “болтаться”
Оптимальные команды - четкие, но пропорциональные ошибке
🤔 Рефлексия: от теории к практике
🎯 Что мы освоили
Технические навыки:
✅ Программирование пропорционального регулятора
✅ Экспериментальная настройка параметров
✅ Анализ поведения системы по графикам
✅ Отладка и оптимизация алгоритмов
Математическое понимание:
✅ Связь формул с реальным поведением робота
✅ Влияние коэффициентов на устойчивость
✅ Компромиссы между скоростью и точностью
✅ Методы оценки качества регулирования
🔍 Ключевые инсайты
Почему простой P-регулятор так эффективен:
Прост в реализации и понимании
Быстро реагирует на ошибки
Легко настраивается экспериментально
Подходит для большинства задач следования
Ограничения P-регулятора:
Статическая ошибка при постоянных возмущениях
Колебания при слишком большом Kp
Медленная реакция при слишком малом Kp
Не учитывает историю и тренды
🌟 Главное понимание
“Программирование умного регулятора - это не просто набор команд. Это создание цифрового мозга, который думает, анализирует и принимает решения быстрее человека!”
🏠 Домашнее задание
📋 Базовый уровень (для всех)
1. Технический отчет
Создайте отчет о практической работе:
Таблица экспериментов с разными Kp
График зависимости поведения от коэффициента
Обоснование выбора оптимального значения
Описание наблюдаемых эффектов
2. Анализ кода
Изучите написанную программу и ответьте:
Какая строка кода самая важная для работы регулятора?
Что произойдет, если убрать ограничение correction?