קורס סיום פרויקט

2013 בינואר 23

עניינים תוכן

1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . הקדמה 11 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . הרובוט מבנה 23 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . הרובוט של בסיסית להפעלה תכנית 35 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . דרישות 46 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . נספחים 5

הקדמה 1

למימוש דוגמא מהווה גם הפרויקט ומיקרו־בקרים. מכטרוניקה של בנושאים בקורס שנלמד הידע של יישום מאפשר הפרויקטנייד לרובוט בקרה אלגוריתם ונממש נתכנן בפרויקט מיקרובקר). (ע"י מחשב משובצת במערכת סגור בחוג בקרה מערכת

זו. למטרה במיוחד שפותח(בצבע חלק משטח על שחור) (בצבע קו ע"י המסומן מסלול אחרי שיעקוב כך לרובוט תכנית לכתוב היא בפרויקט הדרישהקופסת של לזה דומה יהיה מכשול כל (גודל ולעקוף לזהות צריך הרובוט אשר מכשולים ימוקמו המסלול גבי על כן, כמו לבן).

הבא: בקישור המוצג בסרט הרובוט של הרצויה לפעולה מדוגמא להתרשם ניתן שימורים).

https://www.youtube.com/watch?v=Q_qDNzBf_n0

מערכת תכנון של התאורטיים בצדדים עוסק אינו זה פרויקט אך סגור, בחוג בקרה מערכת היא הדרושה הבקרה מערכתאחת רק הוא PID (בקר מתאים פתרון הוא שהוצגה הדרישה את המאפשר סגור) (חוג בקרה לחוג רעיון כל לכן, הבקרה.לינארי, לא מודל הוא במרחב שלו המקום את המתאר הרובוט מודל כללי, באופן ברובוט). לממש שניתן האפשרויות מתוך

מודל. מבוססת בקרה לתכנון דרישה זו בעבודה אין לכן

הרובוט מבנה 2

.1 באיור שמוצג עכפי לתאר ניתן הרובוט מצב את

u1

y

x

θ

(x, y)

הקינמאטי המודל תרשים :1 איור

1

במהירות הגלגלים משני אחד כל סיבוב ע"י מתאפשר הרובוט תמרון גלגל. מסובב מנוע כל חשמליים, מנועים שני לרובוטלבין ω2 ו־ ω1 הגלגלים, של הסיבוב מהירויות בין הקשר את קובעות מטה המוצגות הקינמאטיות הנוסחאות שונה. סיבוב

הרובוט. של θ הזוויתית והמהירות u1 הקווית }המהירותu1 = R

2 (ω1 + ω2)

θ = Ra (ω1 − ω2)

הגלגלים. בין המרחק הוא a ו־ הגלגל רדיוס הוא R כאשר(הניתן מתח מייצב כולל הרובוט השאר, בין .1 ובטבלה באיורים שמפורט כפי אלקטרונים רכיבים מספר כולל הרובוטמרחק וחיישן אור החזר חיישני שמונה בעל סרגל למנועים, ערוצים שני עם דרייבר סוללות), ע"י או חיצוני ספק ע"י להזנה

אישור. ללא מהרובוט רכיב כל או אותה להסיר ואין קבועה היא הרובוט על שמורכבת הפיתוח ערכת אופטי.

הרובוט של תמונות :2 איור

Component Name: הרכיב: תיאור #

STM32VLDISCOVERY הפיתוח ערכת 1TB6612FNG Dual Motor Driver Carrier מנועים דרייבר 2Breadboard Power Supply 5V/3.3V מתח מיצב 3

מתח כניסת בורר 46-12V input חיצוני מתח לכניסת שקע 54x AA 1.5V batteries בטריות בית 6QTR-8RC Re�ectance Sensor Array אופטיים אור החזר חיישני סרגל 7SHARP GP2Y0A21(10-80cm) or GP2D120(4-30cm) אופטי מרחק חיישן 8Tamiya 70097 Twin-Motor Gearbox Kit DC+תמסורות מנועי זוג 9

תמיכה גלגל 10

ברובוט השונים הרכיבים תיאור :1 טבלה

2

USBה־ חיבור דרך או חיצוני מספק מתח מקבלים הפיתוח וערכת שהחיישנים כך בנויה המתחים הספקת ברובוט: מתחיםבתמונה) 4 (מספר המתח כניסת בורר מהסוללות). (או בלבד החיצוני מהספק מתח מקבלים והמנועים זמנית) בו שניהם (אוכולל המתח מיצב נמצא עליו האדום הלוח .(1.5V AA סוללות 4) מסוללות או חיצוני מספק היא ההזנה האם לקבוע מאפשרוגם השונים והחיישנים הערכה של הפעלה מאפשר (USB ה־ (דרך למחשב הפיתוח ערכת חיבור קטן. הפעלה מפסק הוא גםחיצוני ספק עם ועובדים במידה סוללות). או חיצוני לספק חיבור ללא יפעלו לא (המנועים ריצה בזמן DEBUGGERב־ שימוש

המתח. כניסת בורר ליד שנמצא לשקע אותו לחבר יש

עמדה בכל שמצוי המתח לספק הרובוט לחיבור מ' 4 באורך חשמלי כבל יש עמדה לכל חיצוני: מתח למקור הרובוט חיבורלרובוט. הספק חיבור לפני 7Vל־ לכוון יש המתח ספק את .3 באיור מתואר ולרובוט המתח לספק החיבור אופן במעבדה.להארכת מ' 2 באורך בכבלים להשתמש ניתן הצורך, במידת המעבדה. לטכנאי או למדריך לפנות יש זה בנושא נוספות לשאלות

במעבדה. נמצאים אשר USB

בדרייבר שימוש בעזרת אפשרית במנועים שליטה ותמסורת. DC מנועי מזוג מורכבת ברובוט ההינע מערכת המנועים: הפעלתעל מידע לקבלת במנועים. השליטה מתאפשרת וכך הפיתוח מערכת PWM ה־ אות את מקבל הדרייבר אליהם. שמחובר

בנספחים. המתאימה לספרות לפנות יש הדרייבר הפעלת

ופוטו־טרנזיסטור אדום) אינפרה LED) אור פולטת דיודה של מצמד מורכבים האור החזר חיישני אור: החזר חיישני סרגלהמסלול לאיתור ומיועדים הרובוט בתחתית מורכבים והסרגלים חיישנים שמונה ממוקמים הסרגל על האור. החזר לקריאתשמוחזרת האור לכמות בהתאם וולט 3 עד 0 בטווח הם מחזירים שהחיישנים המתח רמות אחריו. לעקוב נדרש שהרובוט

הנמדד. מהמשטח

Position) PSD אדום), (אינפרה אור פולטת דיודה של מקומבינציה מורכב Sharp חברת של החיישן מרחק: למדידת חיישןמוליך משטח גבי על אור נקודת של מיקום למדוד מסוגל אשר התקן הוא PSD אותות. לעיבוד ומעגל (Sensitive Detectorלאחר מתקבלת המדידה יחודית. עדשה דרך PSD להתקן מוחזר אשר אור פולטת שהדיודה הוא הפעולה עיקרון למחצה.ו־ 4cm במרחק מכשול עבור 3V הם זה מחיישן המתקבלים הערכים .PSD ה־ התקן דרך הזרמים קריאת של אותות עיבודרובוט). כל רשום המדידה טווח דומה. באופן ס"מ 80 ־ 10 בטווח פועלים מהחיישנים (חלק 30cm במרחק מכשול עבור 0.3V

הבא: הקשר לפי החיישן מדידת את לכייל ניתן

distance =A

measure−B

אמפיריים. קבועים הם A,B ו־ המדידה ערך הוא measure המרחק, הוא distance כאשר

חשובה: הערההנכונה באוריינטציה אותם להרכיב מאוד וחשוב קלה לשליפה ניתנים השונים האלקטרונים הרכיביםאת לפרק איסור חל לכן, המשתמש. את ולסכן לערכה נזק לגרום שעלולים חשמליים קצרים למניעתהמעבדה. טכנאי או למדריך פנו אנא ממקומו השתחרר הרכיבים ואחד במידה השונים. הרכיבים

הרובוט של בסיסית להפעלה תכנית 3

נכתבה זו תכנית הרובוט. של והחיישנים המנועים להפעלת ספרייה פונקציות שמכילה עזר תכנית למצוא ניתן הקורס באתרהפרויקט. ביישום לסייע נועדה והיא הפרויקט עבור במיוחדבתכנית: קיימות אשר השונות הפונקציות רשימת להלן

:Extra_Built_In_Functions.c בקובץ פונקציות •

void Blue_button_Init( void ) �לוגית. ככניסה הכחול הכפתור את מאתחלת הסבר:

־ נכנס: ערכ־ חוזר: ערך

3

חיצוני מתח לספק הרובוט חיבור :3 איור

void Blue_Led_Init(void) �הלד את ויכבה שיפעיל (Timer 4) מונה ומגדירה לוגית כיציאה הכחול הלד את מאתחלת הסבר:

־ נכנס: ערכ־ חוזר: ערך

void Delay(unsigned long count); �פשוטה) לולאה בעזרת (ממומש הנכנס לערך פרופורציוני לעיקוב שגורמת פונקציה הסבר:

[0,0xFFFFFFFF] התחום: מספר יהיה count הפרמטר נכנס: ערכ־ חוזר: ערך

:Motor.c בקובץ פונקציות •

void Motor_Init(void); �המנוע. דרייבר לתפעול והיציאות הכניסות את מאתחלת הסבר:

־ נכנס: ערכ־ חוזר: ערך

void Motor_Drive(unsigned int MOTORx, int Duty) �יהיו לא המתקבלים והערכים במידה רצויה. ובעוצמה רצוי בכיוון המנועים את מפעילה אשר פונקציה הסבר:

בפונקציה. אינסופית בלולאה תיתקע בתוכנית ריצת המתאים, בטווח[-12000,12000] בתחום מספר יהיה Duty הפרמטר (2 x=1,2 כאשר MOTORx (1 נכנס: ערכ

־ חוזר: ערך

void Motor_Stop(unsigned int MOTORx) �מנוע עצירת הסבר:

x=1,2 כאשר MOTORx נכנס: ערכ־ חוזר: ערך

void Motor_Standby(void) �באנרגיה) (חיסכון Standby למצב ומעבר מנועים עצירת הסבר:

־ נכנס: ערכ־ חוזר: ערך

4

:Sensors.c בקובץ פונקציות •

void Sensor_Reading_Init(void) �רלוונטיים ADC ורכיבי החיישנים כניסות את מאתחלת הסבר:

־ נכנס: ערכ־ חוזר: ערך

struct Sensors Read_Sensors() �את .Sensors מטיפוס נתונים למבנה כולן ואת ומחזירה החיישנים כניסות כל את קוראת אשר פונקציה הסבר:ואת int מטיפוס תאים 8 בעל array_sensor מערך לשדה תחזיר הפונקציה החיישנים מסרגל הקריאות שמונת

distance לשדה המרחק חיישן קריאת־ נכנס: ערכ

:Sensors מסוג נתונים מבנה חוזר: ערך

ערך קריאת ופונקצית וולט 3 עד 0 בתחום אנלוגיות מתח רמות מחזירים השונים החיישנים כי לב לשים ישלמשתנה המדידה את להמיר מאפשר שלנו במיקרובקר ADC רכיב אלה. לערכים ADC המרת מבצעת החיישנים

.(4095=3V) 4095 עד 0 בתחום ערכים יהיו בתוכנה החיישנים קריאות ולכן, ביט 12 באורך

הדוגמא: תכנית של הפעולה אופן

לתיקייה להעתיק יש התכנית תיקיית את הקורס. אתר של הידע במאגר רלוונטי בקובץ נמצאת הדוגמא תכניתכה. עד התכניות כל הופעלו שממנה stm32vldiscovery_LAB_DIR\projects

לסמן כדי יפעל הירוק הלד האתחול לאחר כחול. וכפתור כחול לד מנועים, החיישנים, אתחול בפעולות מתחילה התכניתהכפתור על לחיצה לאחר הכחול. הכפתור ללחיצת ממתינה שהתוכנית לסמן מנת על יהבהב הכחול והלד פועל המנוע שדרייברבמקום. ימינה וסיבוב במקום שמאלה סיבוב אחורה, נסיעה קדימה, נסיעה הבאים: התמרונים בביצוע תתחיל התוכנית הכחוללמשתנה החיישנים קריאות את ומעבירה (standby) השהייה למצב המנועים את מעבירה התוכנית התמרונים רצף סיום לאחר

אינסופית. בלולאה m0בשינויים ולצפות אמת בזמן התוכנית את לדבג יאפשר discovery בערכת usbה־ ממשק בעזרת למחשב הרובוט חיבור

.Live Watch בחלון אמת בזמן החיישנים בקריאות

דרישות 4

לתכנן יש בנוסף, שרירותי. באופן משתנה עקמומיות בעל מסלול אחרי לעקיבה בקרה אלגוריתם לממש היא הפרויקט מטרתבאופן שימוקמו סטטיים עצמים יהיו המכשולים אותם. ולעקוף המסלול גבי על מכשולים לזהות שיתאפשר כך המערכת אתכל (רוחב הרובוט בחזית שמותקן המרחק חיישן באמצעות זיהוי שיאפשרו כאלה יהיו ומידותיהם המסלול גבי על שרירותי

שחור). בצבע יהיו המכשולים שימורים, קופסת של לרוחב דומה יהיה מכשול

5

דגשים:

לפתור מנת על הרובוט. על שמורכבים בחיישנים קיימת זו בעיה מדידה. רעש היא חיישנים עם בעבודה נפוצה בעיה .1בדידה במערכת LPF למימוש הפשוטות הדרכים אחת .low-pass �lter בעזרת המדידה רעשי את לסנן יש הבעיה את

הבאה: הנוסחה לפי היא

Y [n] = αX [n] + (1− α)Y [n− 1]

זה בנושא נוסף למידע המסוננים. הערכים הם Y [n]ו־ המדידה ערכי הם X [n] המסנן, קבוע הוא 0 ≤ α ≤ 1 כאשרבנספחים. מתאים לקישור לפנות ניתן

בתכנית בלולאה ולא ופסיקות בטיימרים שימוש בעזרת הבקרה אות חישוב ואת החיישנים קריאות את לתזמן יש .2הראשית.

אחרים! במונים להשתמש יש ולכן הדוגמה בתכנית בשימוש כבר 4 ו־ 3 טיימרים הערה:

נספחים 5

Low-Pass Filter:

http://en.wikipedia.org/wiki/Low-pass_filter

Proportional�Integral�Derivative controller (PID controller):

http://en.wikipedia.org/wiki/PID_controller

Breadboard Power Supply 5V/3.3V

http://www.sparkfun.com/products/114

QTR-8RC Re�ectance Sensor Array:

http://www.pololu.com/catalog/product/960

TB6612FNG Dual Motor Driver Carrier:

http://www.pololu.com/catalog/product/713

Tamiya 70097 Twin-Motor Gearbox Kit:

http://www.pololu.com/catalog/product/61

Sharp Distance Sensor GP2D120 (4-30cm):

http://www.pololu.com/catalog/product/1136

Sharp Distance Sensor GP2Y0A21 (10-80cm):

http://www.pololu.com/catalog/product/136

6

חשמליים: חיבורים

Pinnumber(P1)

Boadpin

name

Function Target Name Target pin

18 PA3 ADC1_IN3 Sharp 2D120X Vout19 PA4 ADC1_IN4 QTR-8RC (front) Sensor output 820 PA5 ADC1_IN5 QTR-8RC (front) Sensor output 721 PA6 ADC1_IN6 QTR-8RC (front) Sensor output 622 PA7 ADC1_IN7 QTR-8RC (front) Sensor output 523 PC4 ADC1_IN14 QTR-8RC (front) Sensor output 424 PC5 ADC1_IN15 QTR-8RC (front) Sensor output 325 PB0 ADC1_IN8 QTR-8RC (front) Sensor output 226 PB1 ADC1_IN9 QTR-8RC (front) Sensor output 1

הפיתוח ערכת בלוח P1 חלקה של חיבורים :2 טבלה

Pinnumber(P2)

Boadpin

name

Function Target Name Target pin

2 PC6 TIM3_CH1 TB6612FNG Dual Motor Driver Carrier PWMA3 PC7 TIM3_CH2 TB6612FNG Dual Motor Driver Carrier PWMB4 PC8 ־ ־ -5 PC9 Digital output TB6612FNG Dual Motor Driver Carrier

+ green ledSTBY

6 PA8 Digital output TB6612FNG Dual Motor Driver Carrier AIN17 PA9 Digital output TB6612FNG Dual Motor Driver Carrier AIN28 PA10 Digital output TB6612FNG Dual Motor Driver Carrier BIN19 PA11 Digital output TB6612FNG Dual Motor Driver Carrier BIN2

הפיתוח ערכת בלוח P2 חלקה של חיבורים :3 טבלה

7

ברובוט: המתחים תשתית •

.LEDON ל־ וגם Vcc ל־ 3V מתח דרוש החיישנים סרגל להפעלת •

.5V דרוש המרחק חיישן להפעלת •

8