blob: d909db89eaeb0535a19c88de5e025d03fb5cd989 (
plain)
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
|
#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
#include <LIS3MDL.h> // https://github.com/pololu/lis3mdl-arduino
#include <LSM6.h> // https://github.com/pololu/lsm6-arduino
#include <TimerThree.h> // https://www.arduino.cc/reference/en/libraries/timerthree/
#include <Servo.h>
// --- Configuratie van pinnen ---
#define LEFT_MOTOR_PIN 3 // Pin voor de linker motor
#define RIGHT_MOTOR_PIN 5 // Pin voor de rechter motor
#define KICKER_PIN 9 // Pin voor de kicker servo
#define SENSOR_PIN A0 // Analoge pin voor bal detectie
Servo kickerServo; // Servo object voor de kicker
// --- Globale variabelen ---
bool ballDetected = false; // Variabele om baldetectie bij te houden
// Constanten voor leesbaarheid
const int KICK_POSITION = 0; // Trappositie voor de kicker
const int RESET_POSITION = 90; // Resetpositie voor de kicker
const int BALL_DETECTION_THRESHOLD = 500; // Drempelwaarde voor baldetectie
const unsigned long MOVE_DURATION = 1000; // Tijd om te bewegen in milliseconden
const unsigned long SEARCH_DURATION = 500; // Tijd om te zoeken in milliseconden
const unsigned long KICK_DELAY = 500; // Tijd voor de kicker beweging
// --- Initialisatie ---
void setup() {
pinMode(LEFT_MOTOR_PIN, OUTPUT);
pinMode(RIGHT_MOTOR_PIN, OUTPUT);
pinMode(SENSOR_PIN, INPUT);
kickerServo.attach(KICKER_PIN);
kickerServo.write(RESET_POSITION); // Startpositie van de kicker
Serial.begin(9600); // Seriële communicatie voor debugging
}
// --- Hoofdprogramma ---
void loop() {
ballDetected = detectBall();
if (ballDetected) {
Serial.println("Bal gedetecteerd!");
moveTowardsBall();
kickBall();
} else {
Serial.println("Zoeken naar bal...");
searchForBall();
}
}
// --- Functies ---
// Detecteert of er een bal in de buurt is
bool detectBall() {
int sensorValue = analogRead(SENSOR_PIN);
Serial.print("Sensorwaarde: ");
Serial.println(sensorValue);
return sensorValue > BALL_DETECTION_THRESHOLD; // Controleer drempelwaarde
}
// Beweeg de robot naar de bal
void moveTowardsBall() {
digitalWrite(LEFT_MOTOR_PIN, HIGH);
digitalWrite(RIGHT_MOTOR_PIN, HIGH);
delay(MOVE_DURATION); // Beweeg voor een vaste duur
stopMotors();
}
// Laat de robot zoeken naar de bal
void searchForBall() {
digitalWrite(LEFT_MOTOR_PIN, HIGH);
digitalWrite(RIGHT_MOTOR_PIN, LOW);
delay(SEARCH_DURATION); // Draai gedurende een vaste duur
stopMotors();
}
// Stop beide motoren
void stopMotors() {
digitalWrite(LEFT_MOTOR_PIN, LOW);
digitalWrite(RIGHT_MOTOR_PIN, LOW);
}
// Trap de bal
void kickBall() {
Serial.println("Bal wordt getrapt!");
kickerServo.write(KICK_POSITION); // Beweeg de kicker naar de trappositie
delay(KICK_DELAY); // Wacht een vaste duur
kickerServo.write(RESET_POSITION); // Reset naar de oorspronkelijke positie
delay(KICK_DELAY); // Wacht om stabiliteit te verzekeren
}
|
