Skip to content

Commit 4c2f884

Browse files
EHammer98EHammer98
committed
All functions integraded (untested)
>Laser detection functions (tested) >IR detection functions (not tested) >Servo drive functions (not tested)
1 parent 13ad1df commit 4c2f884

File tree

1 file changed

+127
-33
lines changed

1 file changed

+127
-33
lines changed

‎TheSocaBot.ino‎

Lines changed: 127 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44
#Hardware: Arduino Mega 2560 #
55
#Eerste opzet: 26-11-2019 #
66
#Auteurs: E. Hammer | N. Vollebregt | M. Remmig | O. Cekem #
7-
#Laatst gewijzigd: 03-12-2019 #
8-
#Versie: 1.0.3 #
7+
#Laatst gewijzigd: 17-12-2019 #
8+
#Versie: 1.0.4 #
99
#############################################################
1010
1111
##WAT JE NIET MAG GEBRUIKEN##
@@ -53,36 +53,51 @@ int IR2 = A7; //IR rechts
5353
int IR3 = A8; //*IR reserve*
5454

5555
//Thresholds
56-
int thresholdDistance = 5; //Drempelwaarde om de afstand mee te vergelijken (in CM)
57-
int laserThreshold = 850; //800
58-
int irThreshold = 0; //0
56+
int thresholdDistance = 5; //Drempelwaarde om de afstand mee te vergelijken (in CM)#5
57+
int laserThreshold = 850; //Drempelwaarde om de laser mee te detecteren #800
58+
int irThreshold = 300; //Drempelwaarde om de leader (IR) mee te detecteren #300
5959

6060
//Sensoren
6161
int laserDetected = 0; //0=geen|1=linksVoor|2=Voor|3=rechtsVoor|4=rechtsAchter|5=linksAchter
6262
int irDetected = 0; //0=geen|1=links|2=front|3=right
6363
int distance = 0; //0=stop|1=checkLDR|2=reverse
64+
65+
void distanceCheck(); // Prototype for checking the distance.
66+
void checkLDR(); //Prototype for detecting a laser function.
67+
void laserDrive(); // Prototype for driving to the laser function.
68+
void checkIR(); // Prototype for detecting IR (leader)function.
69+
void irDrive(); // Prototype for driving to the leader function.
70+
void Drive (int timeL, int timeR);
71+
void ServoStop(); // Prototype for the ServoStop function.
72+
void ServoForward(); // Prototype for the ServoForward function.
73+
void ServoTurnLeft(); // Prototype for the ServoTurnLeft function.
74+
void ServoTurnRight(); // Prototype for the ServoTurnRight function.
75+
void ServoSharpLeft(); // Prototype for the ServoSharpLeft function.
76+
void ServoSharpRight(); // Prototype for the ServoSharpRight function.
77+
void ServoBackward(); // Prototype for the ServoBackward function.
78+
6479
//Initializeren van de firmware
6580
void setup() {
6681
Serial.begin(9600); //Start een serieële verbinding
67-
pinMode(LED0, OUTPUT); //Defineer LED0 als een uitgang
68-
pinMode(LED1, OUTPUT); //Defineer LED1 als een uitgang
69-
pinMode(LED2, OUTPUT); //Defineer LED2 als een uitgang
70-
pinMode(LED3, OUTPUT); //Defineer LED3 als een uitgang
71-
pinMode(LED4, OUTPUT); //Defineer LED4 als een uitgang
72-
pinMode(ultraT, OUTPUT); //Defineer ultraT als een uitgang
73-
pinMode(ultraE, INPUT); //Defineer ultraE als een ingang
74-
pinMode(speaker, OUTPUT); //Defineer speaker als een uitgang
75-
pinMode(servoR, OUTPUT); //Defineer servoR als een uitgang
76-
pinMode(servoL, OUTPUT); //Defineer servoL als een uitgang
77-
pinMode(LDR0, INPUT); //Defineer LDR0 als een uitgang
78-
pinMode(LDR1, INPUT); //Defineer LDR1 als een uitgang
79-
pinMode(LDR2, INPUT); //Defineer LDR2 als een uitgang
80-
pinMode(LDR3, INPUT); //Defineer LDR3 als een uitgang
81-
pinMode(LDR4, INPUT); //Defineer LDR4 als een uitgang
82-
pinMode(IR0, INPUT); //Defineer IR1 als een uitgang
83-
pinMode(IR1, INPUT); //Defineer IR2 als een uitgang
84-
pinMode(IR2, INPUT); //Defineer IR3 als een uitgang
85-
pinMode(IR3, INPUT); //Defineer IR4 als een uitgang
82+
pinMode(LED0, OUTPUT); //Defineer LED0 als een uitgang
83+
pinMode(LED1, OUTPUT); //Defineer LED1 als een uitgang
84+
pinMode(LED2, OUTPUT); //Defineer LED2 als een uitgang
85+
pinMode(LED3, OUTPUT); //Defineer LED3 als een uitgang
86+
pinMode(LED4, OUTPUT); //Defineer LED4 als een uitgang
87+
pinMode(ultraT, OUTPUT); //Defineer ultraT als een uitgang
88+
pinMode(ultraE, INPUT); //Defineer ultraE als een ingang
89+
pinMode(speaker, OUTPUT); //Defineer speaker als een uitgang
90+
pinMode(servoR, OUTPUT); //Defineer servoR als een uitgang
91+
pinMode(servoL, OUTPUT); //Defineer servoL als een uitgang
92+
pinMode(LDR0, INPUT); //Defineer LDR0 als een uitgang
93+
pinMode(LDR1, INPUT); //Defineer LDR1 als een uitgang
94+
pinMode(LDR2, INPUT); //Defineer LDR2 als een uitgang
95+
pinMode(LDR3, INPUT); //Defineer LDR3 als een uitgang
96+
pinMode(LDR4, INPUT); //Defineer LDR4 als een uitgang
97+
pinMode(IR0, INPUT); //Defineer IR1 als een uitgang
98+
pinMode(IR1, INPUT); //Defineer IR2 als een uitgang
99+
pinMode(IR2, INPUT); //Defineer IR3 als een uitgang
100+
pinMode(IR3, INPUT); //Defineer IR4 als een uitgang
86101
}
87102

88103
void loop() {
@@ -158,29 +173,32 @@ void laserDrive() { //SFC 5
158173
case 1:
159174
Serial.println("LINKSvoor");
160175
digitalWrite(LED1, HIGH);
161-
//ServoTurnLeft //Boe-Bot draait naar links
176+
ServoTurnLeft(); //Boe-Bot draait naar links
162177
loop();
163178
break;
164179
case 2:
165180
Serial.println("VOORvoorkant");
166181
digitalWrite(LED3, HIGH);
167-
//ServoForward // Boe-Bot gaat naar voren
182+
ServoForward(); // Boe-Bot gaat naar voren
168183
loop();
169184
break;
170185
case 3:
171186
Serial.println("RECHTSvoor");
172187
digitalWrite(LED2, HIGH);
173-
//ServoTurnRight // Boe-Bot draait naar rechts
188+
ServoTurnRight(); // Boe-Bot draait naar rechts
189+
loop();
174190
break;
175191
case 4:
176192
Serial.println("RECHTSachter");
177193
digitalWrite(LED2, HIGH);
178-
//ServoTurnRight // Boe-Bot draait naar rechts
194+
ServoTurnRight(); // Boe-Bot draait naar rechts
195+
loop();
179196
break;
180197
case 5:
181198
Serial.println("LINKSachter");
182199
digitalWrite(LED1, HIGH);
183-
//ServoTurnLeft //Boe-Bot draait naar links
200+
ServoTurnLeft(); //Boe-Bot draait naar links
201+
loop();
184202
break;
185203
default:
186204
loop();
@@ -217,25 +235,101 @@ void irDrive() { //SFC 4
217235
switch (irDetected) {
218236
case 0:
219237
Serial.println("GEEN ir, rondje");
220-
//servoArround();
238+
ServoTurnLeft();
239+
loop();
221240
break;
222241
case 1:
223242
Serial.println("LINKS");
224243
digitalWrite(LED1, HIGH);
225-
//servoSharpLeft
244+
ServoSharpLeft();
245+
loop();
226246
break;
227247
case 2:
228248
Serial.println("RECHTS");
229249
digitalWrite(LED2, HIGH);
230-
//servoSharpRight
250+
ServoSharpRight();
251+
loop();
231252
break;
232253
case 3:
233254
Serial.println("VOOR");
234255
digitalWrite(LED3, HIGH);
235-
//servoForward
256+
ServoForward();
257+
loop();
236258
break;
237259
default:
238260
loop();
239261
break;
240262
}
241263
}
264+
265+
// Drive function for giving the servo's the correct time to turn on and off based on their given timers. This allows the vehicle to drive with no limits.
266+
// The values given should be between 1000-2000 in order to work properly.
267+
void Drive (int timeL, int timeR){
268+
269+
// If the time is equal then there is no need to do timeL - timeR.
270+
if(timeL == timeR){
271+
digitalWrite(servoL, HIGH);
272+
digitalWrite(servoR, HIGH);
273+
delayMicroseconds(timeL);
274+
digitalWrite(servoR, LOW);
275+
digitalWrite(servoL, LOW);
276+
}
277+
278+
// If timeL is greater than timeR then turn servoR low faster.
279+
else if(timeL > timeR){
280+
digitalWrite(servoL, HIGH);
281+
digitalWrite(servoR, HIGH);
282+
delayMicroseconds(timeR);
283+
digitalWrite(servoR, LOW);
284+
delayMicroseconds(timeL-timeR);
285+
digitalWrite(servoL, LOW);
286+
}
287+
288+
// If timeR is greater than timeR then turn servoL low faster.
289+
else if(timeL < timeR){
290+
digitalWrite(servoL, HIGH);
291+
digitalWrite(servoR, HIGH);
292+
delayMicroseconds(timeL);
293+
digitalWrite(servoR, LOW);
294+
delayMicroseconds(timeR-timeL);
295+
digitalWrite(servoL, LOW);
296+
}
297+
298+
// Delay of 20 ms.
299+
delay(20);
300+
}
301+
302+
// Function to stay put.
303+
void ServoStop(){
304+
Drive (1500, 1500);
305+
}
306+
307+
// Function to drive forward.
308+
void ServoForward(){
309+
Drive (1600, 1400);
310+
}
311+
312+
// Function to turn left.
313+
void ServoTurnLeft(){
314+
Drive (1500, 1400);
315+
}
316+
317+
// Function to turn right.
318+
void ServoTurnRight(){
319+
Drive (1600, 1500);
320+
}
321+
322+
// Function to turn sharp left.
323+
void ServoSharpLeft(){
324+
Drive (1600, 1600);
325+
}
326+
327+
// Function to turn sharp right.
328+
void ServoSharpRight(){
329+
Drive (1400, 1400);
330+
}
331+
332+
// Function to drive backward.
333+
void ServoBackward(){
334+
Drive (1400, 1600);
335+
}

0 commit comments

Comments
 (0)