int alpha0 void calibrate int center130 right90 left180

Мотор сортировщика Дискретный П-регулятор Калибровка на старте int alpha=0; void calibrate() int center=130, right=90, left=180; { task motors() motor[motor. A]=-5; { sleep(2000); int a=0, ka=1, da=1; n. Motor. Encoder[motor. A]=0; while(true) { start. Task(motors); } int e 1=n. Motor. Encoder[motor. A]; alpha = center; a=a+sgn(alpha-a)*da; sleep(1000); motor[motor. A]=ka*(a-e 1); } wait 1 Msec(10); task main() { calibrate(); } }

Поворот с защитой от застреваний Поворот с защитой по времени Проезд по квадрату int dir=0; void square(int t) void turn_right(int angle) { { for(int i=0; i<4; i++) dir = dir + angle; motor[motor. B]=30; { motor[motor. C]=-30; sleep(t); Clear. Timer(T 1); motor[motor. B]=motor[motor. C]=-20; while(get. Gyro. Degrees()<dir sleep(500); motor[motor. B]=motor[motor. C]=50; && time 1[T 1]<2000) turn_right(90); sleep(10); motor[motor. B]=0; motor[motor. C]=0; } } }

Калибровка и сортировка Задача сортировки int alpha=0, my=2, enemy=5; task sorting() else if(Sensor. Value[S 1]==enemy) { alpha=left; { // Определение зоны sleep(700); if (Sensor. Value[S 2]==5) } {my=5; enemy=2; } while(true) else alpha=center; { } if(flag_sort) { else if(Sensor. Value[S 1]==my || (my==2 && Sensor. Value[S 1]==3)) alpha=center; sleep(20); { } alpha=right; sleep(700); } }

Поворот с регулятором Универсальный поворот по времени на регуляторе motor[motor. B]=-u; motor[motor. C]=u; wait 1 Msec(10); void turn (int angle) } { motor[motor. B]=0; int e, u; dir = dir + angle; clear. Timer(T 1); while(time 1[T 1]<2000){ e=dir-get. Gyro. Degrees(S 3); if(abs(e)>30) u=sgn(e)*30; else u = e; motor[motor. C]=0; }