Newer
Older
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
#include <Adafruit_MotorShield.h>
//General
uint8_t regions[2] = {FORWARD, BACKWARD};
int region = 0;
#define floating_avg_num 6
//Motors
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motors[] = {AFMS.getMotor(3), AFMS.getMotor(4)};
int left = 0; //index value in motor pointer array
int right = 1;
//IR
int IR_threshold = 400;
int white = 1;
int black = 0;
#define num_IR 4
float IR_avgs[num_IR] = {0}; //LRFB
int IR_colours[num_IR] = {0};
int IR_locns[num_IR] = {A0, A1, A2, A3};
float IR_vals[num_IR][floating_avg_num] = {{0}}; //check
int IR_float_counter[num_IR] = {0};
void setup() {
// put your setup code here, to run once:
for(int j=0; j<2; j++) {
motors[j]->run(RELEASE);
}
//setpinmode
}
void loop() {
// put your main code here, to run repeatedly:
for(int i=0; i < 2*floating_avg_num; i++) { //Get some values in before evaluation starts
update_IR_vals();
}
for(int j = 0; j<2; j++) {
motors[j]->run(regions[region]);
}
while (true) {
update_IR_vals();
int IR_check_val = IR_check(IR_colours);
if (IR_check_val == 0) {
move_both(motors, 150);
}
else if (IR_check_val == 1) {
move_one_motor(motors, 100, right);
}
else if (IR_check_val == 2) {
move_one_motor(motors, 100, left);
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
}
else {
Serial.println("Else block fml");
}
}
}
void move_one_motor(Adafruit_DCMotor *motors[], int velocity, int motor) { //May need to account for direction later
motors[motor]->setSpeed(velocity);
motors[(motor+1)%2]->setSpeed(0);
}
void move_both(Adafruit_DCMotor *motors[], int velocity) {
for(int j=0; j<2; j++) {
motors[j]->setSpeed(velocity);
}
}
float floating_average(int *index, float vals[], float curAvg, float newVal, int arr_len) {
curAvg = ((curAvg * float(arr_len)) - vals[*index] + newVal)/float(arr_len);
vals[*index] = newVal;
*index = (*index + 1) % arr_len;
return curAvg;
}
int IR_check(int LRFB[]) {
if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == white && LRFB[3] == white) {
return 0;
}
else if (LRFB[0] == white && LRFB[1] == black) {
return 1;
}
else if (LRFB[0] == black && LRFB[1] == white) {
return 2;
}
else {
return 3;
}
}
void update_IR_vals() {
for(int i=0; i<num_IR; i++) {
int new_val = analogRead(IR_locns[i]);
IR_avgs[i] = floating_average(&IR_float_counter[i], IR_vals[i], IR_avgs[i], new_val, floating_avg_num);
if (IR_avgs[i] > IR_threshold) {
IR_colours[i] = black;
}
else {
IR_colours[i] = white;
}
}
}