Self Balancing Robot From Magicbit: 6 βήματα

2024 Συγγραφέας: John Day | [email protected]. Τελευταία τροποποίηση: 2024-01-30 08:31

Αυτό το σεμινάριο δείχνει πώς να φτιάξετε ένα αυτορυθμιζόμενο ρομπότ χρησιμοποιώντας τον πίνακα Magicbit dev. Χρησιμοποιούμε το magicbit ως πίνακα ανάπτυξης σε αυτό το έργο που βασίζεται στο ESP32. Επομένως, οποιοσδήποτε πίνακας ανάπτυξης ESP32 μπορεί να χρησιμοποιηθεί σε αυτό το έργο.

Προμήθειες:

• μάγκιμπιτ
• Διπλός οδηγός μοτέρ H-bridge L298
• Γραμμικός ρυθμιστής (7805)
• Μπαταρία Lipo 7.4V 700mah
• Μονάδα αδρανειακής μέτρησης (IMU) (6 βαθμοί ελευθερίας)
• κιβώτια ταχυτήτων 3V-6V DC

Βήμα 1: Ιστορία

Ρε παιδιά, σήμερα σε αυτό το σεμινάριο θα μάθουμε για λίγο περίπλοκα πράγματα. Πρόκειται για ρομπότ αυτο -εξισορρόπησης που χρησιμοποιεί το Magicbit με το Arduino IDE. Ας ξεκινήσουμε λοιπόν.

Πρώτα απ 'όλα, ας δούμε τι είναι αυτόματο ρομπότ. Το αυτορυθμιζόμενο ρομπότ είναι ρομπότ με δύο τροχούς. Το ιδιαίτερο χαρακτηριστικό είναι ότι το ρομπότ μπορεί να εξισορροπηθεί χωρίς να χρησιμοποιήσει εξωτερική υποστήριξη. Όταν η τροφοδοσία είναι ενεργοποιημένη, το ρομπότ θα σηκωθεί και θα ισορροπεί συνεχώς χρησιμοποιώντας κινήσεις ταλάντωσης. Οπότε τώρα έχετε όλη τη γενική ιδέα για το αυτορυθμιζόμενο ρομπότ.

Βήμα 2: Θεωρία και Μεθοδολογία

Για να εξισορροπήσουμε το ρομπότ, πρώτα λαμβάνουμε δεδομένα από κάποιον αισθητήρα για τη μέτρηση της γωνίας του ρομπότ σε κάθετο επίπεδο. Για το σκοπό αυτό χρησιμοποιήσαμε MPU6050. Αφού λάβουμε τα δεδομένα από τον αισθητήρα υπολογίζουμε την κλίση στο κάθετο επίπεδο. Εάν το ρομπότ βρίσκεται σε ευθεία και ισορροπημένη θέση, τότε η γωνία κλίσης είναι μηδενική. Εάν όχι, τότε η γωνία κλίσης είναι θετική ή αρνητική τιμή. Εάν το ρομπότ έχει κλίση στην μπροστινή πλευρά, τότε το ρομπότ θα πρέπει να μετακινηθεί στην μπροστινή κατεύθυνση. Επίσης, εάν το ρομπότ έχει κλίση προς την αντίστροφη πλευρά, τότε το ρομπότ θα πρέπει να κινηθεί προς την αντίστροφη κατεύθυνση. Εάν αυτή η γωνία κλίσης είναι υψηλή, τότε η ταχύτητα απόκρισης πρέπει να είναι υψηλή. Αντίστροφα, η γωνία κλίσης είναι χαμηλή, τότε η ταχύτητα αντίδρασης πρέπει να είναι χαμηλή. Για τον έλεγχο αυτής της διαδικασίας χρησιμοποιήσαμε συγκεκριμένο θεώρημα που ονομάζεται PID. Το PID είναι σύστημα ελέγχου που χρησιμοποιείται για τον έλεγχο πολλών διαδικασιών. Το PID σημαίνει 3 διαδικασίες.

• Ρ- αναλογική
• I- αναπόσπαστο
• Δ- παράγωγο

Κάθε σύστημα έχει είσοδο και έξοδο. Με τον ίδιο τρόπο αυτό το σύστημα ελέγχου έχει επίσης κάποια είσοδο. Σε αυτό το σύστημα ελέγχου είναι η απόκλιση από τη σταθερή κατάσταση. Το ονομάσαμε λάθος. Στο ρομπότ μας, το σφάλμα είναι η γωνία κλίσης από κάθετο επίπεδο. Εάν το ρομπότ είναι ισορροπημένο, τότε η γωνία κλίσης είναι μηδενική. Έτσι, η τιμή σφάλματος θα είναι μηδέν. Ως εκ τούτου, η έξοδος του συστήματος PID είναι μηδενική. Αυτό το σύστημα περιλαμβάνει τρεις ξεχωριστές μαθηματικές διαδικασίες.

Το πρώτο είναι πολλαπλασιασμός σφάλματος από αριθμητικό κέρδος. Αυτό το κέρδος συνήθως ονομάζεται Kp

P = σφάλμα*Kp

Το δεύτερο είναι να δημιουργήσει το ολοκλήρωμα του σφάλματος στον τομέα χρόνου και να το πολλαπλασιάσει από κάποιο κέρδος. Αυτό το κέρδος ονομάζεται Ki

I = Ολοκληρωτικό (σφάλμα)*Ki

Το τρίτο είναι παράγωγο του σφάλματος στον τομέα χρόνου και πολλαπλασιάστε το με κάποιο κέρδος. Αυτό το κέρδος ονομάζεται Kd

D = (d (σφάλμα)/dt)*kd

Μετά την προσθήκη των παραπάνω λειτουργιών λαμβάνουμε την τελική μας έξοδο

ΕΞΟΔΟΣ = Ρ+Ι+Δ

Λόγω του τμήματος Ρ το ρομπότ μπορεί να πάρει σταθερή θέση η οποία είναι ανάλογη με την απόκλιση. Το μέρος υπολογίζει την περιοχή σφάλματος έναντι του γραφήματος χρόνου. Έτσι προσπαθεί να φέρει το ρομπότ σε σταθερή θέση πάντα με ακρίβεια. Το τμήμα D μετρά την κλίση στο χρόνο έναντι του γραφήματος σφάλματος. Εάν το σφάλμα αυξάνεται, αυτή η τιμή είναι θετική. Εάν το σφάλμα μειώνεται, αυτή η τιμή είναι αρνητική. Εξαιτίας αυτού, όταν το ρομπότ κινείται σε σταθερή θέση, τότε η ταχύτητα αντίδρασης θα μειωθεί και αυτό θα βοηθήσει στην απομάκρυνση των περιττών υπερβάσεων. Μπορείτε να μάθετε περισσότερα για τη θεωρία PID από αυτόν τον σύνδεσμο που φαίνεται παρακάτω.

www.arrow.com/en/research-and-events/articles/pid-controller-basics-and-tutorial-pid-implementation-in-arduino

Η έξοδος της συνάρτησης PID περιορίζεται στο εύρος 0-255 (ανάλυση PWM 8 bit) και θα τροφοδοτείται με κινητήρες ως σήμα PWM.

Βήμα 3: Ρύθμιση υλικού

Τώρα αυτό είναι μέρος εγκατάστασης υλικού. Ο σχεδιασμός του ρομπότ εξαρτάται από εσάς. Όταν σχεδιάσατε το σώμα του ρομπότ, πρέπει να το θεωρήσετε συμμετρικό ως προς τον κάθετο άξονα που βρίσκεται στον άξονα του κινητήρα. Η μπαταρία βρίσκεται παρακάτω. Ως εκ τούτου, το ρομπότ είναι εύκολο να ισορροπήσει. Στο σχεδιασμό μας στερεώνουμε τον πίνακα Magicbit κάθετα στο σώμα. Χρησιμοποιήσαμε δύο κινητήρες μετάδοσης 12V. Αλλά μπορείτε να χρησιμοποιήσετε οποιοδήποτε είδος κινητήρα ταχυτήτων. εξαρτάται από τις διαστάσεις του ρομπότ σας.

Όταν συζητάμε για κύκλωμα, τροφοδοτείται από μπαταρία Lipo 7.4V. Το Magicbit χρησιμοποίησε 5V για τροφοδοσία. Ως εκ τούτου, χρησιμοποιήσαμε ρυθμιστή 7805 για τη ρύθμιση της τάσης της μπαταρίας στα 5V. Σε μεταγενέστερες εκδόσεις του Magicbit, αυτός ο ρυθμιστής δεν χρειάζεται. Επειδή τροφοδοτεί έως και 12V. Παρέχουμε απευθείας 7,4V για οδηγό κινητήρα.

Συνδέστε όλα τα εξαρτήματα σύμφωνα με το παρακάτω διάγραμμα.

Βήμα 4: Ρύθμιση λογισμικού

Στον κώδικα χρησιμοποιήσαμε τη βιβλιοθήκη PID για τον υπολογισμό της εξόδου PID.

Μεταβείτε στον παρακάτω σύνδεσμο για να το κατεβάσετε.

www.arduinolibraries.info/libraries/pid

Κατεβάστε την τελευταία έκδοση του.

Για καλύτερη ανάγνωση αισθητήρων χρησιμοποιήσαμε βιβλιοθήκη DMP. Το DMP σημαίνει διαδικασία ψηφιακής κίνησης. Αυτό είναι ενσωματωμένο χαρακτηριστικό του MPU6050. Αυτό το τσιπ έχει ενσωματωμένη μονάδα διαδικασίας κίνησης. Χρειάζεται λοιπόν ανάγνωση και ανάλυση. Αφού δημιουργήσει αθόρυβες ακριβείς εξόδους στον μικροελεγκτή (σε αυτήν την περίπτωση Magicbit (ESP32)). Υπάρχουν όμως πολλές εργασίες στην πλευρά του μικροελεγκτή για να ληφθούν αυτές οι ενδείξεις και να υπολογιστεί η γωνία. Έτσι, απλά χρησιμοποιήσαμε τη βιβλιοθήκη MPU6050 DMP. Κατεβάστε το goint στον παρακάτω σύνδεσμο.

github.com/ElectronicCats/mpu6050

Για να εγκαταστήσετε τις βιβλιοθήκες, στο μενού Arduino μεταβείτε στα εργαλεία-> περιλαμβάνουν βιβλιοθήκη-> βιβλιοθήκη add.zip και επιλέξτε το αρχείο βιβλιοθήκης που κατεβάσατε.

Στον κώδικα πρέπει να αλλάξετε σωστά τη γωνία σημείου ρύθμισης. Οι σταθερές τιμές PID διαφέρουν από ρομπότ σε ρομπότ. Έτσι, για να το συντονίσετε, πρώτα ορίστε τιμές Ki και Kd μηδέν και, στη συνέχεια, αυξήστε την Kp μέχρι να έχετε καλύτερη ταχύτητα αντίδρασης. Περισσότερα Kp προκαλούν περισσότερες υπερβάσεις. Στη συνέχεια, αυξήστε την τιμή Kd. Αυξήστε το πάντα σε πολύ μικρή ποσότητα. Αυτή η τιμή είναι γενικά χαμηλή από άλλες τιμές. Τώρα αυξήστε το Ki μέχρι να έχετε πολύ καλή σταθερότητα.

Επιλέξτε τη σωστή θύρα COM και πληκτρολογήστε τον πίνακα. ανεβάστε τον κωδικό. Τώρα μπορείτε να παίξετε με το DIY ρομπότ σας.

Βήμα 5: Διαγράμματα

Βήμα 6: Κωδικός

#περιλαμβάνω

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 mpu; bool dmpReady = false; // ορίστε true αν το DMP init ήταν επιτυχές uint8_t mpuIntStatus. // κατέχει πραγματικό byte κατάστασης διακοπής από MPU uint8_t devStatus. // επιστροφή κατάστασης μετά από κάθε λειτουργία συσκευής (0 = επιτυχία,! 0 = σφάλμα) uint16_t packetSize; // αναμενόμενο μέγεθος πακέτου DMP (προεπιλογή είναι 42 byte) uint16_t fifoCount; // αρίθμηση όλων των byte που υπάρχουν στο FIFO uint8_t fifoBuffer [64]; // Ρυθμιστικό αποθήκευσης FIFO Quaternion q; // [w, x, y, z] quaternion container VectorFloat gravity; // [x, y, z] βαρύτητας διάνυσμα float ypr [3]; // [yaw, pitch, roll] yaw/pitch/roll container και gravity vector double originalSetpoint = 172.5; διπλό σημείο ρύθμισης = originalSetpoint; διπλό κινούμενοAngleOffset = 0,1; διπλή είσοδος, έξοδος. int moveState = 0; διπλό Kp = 23; // σύνολο P πρώτο διπλό Kd = 0.8; // αυτή η τιμή γενικά μικρή διπλή Ki = 300; // αυτή η τιμή θα πρέπει να είναι υψηλή για καλύτερη σταθερότητα PID pid (& είσοδος, & έξοδος, & σημείο ρύθμισης, Kp, Ki, Kd, DIRECT); // pid αρχικοποίηση int motL1 = 26; // 4 ακίδες για κίνηση κινητήρα int motL2 = 2; int motR1 = 27; int motR2 = 4; πτητικό bool mpuInterrupt = false; // υποδεικνύει εάν ο πείρος διακοπής MPU έχει πάει πολύ void dmpDataReady () {mpuInterrupt = true; } void setup () {ledcSetup (0, 20000, 8); // pwm setup ledcSetup (1, 20000, 8); ledcSetup (2, 20000, 8); ledcSetup (3, 20000, 8); ledcAttachPin (motL1, 0); // pinmode των κινητήρων ledcAttachPin (motL2, 1); ledcAttachPin (motR1, 2); ledcAttachPin (motR2, 3); // συμμετοχή στο δίαυλο I2C (η βιβλιοθήκη I2Cdev δεν το κάνει αυτόματα) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin (); Wire.setClock (400000); // ρολόι I2C 400kHz. Σχολιάστε αυτήν τη γραμμή εάν αντιμετωπίζετε δυσκολίες στη σύνταξη #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire:: setup (400, true); #endif Serial.println (F ("Αρχικοποίηση συσκευών I2C …")); pinMode (14, ΕΙΣΟΔΟΣ); // αρχικοποίηση σειριακής επικοινωνίας // (115200 επιλέχθηκε επειδή απαιτείται για την έξοδο Teapot Demo, αλλά είναι // εξαρτάται από εσάς ανάλογα με το έργο σας) Serial.begin (9600); ενώ (! Σειριακό)? // περιμένετε την απαρίθμηση του Λεονάρντο, άλλοι συνεχίζουν αμέσως // αρχικοποιούν τη συσκευή Serial.println (F ("Αρχικοποίηση συσκευών I2C …")); mpu.initialize (); // επαλήθευση σύνδεσης Serial.println (F ("Δοκιμές συνδέσεων συσκευής …")); Serial.println (mpu.testConnection ()? F ("Η σύνδεση MPU6050 είναι επιτυχής"): F ("Η σύνδεση MPU6050 απέτυχε")); // φορτώστε και διαμορφώστε το DMP Serial.println (F ("Initializing DMP …")); devStatus = mpu.dmpInitialize (); // προμηθεύστε τις δικές σας αντισταθμίσεις γύρου εδώ, κλιμακωμένες για ελάχιστη ευαισθησία mpu.setXGyroOffset (220). mpu.setYGyroOffset (76); mpu.setZGyroOffset (-85); mpu.setZAccelOffset (1788); // 1688 εργοστασιακή προεπιλογή για το δοκιμαστικό μου τσιπ // βεβαιωθείτε ότι λειτούργησε (επιστρέφει 0 αν ναι) εάν (devStatus == 0) {// ενεργοποιήσετε το DMP, τώρα που είναι έτοιμο Serial.println (F ("Ενεργοποίηση DMP … ")); mpu.setDMPEnabled (true); // ενεργοποίηση εντοπισμού διακοπής Arduino Serial.println (F ("Ενεργοποίηση ανίχνευσης διακοπών (εξωτερική διακοπή Arduino 0) …")); attachInterrupt (14, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus (); // ρυθμίστε τη σημαία DMP Ready έτσι ώστε η κύρια λειτουργία βρόχου () να γνωρίζει ότι είναι εντάξει να τη χρησιμοποιήσετε Serial.println (F ("DMP έτοιμο! Περιμένουμε την πρώτη διακοπή …")); dmpReady = true; // λάβετε το αναμενόμενο μέγεθος πακέτου DMP για μεταγενέστερη σύγκριση packetSize = mpu.dmpGetFIFOPacketSize (); // setup PID pid. SetMode (AUTOMATIC); pid. SetSampleTime (10); pid. SetOutputLimits (-255, 255); } else {// ERROR! // 1 = η αρχική φόρτωση μνήμης απέτυχε // 2 = Οι ενημερώσεις διαμόρφωσης DMP απέτυχαν // (εάν πρόκειται να σπάσει, συνήθως ο κωδικός θα είναι 1) Serial.print (F ("DMP Initialization failed (code")); Serial. εκτύπωση (devStatus); Serial.println (F (")")); }} void loop () {// εάν ο προγραμματισμός απέτυχε, μην προσπαθήσετε να κάνετε τίποτα εάν (! dmpReady) επιστρέψει. // περιμένετε για διακοπή MPU ή επιπλέον πακέτα (α) διαθέσιμα ενώ (! mpuInterrupt && fifoCount <packetSize) {pid. Compute (); // αυτή η χρονική περίοδος χρησιμοποιείται για τη φόρτωση δεδομένων, ώστε να μπορείτε να το χρησιμοποιήσετε για άλλους υπολογισμούς motorSpeed (παραγωγή); } // επαναφορά σημαίας διακοπής και λήψη INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus (); // λάβετε τον τρέχοντα αριθμό FIFO fifoCount = mpu.getFIFOCount (); // έλεγχος για υπερχείλιση (αυτό δεν πρέπει ποτέ να συμβεί εκτός εάν ο κωδικός μας είναι πολύ αναποτελεσματικός) εάν ((mpuIntStatus & 0x10) || fifoCount == 1024) {// μηδενίσει, ώστε να συνεχίσουμε καθαρά mpu.resetFIFO (); Serial.println (F ("FIFO overflow!"); // διαφορετικά, ελέγξτε για διακοπή δεδομένων DMP έτοιμη (αυτό πρέπει να συμβαίνει συχνά)} αλλιώς εάν (mpuIntStatus & 0x02) {// περιμένετε για το σωστό διαθέσιμο μήκος δεδομένων, θα πρέπει να είναι πολύ σύντομη αναμονή ((fifoCount 1 πακέτο διαθέσιμο // (αυτό μας επιτρέπει να διαβάσουμε αμέσως περισσότερα χωρίς να περιμένουμε διακοπή) εκτύπωση ("ypr / t"); Serial.print (ypr [0] * 180/M_PI); // euler angles Serial.print ("\ t"); Serial.print (ypr [1] * 180/M_PI); Serial.print ("\ t"); Serial.println (ypr [2] * 180/M_PI); #endif input = ypr [1] * 180/M_PI + 180;}} void motorSpeed (int PWM) {float L1, L2, R1, R2; εάν (PWM> = 0) {// κατεύθυνση προς τα εμπρός L2 = 0; L1 = abs (PWM); R2 = 0; R1 = abs (PWM); εάν (L1> = 255) { L1 = R1 = 255;}} άλλο {// κατεύθυνση προς τα πίσω L1 = 0; L2 = abs (PWM); R1 = 0; R2 = abs (PWM); εάν (L2> = 255) {L2 = R2 = 255; }} // οδήγηση κινητήρα ledcWrite (0, L1); ledcWrite (1, L2); ledcWrite (2, R1*0.97); // 0.97 είναι γεγονός ταχύτητας ή, επειδή ο δεξιός κινητήρας έχει υψηλή ταχύτητα από τον αριστερό κινητήρα, οπότε τον μειώνουμε έως ότου οι ταχύτητες του κινητήρα είναι ίσες ledcWrite (3, R2*0,97).

}