上星期和上上星期的实验是LEGO机器人

#define LEFT_SENSOR SENSOR_3
#define RIGHT_SENSOR SENSOR_1

#define LEFT_DRIVE OUT_C
#define RIGHT_DRIVE OUT_A

#define VELOCITY 5
#define OFFSET 0
#define K 50 /* Proportion gain */
#define KI 0 /* Integral gain */
#define KD 50 /* Differential gain */

#define THRESHOLD 45

sub init_sensor()
{
SetSensorType(LEFT_SENSOR, SENSOR_TYPE_LIGHT);
SetSensorMode(LEFT_SENSOR, SENSOR_MODE_PERCENT);

SetSensorType(RIGHT_SENSOR, SENSOR_TYPE_LIGHT);
SetSensorMode(RIGHT_SENSOR, SENSOR_MODE_PERCENT);
}

sub Stop()
{
Off(RIGHT_DRIVE);
Off(LEFT_DRIVE);
}

void set_velocity(int x)
{
int l, r;

l = -x + VELOCITY;
r = x + VELOCITY;

if (l > 7) {
l = 7;
}
if (r > 7) {
r = 7;
}
if (l >= 0 && r >= 0) {
SetPower(LEFT_DRIVE, l);
SetPoewr(RIGHT_DRIVE, r);
OnFwd(LEFT_ERIVE + RIGHT_DRIVE);
} else if (l >= 0 && r < 0) { Off(RIGHT_DRIVE); SetPower(LEFT_DRIVE, l); OnFwd(LEFT_DRIVE); } else if (l < 0 && r >= 0) {
Off(LEFT_DRIVE);
SetPower(RIGHT_DRIVE, r);
OnFwd(RIGHT_DRIVE);
} else {
Off(LEFT_DRIVE + RIGHT_DRIVE);
}
}

task trace()
{
int y;
int e, ei, ed, e1;
int p, i, d;

ei = 0;
e1 = 0;
while (true) {
e = RIGHT_SENSOR - LEFT_SENSOR + OFFSET;

ei += e;

ed = e - e1;
el = e

p = (K * e) / 100;
i = (KI * ei) / 100;
d = (KD * ed) / 100;

y = p + i + d;

set_velocity(y);

Wait(3);
}
}

task search()
{
while(true) {
if (LEFT_SENSOR > THRESHOLD && RIGHT_SENSOR > THRESHOLD) {
stop trace;
SetPower(LEFT_DROVE + RIGHT_DRIVE, 2);
OnRev(LEFT_DRIVE + RIGHT_DRIVE);
Wait(10)
start trace;
}
Wait(5);
}
}