פרויקט גמר חלק 1

פרויקט רכב אוטונומי מהיר לנסיעה באזורים אורבניים

צפופים

S.A.M

Smart Autonomous Maneuvering system

פרויקט ברובוטיקה

ומר עמית פריגוזין בהנחיית פרופסור אילון רימון

מגישים:

300424793 - רז צוקרן

200387645 - אורן קנז

301013538 - רן סרוסי

טכניון מכון טכנולוגי לישראל

הפקולטה להנדסת מכונות

פרויקט גמר

2עמוד S.A.M - Smart Autonomous Maneuvering

תוכן עניינים

4 ............................................................................................................................. הקדמה

4 .............................................................................................................................. מטרת הפרויקט

4 ............................................................................................................................... יעדים

5 .................................................................................................................. רכיבי המערכת

5 ................................................................................................................................. רכיבים קיימים

5 ..................................................................................................................................... תוספות תכן

6 ................................................................................................................... יטת הנסיעהש

7 ......................................................................................................... מימוש שיטת הנסיעה

7 ........................................................................................ )שיטה קודמת( –. עקיבה אחר מסלול מסומן 1

8 ................................................................................................... תוצאות ומסקנות משיטה זו

9 .................................................................. שיטת הנסיעה החדשה -. שמירה על מסלול בעזרת קיר ייחוס 2

10 ............................................................................................ יישום שיטת הנסיעה החדשה

10 ......................................................................................... . שימוש בחיישני מרחק מסוג איפרה אדום1

11 ................................................................................................... תוצאות ומסקנות של שיטת יישום זו

14 .................................................................. ימוש בחיישני מרחק מבוססי לייזר וחיישן לייזר מסתובב. ש2

17 .............................................................................תכן שלבי הנסיעה בעזרת חיישני הלייזר

19 .................................................................................................................... בקרת מסלול

21 .............................................................................................................................. דרישות הבקרה

21 ........................................................................................................ סימולציות לקבלת הבקר הנדרש

26 ................................................................................................................. תכן אלקטרוני

28 ................................................................................................................ מנגנוני בטיחות

32 ................................................................................................................... קופסת פיקוד

37 ........................................................................................................................... ניסויים

37 ..........................................................................................................................ניסוי שינוי מהירות

38 .................................................................................................................. ניסויי חיישנים

38 ....................................................................................................................... חיישני אינפרה אדום

42 .................................................................................................................... חיישני לייזר



פרויקט גמר


43 ........................................................................................................................... מקורות

43 .................................................................................................................. פרויקט גמר קודם .1

44 ........................................................................................................................... נספחים

44 ................................................................................................................... . כיול חיישן לייזר שני1

45 ................................................................................................ . כיול חיישן אינפרה אדום ע"י היצרן2

46 .......................................................................................................... דיאגרמת חיבור חיישני לייזר3

ADC ....................................................................................................................... 47.שרטוט לוח 4

48 ................................................................................................ .דיאגרמת חיבורים כרטיסי תקשורת.5

49 ....................................................................................................................... .מודל קופסת פיקוד6

50 .......................................................................................................................................... קודים

ADC ....................... 50חיישני "פס" בעזרת לוח -תקשרות בין מעבדים וקריאת חיישני אינפרה אדום –. קוד 7

55 ........................................................................................................................ . קוד קופסת פיקוד8

62 ................................................................................................................................. . קוד מרכזי9



פרויקט גמר


הקדמה

כגון קניונים כיום ישנו צורך מתגבר לרכבים אוטונומיים מהירים אשר מסוגלים לנווט בשטחים אורבניים צפופים

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

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

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

מטר בשנייה( ע"מ להימנע מהקשיים שבחיזוי תנועת אנשים. 10-הוחלט להשתמש בפלטפורמה מהירה )המגיעה ל

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

מטר לשנייה(. 2-3-כ

ות כה גבוהה יצרה אתגרים רבים כגון בעיות תמרון )בעיקר ברדיוס קטן(, ביצוע עבודה עם כלי רכב הנע במהיר

בלימת חירום בעת הצורך, לוקליזציה ומיפוי של הסביבה וכו'.

מהשלב בו הצטרפנו לצוות המחקר. הפרויקטבדו"ח זה נפרט את התקדמות

מטרת הפרויקט

טפורמה רובוטית אוטונומית מהירה לניווט בסביבה פרויקט זה נעשה במסגרת מחקר רחב אשר עוסק בפיתוח פל

אורבנית צפופה.

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

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

יעדים

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

בניצב לקיר.

.בטיחות מפני תאונותיצירת מנגנוני .2

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



פרויקט גמר


רכיבי המערכת

רכיבים קיימים

פלטפורמת רכב של חברת“thunder tiger” מדגם “ER4-G3“.

מעבדArduino Due .

מעבד Arduino Uno.

אדום לזיהוי מרחק מקיר. המערך חיישני אינפר

.)מקודד אופטי )אנקודר

,חוטים ומחברים(.מייצבי מתח רכיבי חשמל בסיסיים )מטריצת חיווט ,

סוללתLiPo 14.8 וולט

2 .כרטיסי תקשורת

2 חיישניLidar lite לא נמצאים על מערכת הרכב(לזיהוי מרחק מקיר )עדיין

2 יחידות סרבו עליהם מותקנים חיישני הלייזרSAVOX SB-2271SG

יחידת סרבו המשמשת להיגוי– PROTEK RC 160SS

תוספות תכן

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

אדום לקריאת מרחק מקיר התושבות לחיישני אינפר



פרויקט גמר


שיטת הנסיעה

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

ה. מהירות הרכב בכל נקודה במסלול נבחרה תחת אילוצי ימטר לשני 10-הרכב ינוע במסלול במהירות גבוה, עד כ

הבטיחות המוגדרים כך שתמיד ניתן לבצע בלימת חירום בצורה בטוחה.

2ור המהירות בכל נקודה הוא: הביטוי עב maxv a r כאשר ,𝑟 .הינו המרחק מן המכשול

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

9קודם עמ' פרויקט גמר – 1 ראה מקור מספר להרחבה בנושא

כאשר קריאת האנקודר בל קריאה מאנקודר הממוקם על ציר המנוע, המק PIDמהירות הרכב נשלטת ע"י בקר

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

.לשנות את המהירות לפי הצורך

להרחבה בנושא בקר המהירות מסוגPID ת, תכן בקרת מהירו29קודם עמ' פרויקט גמרראה

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

הרכב במסלול.

מאמץ הבקרה המוזן למערכת ינויהמיקום ע"י ש תושגיאתתקן את מערכת הבקרה של הרכב נתונים אלובעזרת

.ההיגוי של הרכב

בקרה אופטימלית למערכת דינמית בשימוש באנרגיה מינימלית. מבטיחה, LQRמערכת הבקרה מנוהלת ע"י בקר

אנו זקוקים למדידת וקטור המצב: LQRעל מנת לממש בקרה מסוג

שגיאת מיקום הרכב ביחס למסלול

קצב שינוי שגיאת המיקום

ת זווית הרכב ביחס לזווית המסלולשגיא

קצב שינוי שגיאת הזווית

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

20 ראה עמודהבקרה מערכת בנושאלמידע נוסף

1

1

2

2

e

eX

e

e



פרויקט גמר


מימוש שיטת הנסיעה

)שיטה קודמת( –. עקיבה אחר מסלול מסומן 1

משורטט לפי עקיבה אחר פס סימון )מודבק לרצפה( ה הייתהבעת הצטרפותנו לפרויקט שיטת העקיבה שנבדקה

חיישני "פס", הממוקמים בתחתית –, כאשר מערך חיישני אינפרה אדום מסלול הרצויהעקום אשר מייצג את ה

ישנים מקבלים את הרכב )שניים מלפנים ושניים מאחור( מזהים את הפס ובעזרת אלגוריתם המנתח את קריאת החי

היגוי הרכב ית הרכב ביחס לפס העקיבה המהווה את המסלול, מידע זה עובר לבקר המתקן את ווזהמיקום הרוחבי ו

שאר במרכז המסלול. יכך שהרכב י

:8A-QTRמאפייני חיישן "פס" מדגם

6QTR-8A :דגם

Pololu יצרן:

V ,3.3 V 5 מתח עבודה:

mA 100 זרם:

מ"מ 74.9 :אורך

מ"מ 12.7 :רוחב

up to 1KHzקצב דגימה:

ביחס למסלול הרצוי בתצורה של חיבור של שני ס''מ סטייה לכל כיוון 8-כקציה של עד יהחיישנים נותנים אינד

חיישנים בטור.

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

עבד השני ייטפל בבקרת המ. כאשר מעבד אחד ייטפל לבדו בקריאות חיישני האינפרה אדום. וI2Cפרוטוקול

ום.המהירות והמיק

מדידות שונות מהחיישנים באופן רציף מה 32הסיבה לכך היא שבעבודה עם חיישני האינפרה אדום אנחנו מקבלים

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

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

יר למעבד השני את המיקום.המדידות ויעב 32המעבד הראשון יאסוף את

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



פרויקט גמר


תוצאות ומסקנות משיטה זו

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

זור המסלול הכניסו אורה שונים באאור מהרצפה בתנאי תהחיישנים רגישים מאוד לתנאי התאורה, החזרי

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

ובצבע, ונבחר הפס האופטימלי.

יקת, צפה על מנת לבצע מדידה בצורה מדומ''מ מהר 6מרחק העבודה האופטימלי עבור חיישנים אלו הוא

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

ולכן מ''מ 6לא ניתן היה לשמור על המרחק המומלץ של עקב קפיצות קלות של הרכב מתנאי כביש סטנדרטים

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

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

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

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



פרויקט גמר


שיטת הנסיעה החדשה -. שמירה על מסלול בעזרת קיר ייחוס 2

אחר דיונים וסקירת אפשרויות שונות הוחלט לעבור לשיטה בה חיישנים מודדים מרחק מקיר ייחוס, כאשר ל

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

צלעות, עד לנקודת nהמסלול מתאר מעבר של הרכב ליד שפת מכשול המוגדר באופן הכללי ביותר כפוליגון בעל

אותו מכשול. עגינה הנמצאת על אחת הצלעות של

על מנת לפשט את הנסיעה לאורך המסלול ובעקבות העובדה שחלקים מסוימים במסלול ימומשו באופן מעט שונה,

המסלול יחולק לשלושה חלקים:

נסיעה לאורך הקיר עד לפינה. (1

מעקף הפינה. (2

התקדמות זהירה אל תחנת העגינה ועצירת הרכב בצורה בטוחה. (3

עקיבה מדויקת אחר המסלול במהירויות גבוהות, ולכן חיישני המרחק צריכים בחלק הראשון נדרש לשמור על

למדוד בצורה מדויקת ולעבוד בקצב מהיר.

בנוסף נדרשת מהמערכת היכולת לשמור ולשנות מהירות על פי מיקום הרכב על המסלול.

זיהוי הפינה. החלק השני של המסלול הוא המורכב יותר והוא מעבר הפינה, כאשר הבעייתיות בו היא

על הרכב לזהות את הפינה מראש, להתאים את המהירות ולבצע פנייה בהתאם המסלול.

בתהליך העגינה על הרכב להגיע לעצירה מוחלטת בדיוק בנקודה המוגדרת. לכן דרושה רגישות גבוהה לשינוי

מהירויות במהירות נמוכה ודיוק גבוה של החיישנים במדידת המרחק מהקיר.



פרויקט גמר


יישום שיטת הנסיעה החדשה

איפרה אדוםחק מסוג חיישני מרשימוש ב. 1

מאפיינים:

GP2Y0A21YK דגם:

SHARPיצרן:

V 5 מתח עבודה:

mA 30 זרם:


מ"מ 13 :רוחב

מ"מ 13.5גובה:

ס"מ 10-80תחום עבודה:

Hz 20: קצב דגימה

ס''מ. 15-50יוק גבוהה למרחק בין החיישנים יכולים לתת מדידה ברמת ד

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

לול כמתואר באיור הבא:למס סלזווית הרכב ביח

-מחושבת באופן הבא ית וכאשר זו1

0.5

Xtg

,| 1 2 |X X X )ההפרש בין המדידות(

חישוב זווית – 1איור

X2

50 cm

מיקום החיישנים

X1

X



פרויקט גמר


תוצאות ומסקנות של שיטת יישום זו

ס"מ מהקיר, במרחק 45-ל 20נמצא שהמרחק האופטימלי לעבודה הוא בין ומבדיקות נוספות לאחר כיול החיישנים

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

ס''מ מקיר הייחוס, כך נקבל מדידות מדויקות לשגיאות לשני הכיוונים. 30-לכן בחרנו לנסוע במרחק של כ

.( 47עמ' – 2ראה נספח מספר ) הכיול הבוצע במעבדה נמצא תואם לגרף כיול הנמצא במפרט החיישן

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

:לעבוד בקונפיגורציה הבאה

מאחור, באופן הזה ניתן לחשב ממוצע קריאות של כל זוג חיישנים ולקבל מדידה 2-בקדמת הרכב ו 2חיישנים 4

אמינה יותר ובכך לקבל מיקום וזוית מדויקים יותר.

מערכת בניסויים בחנו את תגובת היעות וניסויים עם הרכב, חיישנים אלו נרכשו והיו בשימוש, ביצענו מספר נס

למדידות שהתקבלו מהחיישנים.

ית ומרחק ו, מתנאיי התחלה שונים )זוקיר הייחוסמ בקו ישר, במרחק קבוע ת הרכב בנסיעהבדקנו את תגובתחילה

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

דו במצלמת וידאו.הניסויים תוע, וקמ''ש 3-5 ,במהירות נמוכה הנסיעות הראשונות בוצעו

הצילומים ניתן היה לראות כי הרכב לא מתכנס למסלול ואינו מתנהג בצורה המתאימה לתוצאות הסימולציות, תוך מ

הרכב הגיב בצורה חזקה מידי לתנאי ההתחלה.

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

את מאמץ הבקרה כך שהרכב לא יגיב בצורה קיצונית לתנאי ההתחלה. הסימולציות יש להקטין

הגענו למסקנה כי הבעיה אינה במידול המערכת או במאמץ בתוצאות, לאחר מספר רב של ניסויים ללא שיפור

ה.הבקר

בשלב זה חזרנו לבצע ניסויים על החיישנים.

טיב המדידה של מרחק מקיר ייחוס, וזווית ביחס לקיר הייחוס.התמקדנו בהחיישנים בניסויי

אך בווקטור המצב קיימת גם הנגזרת של זווית הרכב ביחס לקיר.

2הנגזרת מחושבת על פי הגדרה, הפרש המדידות חלקיי הזמן בין המדידות 1

t

.

שניות, כל שגיאת מדידה מוכפלת פי אלף. 0.001-קצר מאוד, כ מכיוון שהזמן בין המדידות

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

שגיאה גדולה בנגזרת הזווית.אומנם שגיאה של כחצי מעלה נסבלת מבחינתנו במציאת זווית הרכב, הדבר גורם ל

בשלב זה החלטנו להוריד את מדידת נגזרת הזווית מווקטור המצב.



פרויקט גמר


ניסוי נסיעה במהירויות נמוכות, ללא התייחסות לנגזרת הזווית.בשלב הבא חזרנו על

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

אל המסלול התכנסיה קטנה מאוד והרכב י, המרחק הרצוי מהקיר נשמר בסטתה טובה מאודיובת הרכב היתג

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

מערכת הבקרה.

על מנת לראות את תגובת הרכב ומלייםרנדלעקום בעל קימורים לוללאחר מכן החלטנו לשנות במעט את המס

. לצורך כך הקמנו מערך "קירות" אלסטיים מפלקטים המחולקים לשינויים במסלול ולא רק לתנאי התחלה

למקטעים ובעזרתם יצרנו את המסלול הרצוי.

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

.המסלול

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

תצורות המסלול.

קמ''ש הרכב נסע בצורה טובה מאוד, כאשר הוא מתגבר על תנאי ההתחלה השונים ועל שינויי 8-מהירות של כעד

.ולל שינויים מקעירות לקמירות לאורך המסלולעקמומיות כהרדיוס

קמ''ש הרכב נכשל במעבר המסלול. 10-במהירות של כ

רכב מיקום ה קריאת התברר כי ישנה השהיה בין בצילומי הניסוי התבוננות באמצעותניתוח הנסיעות הנ"ל ב

הדרוש לא התבצע תיקוןגם הולכן כלומר מערכת הבקרה קיבלה את המידע על הרכב באיחור, המיקום האמיתיו

. והרכב התבדר מהמסלול בזמן

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

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

מדידה עצמה או במערכת הרכב.ההשהיה נוצרת ב

מסלול קימורים – 2איור



פרויקט גמר


תוצאות הניסוי היו חד משמעיות, החיישנים אינם עובדים בקצב מספק עבור נסיעה במהירויות גבוהות.

]20 -החיישנים עובדים בתדר של כ ]Hz 10לכן בקצב נסיעה של[ / ]m s מקבלים אינדיקציה למיקום הרכב רק

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

בו הרכב , כך שבמקרה ס''מ 30 -קטן, כ הואחיישנים אלו המרחק שנשמר מהקיר עקב תחום העבודה של בנוסף

היא הנסיעהמהירות ון( ית המקסימלית המתאפשרת בשימוש בסרוו הנתוו)הזו 30בזווית מקסימלית שלנמצא

10[ / ]m s ס''מ אל עבר הקיר לפני עדכון המיקום הבא. 25 -נקבל התקדמות של כ

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

המחשה:

.פה לחיישנים אלומהסיבות הנ"ל הוחלט למצוא חלוכתוצאה

דרישה לחלופה:

]100 -) גדול מ יעבדו בקצב דגימה מהירש חיישני מרחק ]Hz על מנת לקבל אינדיקציה טובה מספיק.)

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

°30

30 cm

V=10 m/s

קיר ייחוס

רכב

וית מקסימלית ביחס לקירזו – 3איור



פרויקט גמר


ר מסתובבוחיישן לייזחיישני מרחק מבוססי לייזר ב שימוש .2

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

להשתמש בחיישני הלייזר הבאים: נבחנה האפשרות

מאפיינים:

LIDAR LITE V2דגם:

Pulsedlightיצרן:

V 5 מתח עבודה:

100mA> זרם:

מ"מ 48 :אורך

מ"מ 20 :רוחב

מ"מ 36.5גובה:

40m~תחום עבודה:

Hz up to 800תדר עבודה:

]2.5שגיאת מדידה: ]cm

שני יתרונות מרכזיים על חיישני האינפרה אדום. LiDAR- לחיישני ה

מקצב העבודה של חיישני האינפרה אדום! 40כפול ,-800Hzכ עד קצב עבודה גבוה, (1

40 -יותר מ פי מטר, 40-ומדויקים עד למרחק של כ בייםאפקטי LiDARמרחק מדידה, חיישני ה (2

של חיישני האינפרה אדום! המקסימלי מהמרחק

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

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

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

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

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

, הדבר מאפשר נסיעה על עקום מוגדר ולא דווקא במקביל לקיר.לקיר

ובכך נוכל להתכונן למעקף הפינה.לזהות פינה לפני הגעת הרכב לפינה סריקת גזרה מאפשר בנוסף

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

המחשה:

חיישן

לייזר

נקודת עגינה

V=10 m/s

5 m

קיר ייחוס

רכב

חיישן לייזר מוכוון לכיוון הנסיעה – 4איור



פרויקט גמר


מאפייני מנוע סרוו:

SB-2271SGדגם:

SAVOXיצרן:

V 7.4 – 4.8מתח עבודה:

Hz 250-333 תדר עבודה:

7.4V@[Nm]1.9~: מקסימלימומנט

]7.4V 0.065@מהירות מקסימלית: / 60 ]s~

]4.8V 0.105@מהירות מינימלית: / 60 ]s~

יחידות של מנועי סרוו כנ"ל משמשות להנעת חיישני הלייזר כמוסבר לעיל. 2

מעלות. 1-2לקבל דגימה מחיישני הלייזר כל -הדרישה ממנועי הסרוו

, על מנת לקיים את הדרישה מנוע הסרוו צריך 800Hz-מאחר וחיישני הלייזר עובדים בתדירות גבוהה של עד כ

יות, כאשר החיישנים פועלים בקצב המהיר ביותר. שנ 0.00125 -לנוע מעלה אחת ב

שניות למעלה אחת. 0.00175 -לפי חישוב שביצענו, במהירות המינימלית של מנוע הסרוו ניתן להגיע ל

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

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

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

בווידאוצילמנו יה( יפריימים לשנ 500על גבי מנועי הסרוו, ובעזרת מצלמה לצילום מהיר ) הותקנו חיישני הלייזר

ייזר.ל-את תנועת מערכת הסרוו

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

הנדרשים.

חיישן לייזר מחובר למנוע סרבו – 5איור



פרויקט גמר


מסתובב על מנת לתגבר את הרכב בחיישן לייזר שנבחרו חיישני המרחק מבוססי הלייזר נשקלה אפשרות לאחר

להרחיב את יכולות הרכב.

לקבל תמונה מרחבית טובה יותר, יכולת לזהות מכשולים כך ניתןן זה יעבוד בנוסף לחיישני הלייזר והוחלט שחייש

במרחב, ובנוסף החיישן יעזור בביצוע מעבר הפינה.

חיישן לייזר מסתובב:

מאפיינים:

RPLIDAR - A1M1דגם:

RoboPeakיצרן:

(סורק)מנוע, V 5 מתח עבודה:

סורק()מנוע, mA 100 ,130 זרם:

מ"מ 100 :אורך

מ"מ 70 :רוחב

מ"מ 58גובה:

m 0.2-6תחום עבודה:

]10קצב דגימה: ]Hzup to

וית ביחס והמאפשר לקבל מידע על מרחק מן העצם והז,Triangulation Ranging על עיקרון מבוססחיישן זה

ן:לחייש

מיטר )יחידת הארה(.בשיטה זאת העינית ממוקמת במרחק קבוע לצד הא

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

.יתוולזו מרחקמיקום פגיעת הקרן בעינית נותן אינדיקציה ל

. °360 -החיישן בעל מנוע מובנה המאפשר סריקה ב

, 10Hz -נמוך יחסית עד כ הקצב העבודה של חיישן ז

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



פרויקט גמר


בעזרת חיישני הלייזר נסיעההתכן שלבי

1שלב

אשר נעים בעזרת LiDAR liteהרכב ישמור על מרחק רצוי מהקיר בעזרת מדידות משני חיישני לייזר קווים מסוג

מנועי סרוו.

המחשה:

רכב קיר ייחוס

מדידת מרחק ע"י חיישני לייזר – 7איור

מיקום חיישני לייזר על גבי הרכב – 6איור

חיישן לייזר מסתובב

לוח אם-מיקום הלוח הראשי של הרכב



פרויקט גמר


2שלב

תהליך זיהוי ומעקף הפינה ייעשה לפני הגעה לפינה וייעשה בעזרת שני חיישני הלייזר וחיישן לייזר מסתובב

המחשה:

3שלב

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

ות ושינוי מהירות על פי הצורך עד עצירה בטוחה בתחנת העגינה. כדי התקדמ

רכב קיר ייחוס

זיהוי פינה בעזרת חיישן לייזר מסתובב – 8איור



פרויקט גמר


בקרת מסלול

.LQRבקרת המסלול מנוהלת על ידי בקר

בקרה אופטימלית למערכת נותןנלמד במסגרת קורס תורת הבקרה ו LQRבחרנו בשיטת בקרה זו מכיוון שבקר

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

נתונה מערכת במרחב המצב:

x Ax Bug

y Cx

הינה מטריצת Aהינו ווקטור הכניסות של המערכת, muווקטור המצב של המערכת, הינו nxכאשר

הינה מטריצת היציאה. C-ו הינה מטריצת הכניסות של המערכת, Bהדינמיקה של המערכת,

אנו מעוניינים לממש משוב מהצורה:

u t Fx t

אשר יביא את המערכת לביצועים מיטביים.

תחילה נגדיר מהם ביצועים מיטביים.

נגדיר פונקציית מחיר:

0

T Tx Q t x u R t u dt

אנו מעוניינים להביא את פונקציית המחיר הזו למינימום.

מתקבל כי הפתרון האופטימלי הינו:

1 TF R B P

משוואת ריקטי הינה הפתרון של P-הינה מטריצת הכניסות למערכת, ו Bהינה מטריצת משקלי הכניסות, Rכאשר

(Riccati:)

1 0T TPA A P PBR B P Q .



פרויקט גמר


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

2 2 2 2 2

1 1 2 2

0

e e e e U t

כאשר גודל המקדמים הוא המחיר )החשיבות( של כל רכיב מוקטור המצב.

דוגמא להמחשת שינויים במטריצת המחירים:

)1 שגיאת מרחקחשיבות לשמירת תיתן -גדול יותר מ בחירת פרמטר )e מינימלית על פני שמירת שגיאת

)1 זווית )e.מינימלית

) ייתן חשיבות לגודל מאמץ הבקרהפרמטר )U .

כך מאמץ הבקרה יהיה קטן יותר. גדול יותר ביחס לשאר הפרמטרים -ש כלכ

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

וקטור המחירים W

על ידי מדידת וקטור המצב המורכב מארבעת המרכיבים הבאים, ניתן לקבל את מאמץ הבקרה הדרוש )זווית

הגלגלים( על מנת לחזור למסלול:

.Matlabאשר מספקת התוכנה LQRויקט בפונקציה ליצירת בקר בפועל אנחנו משתמשים בפר

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

שגיאת מיקום הרכב ביחס למסלול. (1

קצב שינוי שגיאת המיקום. (2

שגיאת זווית הרכב ביחס לזווית המסלול. (3

ווית.קצב שינוי שגיאת הז (4

.שגיאת המיקום נמדדת בעזרת חיישני האינפרה אדום

'(.1את שגיאת הזווית מקבלים בעזרת הפרש מדידות החיישנים )איור מס

1 -את קצב שינוי שגיאת המיקום מחשבים בעזרת מהירות הרכב ושגיאת הזווית 2sin( )e V e .

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

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

1לשימוש בשיטת חישוב הנגזרת הקלאסית 1,

1

laste ee

t

מן של שגיאת הזווית, באופן הבא:קצב שינוי שגיאת הזווית מחושב ע"י נגזרת בז

2 -הדגימות 2הפרש בין הזויות מהקריאה הנוכחית והקודמת מחולק בזמן שעבר בין 2,

2

laste ee

t

בצורת חישוב זו קיבלנו שגיאות מדידה גדולות, נכון לזמן כתיבת הדו''ח לא נמצאה דרך יעילה יותר לחשב את

בבקרת הרכב. 2e-ל נגזרת הזווית, ולכן לא נתייחס

1

1

2

2

e

eX

e

e



פרויקט גמר


דרישות הבקרה:

.חזרה מהירה ככל שניתן למסלול (1

.בשגיאת המיקום תגובת יתר מינימלית (2

)זווית ההיגוי המקסימלית של הרכב( מעלות. 30לשמור על מאמץ בקרה נמוך, לא לעבור זווית היגוי של (3

בוד בזוויות קטנות מידי אשר הסרוו לא יכול לספק למרות שמירה על מאמץ בקרה נמוך, לשים לב לא לע (4

מעלות, ככל הנראה המתח המסופק לא נותן מספיק כח למנוע 2-)הסרוו אינו מגיב לזוויות קטנות מ

(להזיז את הגלגלים תחת משקל הרכב.

סימולציות לקבלת הבקר הנדרש:

המערכת:, להלן מודל Simulinkהסימולציות מבוצעות דרך תוכנת

להרחבה בנושא מודל בקרתLQR בקרת מיקום רוחבי.21קודם עמ' פרויקט גמרראה ,

.0-לכן נרצה לקבל התכנסות מהירה ל הסימולציות מדמות נסיעה לאורך קו ישר עבור תנאי התחלה שונים.

תנאי התחלה לסימולציות

דיאגרמת מודל המערכת – 9איור

1

1

2

2

5[ ]

0

5

0

o

e cm

eX

e

e

5m

Vs



פרויקט גמר


. Xזהה לכל וקטור המצב Wקטור משקלים ונתחיל מו

קת מספיק למדוד את שנכון לרגע זה לא מצאנו דרך מדוימכיוון 0-כאשר את המשקל על שינוי קצב הזווית נציב כ

פרמטר זה.

1 1 1 0 1W

1קום והזווית בזמן, סימולציה תיאור שגיאות המי – 1 גרף

1תיאור מאמץ הבקרה בזמן, סימולציה – 2 גרף



פרויקט גמר


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

.שניות 30-נראה התכנסות לאחר כ

צה להעלות את משקל שגיאת המיקום.לכן נר

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

מעלות( 30לא סטינו ממאמץ הבקרה המקסימלי של המערכת ) בגרף השני ניתן לראות כי

יים לא נשנה את משקל מאמץ הבקרה.לכן בנת

סימולציה שנייה:

עבור וקטור המשקלים הבא: 5 0.5 1 0 1W

2תיאור שגיאות המיקום והזווית בזמן, סימולציה – 3 גרף

2תיאור מאמץ הבקרה בזמן, סימולציה – 4 גרף



פרויקט גמר


את המיקום וגם שגיאת הזווית מתכנסות בפחות ניתן לראות בבירור את השיפור מהסימולציה הראשונה, גם שגי

משתי שניות.

שניות גרף שגיאת המיקום אינו חלק. בנקודה זאת שגיאת הזווית הייתה גדולה ולכן 0.2ניתן לראות כי לאחר

נעשה תיקון לכיוון ההפוך. ניתן לראות זאת גם מגרף מאמץ הבקרה.

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

לכן נשמר את יחס המשקלים בין שגיאת המיקום ונגזרתה.

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

סימולציה שלישית:

עבור וקטור המשקלים הבא: 1.1 0.05 1 0 1W

3תיאור שגיאות המיקום והזווית בזמן, סימולציה – 5 גרף

3ימולציה תיאור מאמץ הבקרה בזמן, ס – 6 גרף



פרויקט גמר


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

משתלב בנתיב בתנועת "גלישה".

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

סימולציה רביעית:

בסימולציה הבאה ניתן משקל גם לחלקות התכנסות שגיאת הזווית.

1.1 0.05 1 0.02 1W

ההתכנסות קצת יותר איטית. אך אנחנו נמנעים מתגובת יתר בהתכנסות שתי השגיאות מה שמדמה בצורה הטובה

ביותר את השתלבות רכב למסלול נסיעה.

4תיאור מאמץ הבקרה בזמן, סימולציה – 7 גרף 4תיאור שגיאות המיקום והזווית בזמן, סימולציה – 8 גרף



פרויקט גמר


תכן אלקטרוני

וגי לדיגיטליכרטיס ממיר אנל

)ממיר מאנאלוגי לדיגיטלי( ADCלוח תכננואדום פרהלקרוא את קריאות חיישני האינעל מנת

ניסות אנאלוגיות של כ מספקתקריאות אנאלוגיות, מכיוון שאין לנו במעבד כמות 32אדום נותנים החיישני האינפר

בנינו את המעגל.

תפקוד המעגל:

.MCP3208לדיגיטלי מדגם כל חיישן מחובר לממיר אנאלוגי

אל המעבד.אשר מחובר הממירים מחוברים לפורט

. כאשר המעבד בוחר מאיזה חיישן לקרוא, ואיזה SPIהתקשורת בין המעבד לרכיבים מתבצעת בעזרת תקשורת

ערוץ לקרוא מתוך החיישן.

כולל מחברים ותושבות: ADC -לוח ה

48עמ' – 4*לשרטוט הלוח ראה נספח מספר



פרויקט גמר


כרטיסי תקשורת

מטרת התקשורת האלחוטית היא לשמור את תוצאות הניסויים כך שנוכל לנתח אותן ולמצוא איפה היו תקלות ומה

היו גורמים.

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

:בעזרתו ניתן היה לבצע מספר פעולות Xbeeמימשנו תקשורת בעזרת רכיב תחילה

תחילת נסיעה וקביעת המהירות לתת פקודת (1

שינוי מהירות תוך כדי נסיעה (2

פקודת בלימת חירום (3

שינוי ערכיי הבקר (4

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

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

הניסוי ולעבד אותן.

.NRF24L01כרגע אנחנו עובדים עם כרטיסי תקשורת מסוג

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

הרכב מקבל פקודה הכוללת את המהירות הרצויה והמרחק הרצוי

ים:הנתונים הבאאת ומשיב חזרה אל הקופסא

וקטור המצב )שגיאת מיקום וזווית( (1

מהירות הרכב (2

זמן ריצה (3

מרחק שעבר (4

מאמץ הבקרה )זווית ההיגוי( (5

מתח הסוללה (6

נתונים אלו נשמרים בכל סוף ניסוי וכך ניתן לנתח את תוצאות הניסוי בצורה מדויקת ולזהות באילו נקודות הרכב

נתקל בבעיות בעקבה אחר המסלול.

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



פרויקט גמר


בטיחותי מנגנונ

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

במהלך הנסיעה.

תקינות האנקודר. .1

תקשורת פעילה בין הרכב לקופסת הפיקוד. .2

.נמוךמתח סוללה .3

הנסיעה.הגבלת מרחק .4

תקינות אנקודר:

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

קריאת האנקודר קריטית מכיוון שהרכב ייעלה את מאמץ הבקרה במטרה להגיע למהירות הרצויה עד שקריאת

האנקודר תראה כי המהירות הושגה.

מקרה בו האנקודר לא מתפקד בצורה תקינה הרכב ייעלה את מאמץ הבקרה עד למצב בו הרכב נוסע במהירות ב

קמ''ש. כמובן שמצב זה מסוכן מאוד כפי שניתן לראות בתמונה למעלה. 70-מקסימלית של כ

הרכב ברמת מנגנון הכשל כרגע מתפקד ברמת התוכנה, כאשר בעתיד מתוכנן להוסיף רכיב בדיקת מתח שינטרל את

החומרה.

הרכב לאחר תאונה – 10איור



פרויקט גמר


אופן תפקוד המנגנון:

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

להלן התוצאות:

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

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

מימוש הקוד:

y = 6102.4x-1.129

R² = 0.9977

0

200

400

600

800

1000

1200

1400

1600

1800

2000

0 10 20 30 40 50 60

Time in milliseconds

Desiered Velocity in KMH

זמן תחילת נסיעה כתלות במהירות רצויה

זמן שחלף עד תחילת הנסיעה – 9 גרף



פרויקט גמר


בדיקת תקשורת:

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

קופסת הפיקוד והרכב.

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

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

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

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


תח סוללה:בדיקת מ

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

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

וללה יורד.הס

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

ללמוד את התנהגות הסוללה לאורך זמן.



פרויקט גמר


הגבלת מרחק הנסיעה:

בעזרת קריאת האנקודר ניתן בעזרת אינטגרציה לקבל את מרחק הנסיעה.

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

המרחק מחושב בצורה הבאה:

2encoder motor motor wheelDist EncoderPulses G G R

1,

8

169,

2150

0.05 ,

encoder motor

motor wheel

EncoderPulses Number of encoder reads

G Transmition from encoder reads to motor cycle

G Transmision from motor to the wheels

R m Wheel radius




פרויקט גמר


קופסת פיקוד

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

התחלת נסיעה, הפסקת נסיעה, הזנת מהירות, איסוף נתונים ושמירתם, תוכננה קופסת פיקוד לרכב.

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

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

. י לחיצה עם ניצרת בטיחות ואנטנהשל המערכת כולה, שני כפתורי לחיצה, שני כפתור on/offהמקשים, מפתח

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

הרכב לבין קופסת הפיקוד.

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

עצמו מסיבות שיפורטו בהמשך.

שניתנת להחלפה, ניתן לספק גם לקופסא מתח ע"י שנאי חיצוני. 9Vלקופסא מסופק מתח ע"י סוללה של

קופסת הפיקוד – 11איור



פרויקט גמר


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

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

(Polylactic acid).

(:100kPa -מעלות צלסיוס ו 25 -נתוני החומר )בתנאים סטנדרטיים

*ויקיפדיה.

.מחירו נמוך ובנוסף לכלחוזק, וטמפרטורה, קל, ל הדרישות מבחינת נתוני משחומר זה עונה על כ

בנוסף בתוך הקופסא, כל הרכיבים האלקטרוניים שלה יתחברו ללוח אם שיהיה במידות יחסית קטנות בכדי להיכנס

וץ נוסף לתוך הקופסא, ועם זאת בעל מספיק מקום ומספיק חיבורים ע"מ להכיל את כל הרכיבים של הקופסא. איל

שהוא קטן יותר, הן במידות הפיסיות )הסיבה העיקרית שבגללה Arduino unoהשתמשנו במעבד -הוא המעבד

( Arduino dueנבחר מעבד זה( והן ביכולות שלו לבצע עיבוד נתונים במהירות, מהמעבד שנמצא על הרכב עצמו )

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

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

, והעובדה שהספרייה של כרטיס הזיכרון לוקחת Arduino uno-השוואות ולראות התקדמות, אך בשל השימוש ב

רון צנו לוותר על כרטיס הזיכשל המעבד וגורמת ממש לקריסה של המעבד, נאל RAM-חלק ניכר מזיכרון ה

גדול יותר שיכול להריץ את ספריית RAMמשתמשים במעבד בעל זיכרון ובקופסא ולהעבירו לרכב, שם אנ

כרטיס הזיכרון.

*על המעבד שנמצא בקופסא נפרט בהמשך.

מידות הקופסה:

mm 174אורך:

כפי שניתן לראות בתמונה(. )מקסימלי, משום שלקופסא פרופיל של טרפז ישר זוית, 120mmרוחב:

125mmגובה:

רכיבי הקופסא:

.nRF24L01+ -מודול תקשורת .1

.2.4GHz 5dBi. אנטנה 2

מפתח שיניים. -. מפסק בצורת מנעול קטן3

אדום וכתום. -מפסקים מוארים עם מכסיי הגנה 2. 4

כפתורים מתכתיים עם לד בצבעים ירוק וכחול. 2. 5

ם.כפתורי 12 -. לוח מקשים6

7 .LCD .טקסט, שחור על ירוק, ממשק טורי

. SD.כרטיס 8

.שעון ומד טמפרטורה.9



פרויקט גמר


מודול תקשורת:

מאפיינים:

+nRF24L01 דגם

SparkFunיצרן

3.3-7Vמתח עבודה

0dB+עוצמת שידור

250Kbps-2Mbps קצב שידור

מטר 100 : מרחק מקסימלי

אל : אנטנה פנימית

-SPI תקשורת


מ"מ 17.8 :רוחב

Nordicשל חברת +nRF24L01זהו מודול תקשורת )משדר/מקלט( המבוסס על רכיב

Semiconductor עם מחברSMA -RP לחיבור אנטנה חיצונית.

.חוטיים כמו מקלדות, עכברים וכדומה-נפוצים מאוד במוצרים אל Nordicרכיבים של

לאספקת מתח לרכיב וכל הקווים שלו יכולים להתממשק עם 3.3Vמייצב מתח של כרטיס זה כולל

.5Vבקרים הפועלים על

אנטנה:

מאפיינים:

2.4GHzתדר:

5dBi :הגבר

מ"מ 205 :אורך

מ"מ 13 :קוטר

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



פרויקט גמר


מפסק עם מנעול:

, אין אספקת מתח לקופסא, מטרתו העיקרית למנוע הפעלה לא מכוונת של offכל אימת שמפסק זה נמצא על מצב

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

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

כפתורים: 12מקשים לוח

מאפיינים:

מ"מ 72אורך:

מ"מ 52רוחב:

12מספר מקשים:

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

הפעולות שבאפשרותו לבצע.

:LCDמסך

מאפיינים:

20 :מספר תווים

4 :מספר שורות

5x8 pixelsגודל תווים:

שחור :צבע תווים

ירוק :צבע רקע

5Vמתח עבודה:

I2C :תקשורת

מ"מ 105 :אורך

מ"מ 60 :רוחב

תווים המוצגים בצבע 20שורות של 4עם יכולת תקשורת טורית. מסך זה הינו בעל LCDזהו מסך

שחור על רקע ירוק ומיחידת תקשורת טורית על מעגל אחד.

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



פרויקט גמר


: noUArduinoמעבד

Digital 1/0 pins: 14

Analog input pins: 6(DIP) or 8(SMD)

DC current per 1/0 pin: 40mA

Flash memory: 32KB

SRAM: 2KB

EEPROM:1KB

לוח אם לקופסא:

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

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

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

פתאומיים.

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

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

לבין מתח שמסופק ממקור חיצוני )במידה ובמחובר( שמנו דיודה שלא מאפשר אספקת מתח משניהם.

להלן שירטוט של הלוח:



פרויקט גמר


ניסויים

הניסויים היוו כלי למידה מרכזי בכל הנוגע לתפקוד הבקרה נסיעות. ניסויי במהלך העבודה ביצענו עשרות

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

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

ניסוי שינוי מהירות

למהירות אפס. הזמן הנדרש לחזורו מהירות תגובת הרכב לדרישת מהירות שונה, בדיקת - מטרת הניסוי

תוצאות הניסוי:

מתואר אות הרפרנס אליו אנחנו רוצים להגיע. בכתום

מסקנות:

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

קמ''ש ניתנת לאחר שלוש שניות, כפי שניתן לראות לרכב לוקח כשנייה להגיב לפני 5-פקודת תחילת הנסיעה ב

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

קמ''ש. 1-המהירות המוגדרת ב

בצורה כמעט מידית. יי המהירויות תוך כדי נסיעה נעשית שינו

בהתחשב בכך שהרכב במהלך הניסוי באוויר.קמ''ש לעצירה נעשית בפחות משתי שניות. תוצאה טובה 8-האטה מ

הרכב וחיכוך עם הרצפה. אין האטה ממשקל

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

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

מהירות הרכב בקמ"ש כתלות בזמן – 10 גרף



פרויקט גמר


יישניםניסויי ח

חיישני אינפרה אדום

.כיול חיישן 1

בעזרת עקום הכיול ניתן להמיר בין המתח למרחק כפי שבוצע בתוכנית )קוד( הרכב.

y = 28.487x-1.095

R² = 0.9967

0

10

20

30

40

50

60

0 0.5 1 1.5 2

קח

רמ

[m]

[V]מתח

מרחק כתלות במתח

voltage to distance-distancesensors

-voltage to distance)חזקה distance sensors)

רה אדוםכיול חיישן אינפ – 11 גרף



פרויקט גמר


. קריאת חיישן במצב סטטי2

לקבוע את אמינות ודיוק המדידה של החיישן. - הניסוימטרת

שניות, כאשר המרחק הרצוי מקיר 8מ מהקיר במשך ס" 30בדקנו את קריאת החיישן במצב סטטי במרחק של

ס"מ. 30הייחוס הוא


מסקנות:

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

0.4228Avr הממוצע בבדיקה זו הוא 1.48וסטיית התקן .

הרכב ובחלקו ני זוגות חיישנים ממוקמים בקדמתלעבור למערך שונה של חיישנים כך שש לאחר בדיקה זו הוחלט

האחורי.

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

-10

-5

0

5

10

15

0 1 2 3 4 5 6 7 8 9

dis

t[cm

]

t[s]

מרחק כתלות בזמן במצב סטטי

סטטי במצב חיישן קריאת – 12 גרף



פרויקט גמר


ית ממערך החיישניםו. קריאת זו3

יישנים.לקבוע את אמינות ודיוק המדידה של מערך הח - הניסוימטרת

שניות. 4במשך °10וית של וית המתקבלת מקריאות מערך החיישנים, במצב סטטי בזובדקנו את הזו


ית מתקבלת מחישוב גיאומטרי בין הפרש מרחקים בין ממוצע הקריאות של כל זוג, והמרחק בין מרכזי זוגות והזו

.1מוראה באיור מס' ס"מ, כ 50החיישנים, כאשר מרחק זה ידוע ושווה

10.03Avr הממוצע בבדיקה זו הוא 0.96וסטיית התקן .

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

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

-5

0

5

10

15

20

0 0.5 1 1.5 2 2.5 3 3.5 4

angl

e[d

eg]

t[s]

זוית כתלות בזמן במצב סטטי

החיישנים ממערך ויתוז קריאת – 13 גרף



פרויקט גמר


ית משתנה(וית ממערך החיישנים במצב דינאמי )זוו. קריאת זו4

ולקבוע לקבוע את אמינות ודיוק המדידה של מערך החיישנים במצב דינאמי אל מול המצב הסטטי - ניסוימטרת ה

.את קצב הדגימה של החיישנים

ביחס לקיר משך 10-°ית התחלתית של ובזו ,מיוית המתקבלת מקריאות מערך החיישנים, במצב דינאובדקנו את הז

שניות. 8

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

הייחוס.

°20 -ושוב ל °10 -, לאחר מכן חזרה ל °20וית של ו, כאשר הרכב סובב בהדרגה לז 10-°וית ההתחלתית היא והז

צאות הניסוי:תו

מסקנות:

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

.17Hz-כ -, קיבלנו את קצב המדידה של החיישנים Matlabבנוסף לאחר עיבוד האות בתוכנת

-30

-20

-10

0

10

20

30

40

0 1 2 3 4 5 6 7 8 9

angl

e[d

eg]

t[s]

מצב דינאמי -זוית כתלות בזמן

זווית כתלות בזמן במצב דינאמי – 14 גרף



פרויקט גמר


חיישני לייזר

כיול חיישני לייזר

ר את המרחק כתלות בקריאת החיישן.כיול החיישנים על מנת לקבל עקום המתא – מטרה

תוצאות:

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

(. 45עמ' – 1* כיול חיישן לייזר שני נעשה באותו אופן )ראה נספח מספר

.47עמ' – 3* לתרשים חיבור חיישני הלייזר ראה נספח מספר

y = 1.0104x - 11.334

R² = 0.9994

0

50

100

150

200

250

300

350

400

450

500

0 50 100 150 200 250 300 350 400 450 500

Act

ual

Dis

tan

ce[c

m]

Read[cm]

כיול חיישן לייזר

all points

(all points)ליניארי

כיול חיישן לייזר – 15 גרף



פרויקט גמר


מקורות

פרויקט גמר קודם .1 , תכן בקרת מהירות.29ראה עמ' PIDלהרחבה בנושא בקר המהירות מסוג

, בקרת מיקום רוחבי.21ראה עמ' LQRלהרחבה בנושא מודל בקרת



פרויקט גמר


נספחים:

. כיול חיישן לייזר שני1

שיפוע הגרף כמעט זהה לחלוטין לחיישן הקדמי.

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

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

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

y = 1.014x - 7.2089

R² = 0.998

0

50

100

150

200

250

0 50 100 150 200 250

םרי

טמ

טיסנ

בק

חרמ

קריאת החיישן

כיול חיישן לייזר אחורי



פרויקט גמר


כיול חיישן אינפרה אדום ע"י היצרן. 2



פרויקט גמר


. דיאגרמת חיבור חיישני לייזר3



פרויקט גמר


ADCשרטוט לוח .4



פרויקט גמר


דיאגרמת חיבורים כרטיסי תקשורת .5



פרויקט גמר


מודל קופסת פיקוד .6



פרויקט גמר


קודים

ADC" בעזרת לוח חיישני "פס -וקריאת חיישני אינפרה אדום תקשרות בין מעבדים –. קוד 7

#include <Servo.h>

#include <Wire.h>

#define SLAVE_ADDRESS 0x60

Servo SteeringServo;

//LQR DATA

int K1=38.7298;

int K2=2.1620;

int K3=5.2492;

int K4=0.1755;

double Lsensors=100; //Distance between sensors

int EADC1=22; //ADC 1 CS pin




int DATAOUT=109; //MOSI

int DATAIN=108; //MISO

int SPICLOCK=110; //Clock

int LastSpeed=0;

int SensorData[32]; //Sensor Data 1-8 Front Right ; 9-16 Front Left ; 17-24 Back Right ;

25-36 Back Left

double sumFront=0;

double sumBack=0;

double LastPosFront=0; // holds the data for last known position. used to detrmine the

change in position rate

double LastPosBack=0; // holds the data for last known position. used to detrmine the


double LastAngle=0; // holds the data for last known position. used to detrmine the

change in angle rate

double PosFront=0; // position calculated from the sensors read

double PosBack=0; // position calculated from the sensors read

double Angle=0; // angle between center of gravity to the road

double PosSpeed=0;

double AngleSpeed=0;

// timers to calculate runing time for the software

unsigned long time1=1;



פרויקט גמר





float dt=0;

double SteeringAngle=0; // the angle in which we want to steer

byte Speedbytes=0; // bytes we recieve from the slave for speed data

int motorSpeedD=0; //Vechile speed desired in Kph modify it on the master by

serial interupt and send it to the slave through wire interput

int ReadSpeed1=0;

int ReadSpeed2=0;

void setup()

{

Wire.begin();

Serial.begin(9600); //begin transmition (high speed showed better results)

Serial.println("Start init"); //letting us know where we are. to save run time delete this

line.

pinMode(EADC1,OUTPUT); //next 4 lines set the ADC as output

pinMode(EADC2,OUTPUT);



pinMode(DATAOUT,OUTPUT);

pinMode(DATAIN,INPUT);

pinMode(SPICLOCK,OUTPUT);

digitalWrite(EADC1,HIGH); //next 4 lines sets the ADC to closed

digitalWrite(EADC2,HIGH);



digitalWrite(DATAOUT,LOW); // opens transmition

digitalWrite(SPICLOCK,LOW); // start timer

SteeringServo.attach(9); // connect to the servo

}

void loop()

{

//Calc loop time

time2=time1;

time1=millis();

dt=(time1-time2);

// Serial.print("loop run time: ");

// Serial.println(dt);

//read sensors data

for (int i=22 ; i<26 ; i++)

{

for (int j=1 ; j<9 ; j++)

{



פרויקט גמר


SensorData[j+(i-22)*8]=read_adc(j,i);

}

}

LastPosFront=PosFront; //update last position and angle

LastPosBack=PosBack;

LastAngle=Angle;

//Calc Front Dist

PosFront=Calc_Dist(SensorData,1);

//Calc Front Dist

PosBack=Calc_Dist(SensorData,17);

//Calc Angle

Angle=Calc_Angle(PosFront,PosBack);

//Calc Derivative

PosSpeed=(PosFront-LastPosFront)/dt;

AngleSpeed=(Angle-LastAngle)/dt;

//Clac Steering Angle

SteeringAngle=K1*PosFront+K2*PosSpeed+K3*Angle+K4*AngleSpeed;

SteeringAngle=SteeringAngle*180/3.14159265359+90;

SteeringServo.write(90); // write to servo

time3=millis();

for (int j=1;j<3;j++)

{

Wire.requestFrom(SLAVE_ADDRESS, 1); // i2c interupt requests the speed from the

slave and prints it.

int bytes = Wire.available();

for (int i = 0; i < bytes; i++)

{

if (j==1)

{

byte x = Wire.read();

ReadSpeed1=int(x);

}

else

{

byte y = Wire.read();

ReadSpeed2=int(y);

}

}

}

float kmhspeed=ReadSpeed1+float(ReadSpeed2)/100;

Serial.print("kmh: ");

Serial.println(kmhspeed);

}



פרויקט גמר


float Calc_Angle(double PosFront , double PosBack) // this function recives the positions of

front and back sensors and outputs the angle opend between center of gravity and the road

{

float Result=0;

double Temp=abs(PosFront)+abs(PosBack);

Result=double(atan(double(Temp/Lsensors)));

if (PosBack<PosFront) //Detr angle sign - maybe cheack?!

{

Result=Result*-1;

}

return Result;

}

float Calc_Dist(int ADCD[] , int Part) // this function recives the sensors data and outputs

the position

{

double sum=0;

double weight=0;

int k=1;

for (int i=Part ; i<Part+16 ; i++)

{

sum=sum+ADCD[i];

weight=k*ADCD[i];

k=k+1;

}

float Result=((weight/sum)-8.5)/100; //reduce 8.5 to normal and change to meter

return Result;

}

int read_adc(int channel , int ADCD) //Choose channel (0-8) and device (22-25) // function

reads ADC values-35 rows.

{

int adcvalue = 0;

byte commandbits = B11000000; //command bits - start, mode, chn (3), dont care (3)

digitalWrite(ADCD,HIGH);

//allow channel selection

commandbits|=((channel-1)<<3);

digitalWrite(ADCD,LOW); //Select adc

// setup bits to be written

for (int i=7; i>=3; i--)

{

digitalWrite(DATAOUT,commandbits&1<<i);

//cycle clock

digitalWrite(SPICLOCK,HIGH);

digitalWrite(SPICLOCK,LOW);

}

digitalWrite(SPICLOCK,HIGH); //ignores 2 null bits



פרויקט גמר





//read bits from adc

for (int i=11; i>=0; i--)

{

adcvalue+=digitalRead(DATAIN)<<i;

//cycle clock



}

digitalWrite(ADCD, HIGH); //turn off device

return adcvalue;

}

void serialEvent(){ // this is serial interput, takes serial input (desiered speed) and sends it to

the slave

byte Speedbytes1=Serial.read();



Speedbytes3=Serial.read();

int Speed=(Speedbytes1-48)*100+(Speedbytes2-48)*10+Speedbytes3-48;

float floatspeed=float(Speed)/10;

int intspeedx=floatspeed;

int intspeedy=(floatspeed-intspeedx)*10;

Serial.print("wanted speed float *********************:");

Serial.println(floatspeed);

Serial.print("wanted speed int &&&&&&&&&&&&&&&&&&&&&&&:");

Serial.println(Speed);

if ((floatspeed<40)&&(((Speed-LastSpeed)<50)||(LastSpeed==0)))

{

LastSpeed=Speed;

Wire.beginTransmission(0x60);

for (int j=1;j<3;j++)

{

if (j==1)

Wire.write(byte(intspeedx));

else

Wire.write(byte(intspeedy));

}

Wire.endTransmission();

}

else

Serial.println("BAD VALUE");

}



פרויקט גמר


. קוד קופסת פיקוד8

#include <Wire.h>

#include <LiquidCrystal_I2C.h>

#include <Keypad.h>

#include <SPI.h>

#include <RF24.h>

#include <SD.h>

#include <DS3231.h>

#define GreenButton 5 //Define push button 1

#define BlueButton 4 //Define push button 2

#define Saftey 7 //Define switch 1

// Setup structures

bool h12;

bool PM;

bool Century=false;

struct dataStruct1{

float MotorSpeedD1 = 0;

float WheelDist=10;

}DataOut;

struct dataStruct{

unsigned long RunTime;

float e1;

float e1dot;

float e2;

float e2dot;

float Speed;

float WheelDist;

float CtrlEff;

float Voltage;

}Data;

DS3231 Clock;

LiquidCrystal_I2C lcd(0x3F, 20, 4);

float DataFix[9];

int i=100;

//SD Vars

const int chipSelectSD = 8;

//Com. Vars



פרויקט גמר


bool radioNumber = 0;

RF24 radio(6,A3);

byte addresses[][6] = {"1Node","2Node"};

uint8_t ArrowUp[8] = {B00100, B01110, B10101, B00100, B00100, B00100, B00100};

uint8_t ArrowDown[8] = {B00100, B00100, B00100, B00100,B10101 ,B01110 , B00100};

const byte KeyPadROWS = 4;

const byte KeyPadCOLS = 3;

char keys[KeyPadROWS][KeyPadCOLS] = {

{'1','2','3'},

{'4','5','6'},

{'7','8','9'},

{'*','0','#'}

};

//2,3,A2,9

byte KeyPadRowPins[KeyPadROWS] = {A0,3,A2,A1}; //connect to the row pinouts of the

keypad

byte KeyPadColPins[KeyPadCOLS] = {2,10,9}; //connect to the column pinouts of the

keypad

unsigned long DispTime=0;

String fileName;

Keypad keypad = Keypad( makeKeymap(keys), KeyPadRowPins, KeyPadColPins,

KeyPadROWS, KeyPadCOLS );

void setup()

{

Serial.begin(9600);

//******************** set pins ***********************

pinMode(GreenButton, INPUT_PULLUP);

pinMode(BlueButton, INPUT_PULLUP);

pinMode(Saftey, INPUT_PULLUP);

//******************** setup coumunication ***********************

radio.begin();

radio.setPALevel(RF24_PA_MAX);

if(radioNumber){

radio.openWritingPipe(addresses[1]);

radio.openReadingPipe(1,addresses[0]);

}else{

radio.openWritingPipe(addresses[0]);

radio.openReadingPipe(1,addresses[1]);

}



פרויקט גמר


radio.startListening();

//******************** start up lcd ***********************

lcd.begin();

lcd.createChar(0, ArrowUp);

lcd.createChar(1, ArrowDown);

//******************** Welcome Message ***********************

lcd.setCursor(7,0);

lcd.print("S.A.M");

lcd.setCursor(1,2);

lcd.print("Smart Autonomous");

lcd.setCursor(3,3);

lcd.print("Maneuvering");

delay(2500);

lcd.clear();

//******************** check for sd card (can we remove this???)

***********************

if (!SD.begin(chipSelectSD))

{

Serial.println("Card failed, or not present");

return;

}

Serial.println("card initialized.");

}

//******************** main loop ***********************

void loop()

{

if (i>0)

{

lcd.setCursor(0,0);

lcd.print("Enter Desiered Speed");

lcd.setCursor(0,1);

lcd.print("In KMH 3 digits");

}

while (i>0)

{

int digit = keypad.getKey();

if ((digit==35) || (digit==42))

{

digit=0;

}

if (digit==48)

{

digit=99;

}



פרויקט גמר


if ((digit>=48) && (digit<=57))

{

digit=digit-48;

}

if (digit != NO_KEY)

{

if (digit==99)

{

i=i/10;

delay(300);

}

else

{

DataOut.MotorSpeedD1=DataOut.MotorSpeedD1+digit*i;

i=i/10;

lcd.setCursor(0,3);

lcd.print(DataOut.MotorSpeedD1/10,1);

delay(300);

}

}

}

if (i==0)

{

DataOut.MotorSpeedD1=DataOut.MotorSpeedD1/10;

i=-99;

}

lcd.clear();

lcd.setCursor(0,0);

lcd.print(DataOut.MotorSpeedD1,1);

lcd.setCursor(0,1);

lcd.print("Green Button Confirm");

lcd.setCursor(0,2);

lcd.print("Blue Button Cancel");

int temp=digitalRead(BlueButton);

if (temp==0)

{

i=100;

DataOut.MotorSpeedD1=0;

lcd.clear();

return;

}

temp=digitalRead(GreenButton);

if (temp==0)

{

lcd.clear();

int second,minute,hour,date,month,year,temperature;

second=Clock.getSecond();

minute=Clock.getMinute();



פרויקט גמר


hour=Clock.getHour(h12,PM);

date=Clock.getDate();

month=Clock.getMonth(Century);

year=Clock.getYear();

temperature=Clock.getTemperature();

fileName=String(date,DEC)+'_'+String(month,DEC)+'_'+String(hour,DEC)+'_'+String(minut

e,DEC)+".txt";

Serial.println(fileName);

File dataFile = SD.open(fileName, FILE_WRITE);

if (dataFile)

{

Serial.println("Print to file");

dataFile.print("Speed: ");

dataFile.println(DataOut.MotorSpeedD1);

dataFile.print("Driving distance: ");

dataFile.println(DataOut.WheelDist);

dataFile.print("Temp: ");

dataFile.println(temperature);

dataFile.close();

}

Serial.println(DataOut.MotorSpeedD1);

Drive();

}

delay(200);

}

void Drive()

{

while (DataOut.WheelDist>Data.WheelDist)

{

if (digitalRead(Saftey)==1) /* checks saftey status, if it's down transmit desiered

speed=0*/

{


i=100;

lcd.clear();

lcd.setCursor(0,0);

lcd.print("Saftey is Down");

delay(3000);

break;

}

radio.stopListening(); // First, stop listening so we can talk.



פרויקט גמר


if (!radio.write( &DataOut, sizeof(DataOut) ))

{

Serial.println(F("failed"));

}

radio.startListening(); // Now, continue listening

unsigned long started_waiting_at = micros(); // Set up a timeout period, get the

current microseconds

boolean timeout = false; // Set up a variable to indicate if a response

was received or not

while ( ! radio.available() ){ // While nothing is received

if (micros() - started_waiting_at > 200000 ){ // If waited longer than 200ms,

indicate timeout and exit while loop

timeout = true;

break;

}

}

if ( timeout ){ // Describe the results

Serial.println(F("Failed, response timed out."));

}else{

// Grab the response, compare, and send to debugging spew

radio.read( &Data, sizeof(Data) );

File dataFile = SD.open(fileName, FILE_WRITE);

if (dataFile)

{

dataFile.print(Data.RunTime);

dataFile.print(" ");

dataFile.print(Data.e1);


dataFile.print(Data.e1dot);


dataFile.print(Data.e2);


dataFile.print(Data.e2dot);


dataFile.print(Data.Speed);


dataFile.print(Data.WheelDist);


dataFile.print(Data.CtrlEff);


dataFile.println(Data.Voltage);

}

}

if (millis()>DispTime+1000)

{

lcd.setCursor(0,0);

lcd.print("Speed: ");



פרויקט גמר


lcd.setCursor(8,0);

lcd.print(Data.Speed);

lcd.setCursor(0,1);

lcd.print("Delta S: ");

lcd.setCursor(10,1);

lcd.print(Data.WheelDist);

lcd.setCursor(0,2);

lcd.print("Voltage: ");

lcd.setCursor(10,2);

lcd.print(Data.Voltage);

DispTime=millis();

}

// Try again 0.1s later

}

i=100;


delay(3000);

lcd.clear();

}



פרויקט גמר


. קוד מרכזי 9

#include <PID_v1.h> //speed control

#include <Servo.h>

#include <SPI.h> //communication protocol

#include <RF24.h> //wireless communication library

#include <SD.h> //SD library

#include <Wire.h> //I2C Library

#include <LIDARLite.h> //Laser sensor library

//wireless communication

RF24 radio(6, 7); // Hardware configuration: Set up nRF24L01 radio on SPI bus plus pins 6 &

7

byte addresses[][6] = {"1Node", "2Node"};

unsigned long TransmitTime = 0; //use for communication fail

//SD Variable

File SAMdata;

//LidarLite Variable

LIDARLite FrontLidar;

LIDARLite BackLidar;

//information sent to the control box

struct dataStruct {

unsigned long RunTime;

float e1; //lateral error

float e1dot;

float e2; //angle error

float e2dot;

float Speed; //vehicle speed in kmh

float WheelDist; //distance travelled

float CtrlEff; //steering angle

float Voltage;

} Data;

//desired parameters

struct dataStruct1 {

float motorSpeedD1 = 0;

float WheelDistWanted = 0;

} DataIn;

#define gearR 169/2150 //Gear ratio

#define escP 8 //ESC (electric speed controller) PIN

#define EncoderP 10 //Encoder input pin

#define SteeringP 9 //Steering Servo output pin

#define LaserFP 3 //Steering Servo output pin

#define LaserBP 4 //Steering Servo output pin

#define SDP 28 //SD control pin

#define stTime 30 //Period if speed corection



פרויקט גמר


#define Pi 3.141592654

#define r 0.05 //Wheel radius

#define Rad2Deg 57.2957795

#define AngleOffset 73

#define SDLoopCounter 5 //every SDLoopCounter print results to the SD

#define SAMLength 59.94 //Length of Sam the vehicle in cm

Servo ESC;

Servo SteeringServo;

Servo LaserFront;

Servo LaserBack;

/********************* LQR VALUES ************************/

float K1 = 3.3;

float K2 = 0.08129;

float K3 = 1.8;

float K4 = 0;

/********************* Setup Variables ************************/

float LastSpeed = 0;

float LastPosError = 0; // holds the data for last known position. used to detrmine the


float PosError = 0; // position related to the wall

float DesPos = 0.32; // desired position from the wall

float LastAngle = 0; // holds the data for last known position. used to detrmine the change

in angle rate

float Angle = 0; // angle between center of gravity to the road

double PosSpeed = 0;

double AngleSpeed = 0;

float v = 0; //omega*r

unsigned long time1 = 0; //to find derivatives

unsigned long time2 = 0; //to find derivatives

float dt = 0; //to fine derivatives

double SteeringAngle = 0; // the angle in which we want to steer

unsigned long Ctime = 0; //last time the PID updated the control effort

unsigned long TotalPulses = 0; //total pulses of encoder

bool Finish = 0; //0=run, 1=dont run

float WheelDist = 0; //distance already traveled

float WheelDistWanted = 5; //distance to travel

float counter = 0; //encoder counter

float rpm = 0; //motor RPM

float kmh = 0; //vechile speed in Km/h

float motorSpeed = 0; //Desired throttle

float motorSpeedD1 = 0; //Vechile speed desired in Km/h

int SDprint = 0; //print to SD every other loop

float LaserFrontData = 0; //Front lidar data

float LaserBackData = 0; //Back lidar data

float LidarAVG = 0; //average of front and back lidar

float FixedFrontLidar = 0; //FrontLidar distance after party



פרויקט גמר


float FixedBackLidar = 0; //BackLidar distance after calibration

float delta = 0; //Difference of distance from the wall of the front and back

lidar laser

float ratio = 0; //deta/vehicle length

double Setpoint, Input, Output; //PID vars: desired speed , input and output

PID PIDSpeed(&Input, &Output, &Setpoint, 2.38, 10.82, 0, DIRECT);

void setup()

{

Serial.begin(250000); //begin transmition (high speed showed better results)

Serial.println(F("Start init")); //letting us know where we are. to save run time delete

this line.

attachInterrupt(digitalPinToInterrupt(10), EncoderInter, RISING); //digitalPin... sets encoder

to pin number 'EncoderP'. runs EncoderInter function on rise

SteeringServo.attach(SteeringP); // connect to the steering servo at pin 'SteeringP'

LaserFront.attach(LaserFP); // connect to the front laser servo at pin 'LaserFP'

LaserBack.attach(LaserBP); // connect to the back laser servo at pin 'LaserBP'

ESC.attach(escP); // connect to the ESC at pin 'escP'

Setpoint = 70; //define pid set value to stop

Input = 0; //Inital vechile speed

PIDSpeed.SetMode(AUTOMATIC); //Start the PID controller

PIDSpeed.SetSampleTime(stTime); //Set sample time for the PID

ESC.write(motorSpeed); //Send stop command to the motor

/******************wireless communication setup ***************/

radio.begin();

radio.setPALevel(RF24_PA_MAX); //Set power level for the wireless

communication

radio.openWritingPipe(addresses[1]); //Set communication pipes

radio.openReadingPipe(1, addresses[0]); //Set communication pipes

radio.startListening(); // Start listening to the radio

/******************SD setup*************************/

if (!SD.begin(SDP)) {

Serial.println(F("SD initialization failed!"));

return;

}

SAMdata = SD.open("TestRun.txt",FILE_WRITE);

SAMdata.println(F("*******************************New

Test*********************************"));

SAMdata.close();



פרויקט גמר


/***************************lidarlite setup****************************/

FrontLidar.begin(1,true,false,0x63);

BackLidar.begin(1,true,false,0x64);

Serial.println(F("Finish init"));

}

void EncoderInter()

{

counter++; //For Vehicle Speed

TotalPulses++; //For total distance travelled

}

void loop()

{

//find the loop time

time2 = time1;

time1 = micros();

dt = (time1 - time2) * 0.000001;

Serial.println(dt,9);

/****************** Encoder Count & Speed Calc ***************************/

unsigned long temptime = millis();

if (temptime > (Ctime + stTime))

{

Ctime = temptime;

rpm = (counter * 60000) / (8 * stTime);

float W = rpm * gearR / 9.54;

v = W * r;

kmh = v * 3.6;

counter = 0;

Input = 0.4627 * kmh + 89;

motorSpeed = 0.4627 * motorSpeedD1 + 89;

Setpoint = motorSpeed;

PIDSpeed.Compute();

if (Finish == 0)

{

ESC.write(Output + 70); // +70 to start without delay

}

else

{

ESC.write(10); // +70 to start without delay

}

}

/****************** Read From Sensors ***************************/



פרויקט גמר


LaserFrontData = FrontLidar.distance();

LaserBackData = BackLidar.distance();

FixedFrontLidar=LaserFrontData; /*Need to insert calibration function*/

FixedBackLidar=LaserBackData; /*Need to insert calibration function*/

LidarAVG=(FixedSensor1+FixedSensor2)/2;

LidarAVG=LidarAVG/100; // convert to meters

delta=FixedFrontLidar-FixedBackLidar; //delta used for angle calculation

ratio=delta/SAMLength; //non-dimensional ratio of delta/length of stv (sam the

vehicle)

LastPosError=PosError; //update last position and angle

LastAngle=Angle;

Angle=(asin(ratio)); //calculate the angle of the vhicle in radians

PosError=(LidarAVG-DesPos);

/****************** Calc Derivative ***************************/

AngleSpeed = (Angle - LastAngle) / dt;

PosSpeed = v * sin(Angle);

/****************** Calc Steering Angle ***************************/

SteeringAngle = (K1 * PosError + K2 * PosSpeed + K3 * Angle + K4 * AngleSpeed); //Calc

the steering angle from the LQR

SteeringAngle = SteeringAngle * Rad2Deg + AngleOffset; //Fix the steering angle to deg

and add offset due to servo offset

SteeringServo.write(SteeringAngle); // Send steering angle to servo

/****************** Distance Limit Set ***************************/

WheelDist = TotalPulses * 0.00308679743; //Calc total distance we travelled

if (WheelDist >= WheelDistWanted)

{

Finish = 1; //You have reached your destination

}

/****************** Encoder failure *****************************/

if ((millis() > (6102.4 * pow(motorSpeedD1, -1.129)) + 15) && (v < 0.1) && (Finish == 0))

{

Serial.println("ENCODER ERROR!!!");

while (1 == 1)

{

ESC.write(10);

}

}



פרויקט גמר


/****************** wireless Communication between car to control box

***************************/

if ( radio.available()) //the control box sent data to the car

{

while (radio.available()) // While there is data ready

{

radio.read( &DataIn, sizeof(DataIn)); // Get the payload

motorSpeedD1 = DataIn.motorSpeedD1;

WheelDistWanted = DataIn.WheelDistWanted;

}

motorSpeed = 0.4627 * motorSpeedD1 + 89; //Get desired speed in throttle

Setpoint = motorSpeed;

radio.stopListening(); // First, stop listening so we can talk

Data.e1 = Pos; // Data Out Struct Build

Data.e1dot = PosSpeed;

Data.e2 = Angle;

Data.e2dot = AngleSpeed;

Data.Speed = kmh;

Data.WheelDist = WheelDist;

Data.CtrlEff = SteeringAngle;

Data.Voltage = 0;

radio.write( &Data, sizeof(Data) ); // Send data to the control box

radio.startListening(); // start radio communication again

TransmitTime = micros(); // samples time only when radio is

avalible

}

/******************* write to SD card****************************/

if (SDprint==SDLoopCounter)

{

SDprint=1;

Serial.println("0");

SAMdata = SD.open("TestRun.txt",FILE_WRITE); // openning the file

SAMdata.print(micros());

SAMdata.print(" ");

SAMdata.print(Data.e1);

SAMdata.print(" ");

SAMdata.print(Data.e1dot);

SAMdata.print(" ");

SAMdata.print(Data.e2);

SAMdata.print(" ");

SAMdata.print(Data.e2dot);

SAMdata.print(" ");

SAMdata.print(Data.Speed);

SAMdata.print(" ");

SAMdata.print(Data.WheelDist);

SAMdata.print(" ");



פרויקט גמר


SAMdata.print(Data.CtrlEff);

SAMdata.print(" ");

SAMdata.println(Data.Voltage);

SAMdata.close();

}

else{

SDprint=SDprint+1;

Serial.println("1");

}

/****************** check comunication ***************************/

unsigned long StopTransmit = micros(); // samples every time the loop runs

if (StopTransmit - TransmitTime > 100000) { // checks comunication, if there is no

comunication for 0.1[Sec] stop the car.

Serial.println(F("Communication Fail"));

while (1 == 1)

{

ESC.write(10);

}

}

}

פרויקט גמר חלק 1

Documents