-
Notifications
You must be signed in to change notification settings - Fork 0
/
lego.c
62 lines (54 loc) · 988 Bytes
/
lego.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#define MOTOR_RIGHT OUT_A
#define MOTOR_LEFT OUT_B
#define COLOR_SENSOR S1
#define ULTRA_SENSOR S4
#define FORCE 100
#define DISTANCE 100
void forward()
{
OnRev(MOTOR_RIGHT, FORCE);
OnRev(MOTOR_LEFT, FORCE);
}
void backward()
{
OnFwd(MOTOR_LEFT, FORCE);
OnFwd(MOTOR_RIGHT, FORCE);
}
void rotate()
{
OnRev(MOTOR_RIGHT, FORCE);
OnFwd(MOTOR_LEFT, FORCE);
}
task robot()
{
while(true)
{
if(Sensor(COLOR_SENSOR) < 3)
{
backward();
Wait(300);
rotate();
Wait(300);
}
else
{
if(SensorUS(ULTRA_SENSOR) < DISTANCE) // Trying to find an object
{
forward();
Wait(100);
}
else
{
rotate();
}
Wait(15);
}
}
}
task main()
{
SetSensorColorFull(COLOR_SENSOR);
SetSensorUltrasonic(ULTRA_SENSOR);
Wait(3000);
robot();
}