I am programming a Lego brick NXT in Matlab to go through a maze. The robot is supposed to drive forward until the touch sensor is activated by a wall.
The robot should
- stop
- backup
- stop
- read the ultraSonic (distance) sensor
- display the result
If the sensor reads a large distance, the robot turns right and stops and if the sensor reads a small distance, the robot turns left and stops.
Then, the robot continues forward again until it runs into another wall.
Current code:
The robot will hit the wall, stop, back up, stop, and it will display that it’s turning, but it doesn’t turn.
Is there something I am typing incorrectly? Should I initialize the motors before the loop or inside of the loop? I am especially wary of my pauses.
1 %B = NXTMotor('B');
2 %C = NXTMotor('C');
3 OpenSwitch(SENSOR_1);
4 OpenUltrasonic(SENSOR_4);
5
6
7 while 1
8
9 B = NXTMotor('B');
10 C = NXTMotor('C');
11
12 %FORWARD 1
13 disp('move forward');
14 B.Power = 50;
15 C.Power = 50;
16
17 B.SendToNXT();
18 C.SendToNXT();
19 B.WaitFor();
20 C.WaitFor();
21 disp('before getswitch');
22
23 if GetSwitch(SENSOR_1)
24 pause(0.2);
25 B.Power = 0;
26 C.Power = 0;
27 B.SendToNXT();
28 C.SendToNXT();
29 B.WaitFor();
30 C.WaitFor();
31 disp('Hit a wall and stops');
32
33 disp('back up');
34 B.Power = -50;
35 C.Power = -50;
36
37 B.TachoLimit = 720;
38 C.TachoLimit = 720;
39
40 B.SendToNXT();
41 C.SendToNXT();
42 B.WaitFor();
43 C.WaitFor();
44
45 pause(0.5);
46 B.Power = 0;
47 C.Power = 0;
48 B.SendToNXT();
49 C.SendToNXT();
50 B.WaitFor();
51 C.WaitFor();
52 disp('finished back up');
53
54 %ultrasonic
55 distance = GetUltrasonic(SENSOR_4);
56 disp(distance);
57
58 if distance >100
59 %if GetUltrasonic(SENSOR_4) > 100
60
61 %TURN 1
62 disp('turn1 > 100');
63
64 B.Power = 0;
65 C.Power = 100;
66
67 B.TachoLimit = 720;
68 C.TachoLimit = 720;
69
70 B.SendToNXT();
71 C.SendToNXT();
72 B.WaitFor();
73 C.WaitFor();
74 disp('turn1 finished');
75
76 pause(0.2);
77 B.Power = 0;
78 C.Power = 0;
79 B.SendToNXT();
80 C.SendToNXT();
81 B.WaitFor();
82 C.WaitFor();
83
84 end
85
86 if distance <= 100
87
88 disp('turn2 < 100');
89
90 B.Power = 100;
91 C.Power = 0;
92 disp('turn 2');
93 B.TachoLimit = 720;
94 C.TachoLimit = 720;
95
96 B.SendToNXT();
97 C.SendToNXT();
98 B.WaitFor();
99 C.WaitFor();
100 disp('Finished turn 2');
101
102 pause(0.5);
103 B.Power = 0;
104 C.Power = 0;
105 B.SendToNXT();
106 C.SendToNXT();
107 B.WaitFor();
108 C.WaitFor();
109
110 disp('stopped turn2 and ready to go forward');
111
112 end
113
114 B.Stop('off');
115 C.Stop('off');
116 end
117
118 %B.Stop('off');
119 %C.Stop('off');
120 end
121
122 CloseSensor(SENSOR_4);
123 CloseSensor(SENSOR_1);
