nqc/docs/guidebook/diffbot.nqc at master · BrickBot/nqc · GitHub

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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
// diffbot.nqc
//  remote control for DIFFBOT

// motors and sensors
#define DRIVE		OUT_A
#define TURN		OUT_C
#define FWD_BUTTON	SENSOR_1
#define REV_BUTTON	SENSOR_2
#define STEER		SENSOR_3

// thresholds for steering control
#define LEFT	47
#define RIGHT	55

task main()
{
	// configure sensors
	SetSensor(FWD_BUTTON, SENSOR_TOUCH);
	SetSensor(REV_BUTTON, SENSOR_TOUCH);
	SetSensor(STEER, SENSOR_LIGHT);

	// start all tasks
	start steer_task;
	start fwd_task;
	start rev_task;

	// this task is done now...
}


task fwd_task()
{
	while(true)
	{
		// drive forward while button is pressed
		until(FWD_BUTTON == 1);
		OnFwd(DRIVE);
		until(FWD_BUTTON == 0);
		Off(DRIVE);
	}
}


task rev_task()
{
	while(true)
	{
		// drive backwards while button is pressed
		until(REV_BUTTON == 1);
		OnRev(DRIVE);
		until(REV_BUTTON == 0);
		Off(DRIVE);
	}
}


task steer_task()
{
	while(true)
	{
		// should we turn left?
		if (STEER <= LEFT)
		{
			OnFwd(TURN);
			until(STEER > LEFT);
			Off(TURN);
		}

		// should we turn right?
		if (STEER >= RIGHT)
		{
			OnRev(TURN);
			until(STEER < RIGHT);
			Off(TURN);
		}
	}
}