Mapping
0. Objective
Build up a map of a static room by placing the robot in a couple of marked up locations, and have it spin around its axis while measuring the ToF readings.
1. Angular speed control
I choose to use the angular speed control, based on raw gyroscope values. Calculate the yaw angle degrees based on the gyroscope Z axis, then make a left/right spin. After several tests, I achieved a maximum of 18 degrees per step(spin).
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
float yaw = 0, gyrZ = 0;
while(yaw < setpoint)
{
get_tof();
whilt(millis() - current_time < timelimit)
{
previous_time = current_time;
current_time = millis();
time_diff = current_time - previous;
get_IMU();
gyrZ += myICM.gyrZ() * time_diff / 1000.0;
error = gyrZ - expected;
// Perform PID calculations
// ...
// and turn slightly left/right
spin();
yaw += gyrZ;
}
}
void spin()
{
// Right turn
if(PID_out > maxspeed)
{
analogWrite(A0, 0);
analogWrite(A1, maxspeed);
analogWrite(A2, 0);
analogWrite(A3, maxspeed);
}
else if(PID_out >= 0)
{
analogWrite(A0, 0);
analogWrite(A1, PID_out);
analogWrite(A2, 0);
analogWrite(A3, PID_out);
}
// Left turn
else if(PID_out <= maxspeed)
{
analogWrite(A0, maxspeed);
analogWrite(A1, 0);
analogWrite(A2, maxspeed);
analogWrite(A3, 0);
}
else
{
analogWrite(A0, -PID_out);
analogWrite(A1, 0);
analogWrite(A2, -PID_out);
analogWrite(A3, 0);
}
}
2. Read out Distances
Note: This part collaborates with Ruohan Liu (RL592).
I put the robot on the ground and use BLE to send out a start signal, make the robot turns at 18 degrees each time, record the ToF readings then send all data back.
Field test on (0, 3)
. (Video Demo)
Field test on (5, 3)
. (Video Demo)
Field test on (5, -3)
. (Video Demo)
Below are the ToF reading data.
(0, 0)
Angle (degree) | ToF (mm) |
---|---|
0 (360) | 761 |
18 | 839 |
36 | 2110 |
54 | 1689 |
72 | 1428 |
90 | 1339 |
108 | 1363 |
126 | 1235 |
144 | 723 |
162 | 601 |
180 | 1453 |
198 | 1511 |
216 | 1705 |
234 | 1821 |
252 | 1413 |
270 | 1163 |
288 | 822 |
306 | 1368 |
324 | 1738 |
342 | 2314 |
(0, 3)
Angle (degree) | ToF (mm) |
---|---|
0 (360) | 1977 |
18 | 1698 |
36 | 635 |
54 | 433 |
72 | 367 |
90 | 352 |
108 | 378 |
126 | 489 |
144 | 689 |
162 | 718 |
180 | 664 |
198 | 815 |
216 | 2503 |
234 | 2190 |
252 | 1948 |
270 | 2207 |
288 | 2542 |
306 | 909 |
324 | 803 |
342 | 1884 |
(5, 3)
Angle (degree) | ToF (mm) |
---|---|
0 (360) | 392 |
18 | 462 |
36 | 560 |
54 | 418 |
72 | 389 |
90 | 431 |
108 | 529 |
126 | 732 |
144 | 1470 |
162 | 2178 |
180 | 2457 |
198 | 571 |
216 | 423 |
234 | 369 |
252 | 2191 |
270 | 1227 |
288 | 738 |
306 | 532 |
324 | 459 |
342 | 433 |
(-3, -2)
Angle (degree) | ToF (mm) |
---|---|
0 (360) | 3007 |
18 | 1796 |
36 | 1791 |
54 | 2802 |
72 | 2213 |
90 | 684 |
108 | 652 |
126 | 701 |
144 | 853 |
162 | 776 |
180 | 660 |
198 | 610 |
216 | 655 |
234 | 816 |
252 | 870 |
270 | 724 |
288 | 682 |
306 | 710 |
324 | 881 |
342 | 823 |
(5, -3)
Angle (degree) | ToF (mm) |
---|---|
0 (360) | 396 |
18 | 419 |
36 | 490 |
54 | 628 |
72 | 1037 |
90 | 2276 |
108 | 706 |
126 | 768 |
144 | 1638 |
162 | 2891 |
180 | 3022 |
198 | 1325 |
216 | 744 |
234 | 531 |
252 | 452 |
270 | 428 |
288 | 465 |
306 | 575 |
324 | 457 |
342 | 376 |
3. Merge and Plot your readings
Below graph generated by matplotlib.pyplot