DIY DIY Robot Ηλεκτρική σκούπα για Arduino DIY

/*
Πρόγραμμα για τον έλεγχο ενός ρομπότ με δύο κινητήρες.
Το ρομπότ γυρίζει όταν οι κινητήρες αλλάζουν ταχύτητα και κατεύθυνση.
Οι μπροστινοί προφυλακτήρες στις αριστερές και στις δεξιά πλευρές ανιχνεύουν εμπόδια.
Οι υπερήχων μπορούν να συνδεθούν σε αναλογικές εισόδους (δοκιμασμένες σε LV-MaxSonar-EZ1):
- βάλτε καρφίτσες σε σειριακούς πίνακες με την ακόλουθη σειρά: αριστερά, δεξιά, μπροστά, άλλα ..
Παραδείγματα:
1. μόνο τα αριστερά και δεξιά σόναρ που συνδέονται με τις ακίδες 2 και 3: sonarPins [] = {2,3}
2. αριστερά, δεξιά και μπροστά σόναρ που συνδέονται με τους ακροδέκτες 2, 3 και 5: sonarPins [] = {2,3,5}
3. Μόνο μπροστά βυθόμετρο που συνδέεται στον ακροδέκτη 5: sonarPins [] = {-1, -1.5}
4. Μόνο το αριστερό ηχοσύστημα που συνδέεται με τον ακροδέκτη 2: sonarPins [] = {2}
5. Μόνο σωστός σόναρ συνδεδεμένος με ακίδες 3: sonarPins [] = {-1,3}
6.5 ηχάνες που συνδέονται με ακίδες 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Η θωράκιση του κινητήρα χρησιμοποιείται για τη λειτουργία κινητήρων.
*/
const int Baud = 9600; // ταχύτητα θύρας UART

// ιδιότητες βυθομέτρου
int sonarPins [] = {1, 2}; // Αναλογικός ακροδέκτης Αριθμοί σε αισθητήρα σόναρ Καρφίτσωμα AN
const long MinLeftDistance = 20; // Ελάχιστη επιτρεπόμενη αριστερή απόσταση
const long MinRightDistance = 20; // Ελάχιστη επιτρεπόμενη απόσταση
const long MinFrontDistance = 15; // Ελάχιστη επιτρεπόμενη απόσταση
const int SamplesAmount = 15; // περισσότερα δείγματα - ομαλότερη μέτρηση και μεγαλύτερη καθυστέρηση
const int SonarDisplayFrequency = 10; // εμφάνιση μόνο μιας από αυτές τις γραμμές - όχι όλες
int sonarDisplayFrequencyCount = 0;
const μακρύς παράγοντας = 2,54 / 2;
μακρά δείγματα [μέγεθοςof (sonarPins)] [SamplesAmount];
int δείγμαIndex [sizeof (sonarPins)];

// δεξιά πλευρά
const int pinRightMotorDirection = 4; // αυτό μπορεί να επισημανθεί στην ασπίδα του κινητήρα ως "DIR A"
const int pinRightMotorSpeed ​​= 3; // αυτό μπορεί να επισημανθεί στην ασπίδα του κινητήρα ως "PWM A"
const int pinRightBumper = 2; // όπου συνδέεται ο σωστός προφυλακτήρας

// αριστερή πλευρά
const int pinLeftMotorDirection = 7; // αυτό μπορεί να επισημανθεί στην ασπίδα του κινητήρα ως "DIR B"
const int pinLeftMotorSpeed ​​= 6; // αυτό μπορεί να επισημανθεί στην ασπίδα του κινητήρα ως "PWM B"
const int pinLeftBumper = 8; // όπου συνδέεται ο σωστός προφυλακτήρας

// αποσυνδέστε τις επόμενες 2 γραμμές αν το Motor Shield έχει σπάσει
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // αυτό μπορεί να επισημανθεί στην ασπίδα του κινητήρα ως "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // αυτό μπορεί να επισημανθεί στην ασπίδα του κινητήρα ως "BREAKE B"

// πεδία
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// να ρυθμίσει τον μετρητή πόσο καιρό ο κινητήρας λειτουργεί: N / 10 (σε χιλιοστά του δευτερολέπτου)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Αρχικοποίηση
άκυρη ρύθμιση () {
Serial.begin (Baud);
initPins ();

// αποσυνδέστε τις επόμενες 4 γραμμές εάν το Motor Shield έχει σπάσει
// pinMode (pinLeftMotorBreak, OUTPUT).
// pinMode (pinRightMotorBreak, OUTPUT).
// digitalWrite (pinLeftMotorBreak, LOW). // απενεργοποίηση διακοπών
// digitalWrite (pinRightMotorBreak, LOW). // απενεργοποίηση διακοπών

runRightMotorForward ();
runLeftMotorForward ();
startMotors ();
}

// Κύριος βρόχος
void loop () {

verifyAndSetRightSide ();
verifyAndSetLeftSide ();

processRightSide ();
processLeftSide ();

καθυστέρηση (10), // επαναλαμβάνεται κάθε 10 χιλιοστά του δευτερολέπτου
}

//---------------------------------------------------
void initPins () {
pinMode (pinRightMotorDirection, OUTPUT);
pinMode (pinRightMotorSpeed, OUTPUT);
pinMode (pinRightBumper, είσοδος);
pinMode (pinLeftMotorDirection, OUTPUT);
pinMode (pinLeftMotorSpeed, OUTPUT);
pinMode (pinLeftBumper, είσοδος);
για το (int i = 0, i <μέγεθοςof (sonarPins), i ++)
pinMode (sonarPins [i], είσοδος);
}

void startMotors () {
setMotorSpeed ​​(pinRightMotorSpeed, 255);
setMotorSpeed ​​(pinLeftMotorSpeed, 255);
}

void waitWhileAnyBumperIsPressed () {
ενώ (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
καθυστέρηση (20), // ελέγξτε κάθε 20 χιλιοστά του δευτερολέπτου
}
}

void processRightSide () {
αν (countDownWhileMovingToRight MinFrontDistance) // ελέγχει εάν δεν επιτυγχάνεται η ελάχιστη επιτρεπόμενη εμπρόσθια απόσταση
επιστροφή
αν (checkCounterIsNotSet (countDownWhileMovingToLeft)) // αν ο μετρητής δεν μετράει ακόμα
runLeftMotorBackward (); // εκτελέστε το σωστό μοτέρ προς τα πίσω
countDownWhileMovingToLeft = turnLeftTimeout; // ρυθμίστε τον μετρητή στη μέγιστη τιμή για να ξεκινήσετε την καταμέτρηση προς τα κάτω
}

bool checkCounterIsNotSet (int μετρητής) {
μετρητής επιστροφής = ΔείγματαΑποσύνδεσης)
δείγμαIndex [pinIndex] = 0;
δείγματα [pinIndex] [δείγμαIndex [pinIndex]] = τιμή;
επιστροφή true;
}

μακρύς υπολογισμόςAvarageDistance (int pinIndex) {
μακρύς μέσος όρος = 0.
για (int i = 0, i μέσος όρος + = δείγματα [pinIndex] [i];
μέσος όρος επιστροφής / δείγματαΑσυνδυασμός.
}