/
robot.cpp
135 lines (105 loc) · 3.57 KB
/
robot.cpp
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
#include "direction.h"
#include "robot.h"
#include "room.h"
void Robot::step_motors(unsigned int delay) {
digitalWrite(PINS::RIGHT_MOTOR, HIGH);
digitalWrite(PINS::LEFT_MOTOR, HIGH);
delayMicroseconds(delay - readings.update());
digitalWrite(PINS::RIGHT_MOTOR, LOW);
digitalWrite(PINS::LEFT_MOTOR, LOW);
delayMicroseconds(delay - readings.update());
}
void Robot::step_left(unsigned int delay) {
digitalWrite(PINS::LEFT_MOTOR, HIGH);
delayMicroseconds(delay - readings.update());
digitalWrite(PINS::LEFT_MOTOR, LOW);
delayMicroseconds(delay - readings.update());
}
void Robot::step_right(unsigned int delay) {
digitalWrite(PINS::RIGHT_MOTOR, HIGH);
readings.update();
delayMicroseconds(delay);
digitalWrite(PINS::RIGHT_MOTOR, LOW);
readings.update();
delayMicroseconds(delay);
}
void Robot::turn(Direction towards) {
uint16_t steps = STEPS::TURN;
if (towards == Directions::left(facing)) {
left_motor_backward();
right_motor_forward();
} else if (towards == Directions::right(facing)) {
left_motor_forward();
right_motor_backward();
} else if (towards == Directions::opposite(facing)) {
left_motor_forward();
right_motor_backward();
steps *= 2;
} else {
return; //don't need to turn because already facing the right way
}
for (uint16_t i = 0; i < steps; i++) {
step_motors(STEPS::DELAY);
}
facing = towards;
}
void Robot::move(Direction dir) {
turn(dir);
left_motor_forward();
right_motor_forward();
for (uint16_t i = 0; i < STEPS::CELL; i++) {
if (readings.is_wall_front_close) {
return;
}
if (readings.is_wall_left_close) {
step_left(STEPS::DELAY / 2);
} else if (readings.is_wall_right_close) {
step_right(STEPS::DELAY / 2);
}
step_motors(STEPS::DELAY);
}
}
void Robot::explore(Direction dir, Room *dest) {
move(dir);
dest->set_open(Directions::left(facing), readings.is_wall_left);
dest->set_open(Directions::right(facing), readings.is_wall_right);
dest->set_open(facing, readings.is_wall_front);
}
static void shift_right(int arr[], const uint8_t len) {
for (uint8_t i = len - 1; i > 0; i--) {
arr[i] = arr[i - 1];
}
}
static int avg(const int arr[], const uint8_t len) {
int sum = 0;
for (uint8_t i = 0; i < len; i++) {
sum += arr[i];
}
return sum / len;
}
unsigned long Robot::Readings::update(void) {
unsigned long start = micros();
shift_right(left_readings, NUM_READINGS);
shift_right(right_readings, NUM_READINGS);
shift_right(front_readings, NUM_READINGS);
//take a reading
left_readings[0] = analogRead(PINS::LEFT_SENSOR);
right_readings[0] = analogRead(PINS::RIGHT_SENSOR);
front_readings[0] = analogRead(PINS::FRONT_SENSOR);
int left_avg = avg(left_readings, NUM_READINGS),
right_avg = avg(right_readings, NUM_READINGS),
front_avg = avg(front_readings, NUM_READINGS);
is_wall_left = left_avg > THRESHOLDS::WALL;
is_wall_right = right_avg > THRESHOLDS::WALL;
is_wall_front = front_avg > THRESHOLDS::WALL;
is_wall_left_close = left_avg > THRESHOLDS::WALL_CLOSE;
is_wall_right_close = right_avg > THRESHOLDS::WALL_CLOSE;
is_wall_front_close = front_avg > THRESHOLDS::FRONT_WALL_CLOSE;
return micros() - start;
}
void Robot::init_pins(void) {
pinMode(PINS::LEFT_MOTOR, OUTPUT);
pinMode(PINS::LEFT_DIR, OUTPUT);
pinMode(PINS::RIGHT_MOTOR, OUTPUT);
pinMode(PINS::RIGHT_DIR, OUTPUT);
}