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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
|
#include <Wire.h> //https://www.arduino.cc/reference/en/language/functions/communication/wire/
#include "Adafruit_TCS34725.h" //https://github.com/adafruit/Adafruit_TCS34725
#include <Adafruit_MCP23X17.h> //https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library/tree/master
//25, 26, 27, 28 connect to extra pins for potential expention
//i2c addr
#define PCAADDR 0x70 //i2c address of the i2c multiplexer
#define mcp1ADDR 0x20 //first portmultiplexer for ir sensors
#define mcp2ADDR 0x22 //first portmultiplexer for motors
//miltiplex1
#define ir1 0 // GPA0
#define ir2 1 // GPA1
#define ir3 2 // GPA2
#define ir4 3 // GPA3
#define ir5 4 // GPA4
#define ir6 5 // GPA5
#define ir7 6 // GPA6
#define ir8 7 // GPA7
#define ir9 8 // GPB0
#define ir10 9 // GPB1
#define ir11 10 // GPB2
#define ir12 11 // GPB3
#define ir13 12 // GPB4
#define ir14 13 // GPB5
#define ir15 14 // GPB6
#define ir16 15 // GPB7
//miltiplex1
#define pwm5 12 // GPA0 Motor driver 3 pwm1
#define pwm6 13 // GPA1 Motor driver 3 pwm2
#define enB3 14 // GPA2 Motor driver 3 enB
#define enA3 15 // GPA3 Motor driver 3 enA
#define M1A 4 // GPA4 Motor 1 A
#define M1B 5 // GPA5 Morot 1 B
#define M2A 6 // GPA6 Motor 2 A
#define M1B 7 // GPA7 Motpr 2 B
#define pwm1 8 // GPB0 Motor driver 1 pwm1
#define pwm2 9 // GPB1 Motor driver 1 pwm2
#define enB1 10 // GPB2 Motor driver 1 enB
#define enA1 11 // GPB3 Motor driver 1 enA
#define pwm3 12 // GPB4 Motor driver 2 pwm1
#define pwm4 13 // GPB5 Motor driver 2 pwm2
#define enB2 14 // GPB6 Motor driver 2 enB
#define enA2 15 // GPB7 Motor driver 2 enA
//teensy
#define M3A 24 //Motor 3 pin A
#define M3B 9 //Motor 3 pin B
#define dip1 2 //settings dip switch port 1
#define dip2 3 //settings dip switch port 2
#define dip3 4 //settings dip switch port 3
#define dip4 5 //settings dip switch port 4
#define dip5 6 //settings dip switch port 5
#define dip6 27 //settings dip switch port 6
#define buz 8 //buzzer
int dir1 = 1; // data IR1
int dir2 = 1; // data IR2
int dir3 = 1; // etc
int dir4 = 1;
int dir5 = 1;
int dir6 = 1;
int dir7 = 1;
int dir8 = 1;
int dir9 = 1;
int dir10 = 1;
int dir11 = 1;
int dir12 = 1;
int dir13 = 1;
int dir14 = 1;
int dir15 = 1;
int dir16 = 1;
int colsensdata[6] = {99, 99, 99, 99, 99, 99}; //data colorsensors
Adafruit_MCP23X17 mcp1; // Instance for the first MCP23017 at address 0x20
Adafruit_MCP23X17 mcp2; // Instance for the second MCP23017 at address 0x22
Adafruit_TCS34725 tcs = Adafruit_TCS34725(); // Initialise color sensorwith default values (int time = 2.4ms, gain = 1x)
String tlocd = ""; //temp data for goal location
String locd = ""; //goal location in deg
bool readloc = false;
unsigned long datatime = 0;
template <typename T_ty> struct TypeInfo { static const char * name; };
template <typename T_ty> const char * TypeInfo<T_ty>::name = "unknown";
#define TYPE_NAME(var) TypeInfo< typeof(var) >::name
#define MAKE_TYPE_INFO(type) template <> const char * TypeInfo<type>::name = #type;
MAKE_TYPE_INFO( int )
MAKE_TYPE_INFO( float )
MAKE_TYPE_INFO( short )
void setup() {
Serial.begin(9600);
Serial1.begin(19200);
//connect unconected pins to GND
pinMode(40, INPUT_PULLUP);
pinMode(41, INPUT_PULLUP);
// Initialize first MCP23017 with I2C address 0x20
if (!mcp1.begin_I2C(mcp1ADDR)) {
Serial.println("(IR_sensors)Error initializing MCP23017 at 0x20.");
}
// Initialize second MCP23017 with I2C address 0x22
if (!mcp2.begin_I2C(mcp2ADDR)) {
Serial.println("(Motors)Error initializing MCP23017 at 0x22.");
}
// Configure pins for the first MCP23017 (0x20)
mcp1.pinMode(ir1, INPUT);
mcp1.pinMode(ir2, INPUT);
mcp1.pinMode(ir3, INPUT);
mcp1.pinMode(ir4, INPUT);
mcp1.pinMode(ir5, INPUT);
mcp1.pinMode(ir6, INPUT);
mcp1.pinMode(ir7, INPUT);
mcp1.pinMode(ir8, INPUT);
mcp1.pinMode(ir9, INPUT);
mcp1.pinMode(ir10, INPUT);
mcp1.pinMode(ir11, INPUT);
mcp1.pinMode(ir12, INPUT);
mcp1.pinMode(ir13, INPUT);
mcp1.pinMode(ir14, INPUT);
mcp1.pinMode(ir15, INPUT);
mcp1.pinMode(ir16, INPUT);
// Configure pins for the second MCP23017 (0x22)
mcp1.pinMode(ir16, INPUT);
Serial.println("SETUP COMPLETE GOING TO LOOP");
}
void pcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(PCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
void loop() {
Wire.begin();
// Read IR sensors
dir1 = mcp1.digitalRead(ir1);
dir2 = mcp1.digitalRead(ir2);
dir3 = mcp1.digitalRead(ir3);
dir4 = mcp1.digitalRead(ir4);
dir5 = mcp1.digitalRead(ir5);
dir6 = mcp1.digitalRead(ir6);
dir7 = mcp1.digitalRead(ir7);
dir8 = mcp1.digitalRead(ir8);
dir9 = mcp1.digitalRead(ir9);
dir10 = mcp1.digitalRead(ir10);
dir11 = mcp1.digitalRead(ir11);
dir12 = mcp1.digitalRead(ir12);
dir13 = mcp1.digitalRead(ir13);
dir14 = mcp1.digitalRead(ir14);
dir15 = mcp1.digitalRead(ir15);
dir16 = mcp1.digitalRead(ir16);
//Read color sensors
for (uint8_t t=0; t<6; t++) {
pcaselect(t);
for (uint8_t addr = 0; addr<=127; addr++) {
if (addr == PCAADDR) continue;
Wire.beginTransmission(addr);
if (!Wire.endTransmission()) {
if (tcs.begin()) {
uint16_t r, g, b, c, colorTemp, lux;
tcs.getRawData(&r, &g, &b, &c);
colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c);
lux = tcs.calculateLux(r, g, b);
if ((colorTemp) == 5201){
colsensdata[t] = 0;
}
else if ((g)>=(2*(r)) && (g)>=12){
colsensdata[t] = 1;
}
else if ((colorTemp)>= 6750){
colsensdata[t] = 2;
}
else if ((colorTemp) < 6750){
colsensdata[t] = 3;
}
else{
Serial.println(colorTemp);
Serial.println(TYPE_NAME((colorTemp, DEC)));
colsensdata[t] = 98;
}
} else {
Serial.println("No TCS34725 found ... check your connections");
colsensdata[t] = 97;
}
}
}
}
//read camera
if (Serial1.available()){
datatime = millis();
byte byteRead = Serial1.read();
if (byteRead == 's'){
tlocd = "";
readloc = true;
}
else if (byteRead == 'e'){
readloc = false;
locd = tlocd;
}
else if (readloc == true){
tlocd += (char)byteRead;
}
}
else{
if (millis() - datatime > 5000){
locd = "999";
}
}
//read compas
//printing sensor data
//IR
Serial.print("IR data: ");
Serial.print(dir1); Serial.print(", "); Serial.print(dir2);
Serial.print(", "); Serial.print(dir3); Serial.print(", "); Serial.print(dir4);
Serial.print(", "); Serial.print(dir5); Serial.print(", "); Serial.print(dir6);
Serial.print(", "); Serial.print(dir7); Serial.print(", "); Serial.print(dir8);
Serial.print(", "); Serial.print(dir9); Serial.print(", "); Serial.print(dir10);
Serial.print(", "); Serial.print(dir11); Serial.print(", "); Serial.print(dir12);
Serial.print(", "); Serial.print(dir13); Serial.print(", "); Serial.print(dir14);
Serial.print(", "); Serial.print(dir15); Serial.print(", "); Serial.print(dir16);
Serial.println();
delay(250);
//color
Serial.print("sensor data = "); // 0=air, 1=green, 2=black, 3=white, 90=error
for(int i = 0; i < 6; i++){
Serial.print(colsensdata[i]);
Serial.print(", ");
}
Serial.println();
//camera
Serial.print("location of the goal is: ");
if(locd == 999){
Serial.println("unknown, there is no working connection with the camera");
}
else{
Serial.println(locd);
}
//compas
//dip settings
//do stuf with data
/**
first move if line is detected with color sens
is ball in posesion:
yes: go to goal using camera location
no: move to ball using IR data
*
*/
Serial.println();
Serial.println();
Serial.println();
delay(1);
}
|
