Robot Motors and Sensors Setup Sensors pragma configSensor

Подключение датчика Меню Robot -> Motors and Sensors Setup -> Sensors #pragma config(Sensor, S 1, , sensor. Light. Active)

Подключение датчика EV 3 Меню Robot -> Motors and Sensors Setup -> Sensors #pragma config (Sensor, S 1, Light, sensor. EV 3_Color)

Релейный регулятор: движение вдоль границы черного и белого int grey=45; while (true) // grey - значение серого { if (Sensor. Value[S 1]>grey){ motor[motor. B]=100; motor[motor. C]=0; } else{ motor[motor. B]=0; motor[motor. C]=100; } wait 1 Msec(1); } Поиск значения серого: View - > Light Sensor (active) - > Port 1

Калибровка с выводом на экран . . . Взять робота и посмотреть на экран int grey=(white+black)/2; display. Big. Text. Line(1, “white=%d”, white); display. Big. Text. Line(3, “black=%d”, black); display. Big. Text. Line(5, “grey=%d”, grey); Установить робота на старт и нажать кнопку while(Sensor. Value[S 2]==0); wait 1 Msec(100); while(Sensor. Value[S 2]==1);

Автоматическая круговая калибровка Установить робота на линию на границу черного и белого и включить int white=Sensor. Value[S 1]; int black=Sensor. Value[S 1]; motor[motor. B]=30; motor[motor. C]=-30; n. Motor. Encoder[motor. B]=0; while(n. Motor. Encoder[motor. B]<239*4) { int svet=Sensor. Value[S 1]; if (svet>white) white=svet; if (svet<black) black=svet; wait 1 Msec(1); } motor[motor. B]=0; motor[motor. C]=0; grey=(white+black)/2; play. Sound(sound. Beep); // Полный поворот на месте Вывести результат на экран Установить робота на старт и нажать кнопку

Калибровка в процессе движения Установить робота на белое и включить int white=Sensor. Value[S 1]; int black=Sensor. Value[S 1]-10; int grey=(white+black)/2; while(true) { int Sv 1=Sensor. Value[S 1]; u=k*(Sv 1 -grey); motor[motor. B]=v+u; motor[motor. C]=v-u; if (Sv 1>white) white=Sv 1; if (Sv 1<black) black=Sv 1; grey=(white+black)/2; wait 1 Msec(1); } Для отладки выводить результат калибровки на экран

ПД-регулятор: вариант записи Использование ошибки в Д-составляющей eold=0; while(true) { e=Sensor. Value[S 1]-grey; u=k 1*e + k 2*(e-eold); eold=e; motor[motor. B]=v+u; motor[motor. C]=v-u; wait 1 Msec(1); }

ПИД-регулятор float ki=0. 01, kp=0. 5, kd=5, i=0, eold=0, maxi=10; while(true) { e=Sensor. Value[S 1]-grey; p=kp*e; i=i+ki*e; if (abs(i)>maxi) i=sgn(i)*maxi; d=kd*(e-eold); eold=e; u=p+i+d; motor[motor. B]=v+u; motor[motor. C]=v-u; wait 1 Msec(1); }

ПИД-регулятор — два датчика float ki=0. 01, kp=0. 5, kd=5, i=0, eold=0; while(true) { e=Sensor. Value[S 1]-Sensor. Value[S 2]-es; p=kp*e; i=i+ki*e; if (abs(i)>maxi) i=sgn(i)*maxi; d=kd*(e-eold); eold=e; u=p+i+d; motor[motor. B]=v+u; motor[motor. C]=v-u; wait 1 Msec(1); }

Защита от потери линии. Волновой регулятор. last=2; // стартовое положение while(true) { sv 1=Sensor. Value[S 1]; . . . u=(sv 1 -sv 3 -est)*k; // Обычный регулятор if(sv 1<grey 1) {last=1; Clear. Timer(T 1); } else if(sv 2<grey 2) s 1 {last=2; Clear. Timer(T 1); } else if(sv 3<grey 3) {last=3; Clear. Timer(T 1); } else // Сошел с линии if(time 1[T 1]>500) // на 500 мс u=(last-2)*v/2; // поворот motor[motor. B]=v+u; . . . sleep(1); } s 3 s 2

Контроль скорости на горке n. Motor. Encoder[motor. B]=0; n. Motor. Encoder[motor. C]=0; Clear. Timer(T 2); int speed = 120; // градусов в секунду s 1 float kv=1; while(true) { int enc = (n. Motor. Encoder[motor. B] + n. Motor. Encoder[motor. C]) / 2; int err = time 1[T 2] * 0. 001 * speed – enc; v = err * kv; kp = 1 + v / 10; // Корректировка коэффициента. . . // здесь автомат и регулятор motor[motor. B]=-v+u; motor[motor. B]=-v-u; sleep(1); } s 3 s 2

Инверсная линия while(true) { if (Sensor. Value[s 2]>grey 2) r=-1; else r=1; e=Sensor. Value[S 1]-grey 1; u=r*(k*e); motor[motor. B]=50+u; motor[motor. C]=50 -u; wait 1 msec(1); } 1 2

Инверсная линия - ПД int eold=0, r; float k 1=2, k 2=10; while(true) { int r=sgn(grey 2 -Sensor. Value[S 2]); e=Sensor. Value[S 1]-grey 1; u=r*(k 1*e + k 2*(e-eold)); eold=e; motor[motor. B]=50+u; motor[motor. C]=50 -u; wait 1 msec(1); } 1 2

Контроль расстояния с совмещением регуляторов по датчику ультразвука // РЕЛЕЙНЫЙ while(true) { if (Sensor. Value[S 3]>30) { v=100; } // Сюда else v=0; motor[motor. B]=v; motor[motor. C]=v; wait 1 Msec(1); } // ПРОПОРЦИОНАЛЬНЫЙ int e; float v, k=5; while(true) { e=Sensor. Value[S 3]-30; v=e*k; if (v>100) v=100; motor[motor. B]=v; motor[motor. C]=v; wait 1 Msec(1); }

Контроль линии и расстояния с датчиком освещенности и ультразвука int e, grey=45; float u, v=50, kp=3; while(true) { // Follow line e=Sensor. Value[S 1]-grey; u=e*kp; motor[motor. B]=v+u; motor[motor. C]=v-u; wait 1 Msec(1); } float e, v, kv=5; while(true) { // Velocity control e=Sensor. Value[S 3]-30; if (e>0) v=e*kv; else v=0; if (v>100) v=100; motor[motor. B]=v; motor[motor. C]=v; wait 1 Msec(1); } Добавление контроля расстояния до объекта через скорость