פרויקט גמר חלק 1
-
Upload
raz-zukran -
Category
Documents
-
view
98 -
download
0
Transcript of פרויקט גמר חלק 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 .................................................................................................................... חיישני לייזר
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
3עמוד S.A.M - Smart Autonomous Maneuvering
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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
4עמוד S.A.M - Smart Autonomous Maneuvering
הקדמה
כגון קניונים כיום ישנו צורך מתגבר לרכבים אוטונומיים מהירים אשר מסוגלים לנווט בשטחים אורבניים צפופים
. לרכבים אלו ישנן אפליקציות רבות החל מצורכי אבטחה וסיור ועד שינוע ציוד בעת מקרי או רחובות בערים
חירום. בסקר שוק ראשוני שהתבצע נמצא כי ישנה דרישה גבוהה לפלטפורמה שכזו.
ל פרופסור אילון רימון. אי לכך הוחלט לפתח פלטפורמה שכזו במעבדה לניווט רובוטים בטכניון תחת הנחייתו ש
מטר בשנייה( ע"מ להימנע מהקשיים שבחיזוי תנועת אנשים. 10-הוחלט להשתמש בפלטפורמה מהירה )המגיעה ל
המהירות הגבוהה של הרכב מאפשרת התייחסות לסביבת הרכב כסטטית )מהירות ממוצעת של בן אדם הולך הינה
מטר לשנייה(. 2-3-כ
ות כה גבוהה יצרה אתגרים רבים כגון בעיות תמרון )בעיקר ברדיוס קטן(, ביצוע עבודה עם כלי רכב הנע במהיר
בלימת חירום בעת הצורך, לוקליזציה ומיפוי של הסביבה וכו'.
מהשלב בו הצטרפנו לצוות המחקר. הפרויקטבדו"ח זה נפרט את התקדמות
מטרת הפרויקט
טפורמה רובוטית אוטונומית מהירה לניווט בסביבה פרויקט זה נעשה במסגרת מחקר רחב אשר עוסק בפיתוח פל
אורבנית צפופה.
הפרויקט התמקד במערכת עגינה אוטונומית בזמן אופטימלי תחת שמירת אילוצי בטיחות. פרויקט זה מתבסס על
הנחייתו של פרופסור אילון רימון.עבודת הדוקטורט והמאמר אשר כתב ד"ר גיל מנור ונעשה ב
יעדים
הירות משתנה תוך כדי שמירה על אילוצי הבטיחות עד לעגינה בטוחה יי מסלול ידוע במעקיבה אחר .1
בניצב לקיר.
.בטיחות מפני תאונותיצירת מנגנוני .2
שמירת נתונים ושליטה מרחוק בעזרת קופסת פיקוד חכמה. .3
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
5עמוד S.A.M - Smart Autonomous Maneuvering
רכיבי המערכת
רכיבים קיימים
פלטפורמת רכב של חברת“thunder tiger” מדגם “ER4-G3“.
מעבדArduino Due .
מעבד Arduino Uno.
אדום לזיהוי מרחק מקיר. המערך חיישני אינפר
.)מקודד אופטי )אנקודר
,חוטים ומחברים(.מייצבי מתח רכיבי חשמל בסיסיים )מטריצת חיווט ,
סוללתLiPo 14.8 וולט
2 .כרטיסי תקשורת
2 חיישניLidar lite לא נמצאים על מערכת הרכב(לזיהוי מרחק מקיר )עדיין
2 יחידות סרבו עליהם מותקנים חיישני הלייזרSAVOX SB-2271SG
יחידת סרבו המשמשת להיגוי– PROTEK RC 160SS
תוספות תכן
אדום לקריאת פס עקיבה התושבות לחיישני אינפר
אדום לקריאת מרחק מקיר התושבות לחיישני אינפר
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
6עמוד S.A.M - Smart Autonomous Maneuvering
שיטת הנסיעה
ול הנסיעה ידוע מראש ונקבע בהתאם לנקודת ההתחלה, נקודת הסיום ומיקום מכשול בסביבת הנסיעה. מסל
ה. מהירות הרכב בכל נקודה במסלול נבחרה תחת אילוצי ימטר לשני 10-הרכב ינוע במסלול במהירות גבוה, עד כ
הבטיחות המוגדרים כך שתמיד ניתן לבצע בלימת חירום בצורה בטוחה.
2ור המהירות בכל נקודה הוא: הביטוי עב maxv a r כאשר ,𝑟 .הינו המרחק מן המכשול
ביטוי זה נותן את המהירות המקסימלית בתנאי הסביבה הנתונה ויכולת העצירה של הרכב.
9קודם עמ' פרויקט גמר – 1 ראה מקור מספר להרחבה בנושא
כאשר קריאת האנקודר בל קריאה מאנקודר הממוקם על ציר המנוע, המק PIDמהירות הרכב נשלטת ע"י בקר
, ושולח פקודה אל המנוע על מנת מומרת למהירות הרכב בקמ"ש בעזרת יחס התמסורת בין ציר המנוע לגלגלים
.לשנות את המהירות לפי הצורך
להרחבה בנושא בקר המהירות מסוגPID ת, תכן בקרת מהירו29קודם עמ' פרויקט גמרראה
וית והמיקום היחסי של והז המודדים את בחיישני לייזר, הרכב מצויד לשמור על מסלול הנסיעה המתוכנןעל מנת
הרכב במסלול.
מאמץ הבקרה המוזן למערכת ינויהמיקום ע"י ש תושגיאתתקן את מערכת הבקרה של הרכב נתונים אלובעזרת
.ההיגוי של הרכב
בקרה אופטימלית למערכת דינמית בשימוש באנרגיה מינימלית. מבטיחה, LQRמערכת הבקרה מנוהלת ע"י בקר
אנו זקוקים למדידת וקטור המצב: LQRעל מנת לממש בקרה מסוג
שגיאת מיקום הרכב ביחס למסלול
קצב שינוי שגיאת המיקום
ת זווית הרכב ביחס לזווית המסלולשגיא
קצב שינוי שגיאת הזווית
ים בעזרת חיישני הלייזר בזמן אמת.את וקטור המצב אנו מודד
20 ראה עמודהבקרה מערכת בנושאלמידע נוסף
1
1
2
2
e
eX
e
e
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
7עמוד S.A.M - Smart Autonomous Maneuvering
מימוש שיטת הנסיעה
)שיטה קודמת( –. עקיבה אחר מסלול מסומן 1
משורטט לפי עקיבה אחר פס סימון )מודבק לרצפה( ה הייתהבעת הצטרפותנו לפרויקט שיטת העקיבה שנבדקה
חיישני "פס", הממוקמים בתחתית –, כאשר מערך חיישני אינפרה אדום מסלול הרצויהעקום אשר מייצג את ה
ישנים מקבלים את הרכב )שניים מלפנים ושניים מאחור( מזהים את הפס ובעזרת אלגוריתם המנתח את קריאת החי
היגוי הרכב ית הרכב ביחס לפס העקיבה המהווה את המסלול, מידע זה עובר לבקר המתקן את ווזהמיקום הרוחבי ו
שאר במרכז המסלול. יכך שהרכב י
:8A-QTRמאפייני חיישן "פס" מדגם
6QTR-8A :דגם
Pololu יצרן:
V ,3.3 V 5 מתח עבודה:
mA 100 זרם:
מ"מ 74.9 :אורך
מ"מ 12.7 :רוחב
up to 1KHzקצב דגימה:
ביחס למסלול הרצוי בתצורה של חיבור של שני ס''מ סטייה לכל כיוון 8-כקציה של עד יהחיישנים נותנים אינד
חיישנים בטור.
בניסיון לייעל את פעולת המעבד הוחלט לפצל את משימות המעבד לשני מעבדים שונים שיתקשרו ביניהם בעזרת
עבד השני ייטפל בבקרת המ. כאשר מעבד אחד ייטפל לבדו בקריאות חיישני האינפרה אדום. וI2Cפרוטוקול
ום.המהירות והמיק
מדידות שונות מהחיישנים באופן רציף מה 32הסיבה לכך היא שבעבודה עם חיישני האינפרה אדום אנחנו מקבלים
שדורש כוח עיבוד גדול ומאט את קצב עבודת המעבד.
בשביל לא ליצור השהייה בין המדידה לפקודת התיקון הוחלט לפצל לשני מעבדים.
יר למעבד השני את המיקום.המדידות ויעב 32המעבד הראשון יאסוף את
המעבד השני יקבל את המיקום ובעזרת הבקרה ישלח פקודת תיקון.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
8עמוד S.A.M - Smart Autonomous Maneuvering
תוצאות ומסקנות משיטה זו
בעקבות מספר ניסויים שביצענו על מנת לבדוק את יעילות החיישנים הגענו למסקנות הבאות:
זור המסלול הכניסו אורה שונים באאור מהרצפה בתנאי תהחיישנים רגישים מאוד לתנאי התאורה, החזרי
הפרעות למדידות. בעקבות זאת נבדק השימוש במספר סוגים של פסי עקיבה השונים אחד מהשני בברק
ובצבע, ונבחר הפס האופטימלי.
יקת, צפה על מנת לבצע מדידה בצורה מדומ''מ מהר 6מרחק העבודה האופטימלי עבור חיישנים אלו הוא
ק זה ראינו שמתקבלות קריאות לא יציבות ובעלות שגיאות גדולות. במרחק שונה במקצת ממרח
ולכן מ''מ 6לא ניתן היה לשמור על המרחק המומלץ של עקב קפיצות קלות של הרכב מתנאי כביש סטנדרטים
ניתן היה לקבל מדידות אמינות ולבצע ניווט על פי מדידות החיישנים. לא
פס המונח על הרצפה לפי קואורדינטות המסלול נפסלה עקב חוסר יכולת על פי העובדות הנ"ל שיטת העקיבה ע"י
של חיישני המדידה לעבוד ברמת אמינות ודיוק מספקת תחת תנאי הנסיעה.
לכן נאלצנו לשנות את שיטת העקיבה אחר המסלול.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
9עמוד S.A.M - Smart Autonomous Maneuvering
שיטת הנסיעה החדשה -. שמירה על מסלול בעזרת קיר ייחוס 2
אחר דיונים וסקירת אפשרויות שונות הוחלט לעבור לשיטה בה חיישנים מודדים מרחק מקיר ייחוס, כאשר ל
המרחק מהקיר בכל נקודה מגדיר את המסלול.
צלעות, עד לנקודת nהמסלול מתאר מעבר של הרכב ליד שפת מכשול המוגדר באופן הכללי ביותר כפוליגון בעל
אותו מכשול. עגינה הנמצאת על אחת הצלעות של
על מנת לפשט את הנסיעה לאורך המסלול ובעקבות העובדה שחלקים מסוימים במסלול ימומשו באופן מעט שונה,
המסלול יחולק לשלושה חלקים:
נסיעה לאורך הקיר עד לפינה. (1
מעקף הפינה. (2
התקדמות זהירה אל תחנת העגינה ועצירת הרכב בצורה בטוחה. (3
עקיבה מדויקת אחר המסלול במהירויות גבוהות, ולכן חיישני המרחק צריכים בחלק הראשון נדרש לשמור על
למדוד בצורה מדויקת ולעבוד בקצב מהיר.
בנוסף נדרשת מהמערכת היכולת לשמור ולשנות מהירות על פי מיקום הרכב על המסלול.
זיהוי הפינה. החלק השני של המסלול הוא המורכב יותר והוא מעבר הפינה, כאשר הבעייתיות בו היא
על הרכב לזהות את הפינה מראש, להתאים את המהירות ולבצע פנייה בהתאם המסלול.
בתהליך העגינה על הרכב להגיע לעצירה מוחלטת בדיוק בנקודה המוגדרת. לכן דרושה רגישות גבוהה לשינוי
מהירויות במהירות נמוכה ודיוק גבוה של החיישנים במדידת המרחק מהקיר.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
10עמוד S.A.M - Smart Autonomous Maneuvering
יישום שיטת הנסיעה החדשה
איפרה אדוםחק מסוג חיישני מרשימוש ב. 1
מאפיינים:
GP2Y0A21YK דגם:
SHARPיצרן:
V 5 מתח עבודה:
mA 30 זרם:
מ"מ 44.5 :אורך
מ"מ 13 :רוחב
מ"מ 13.5גובה:
ס"מ 10-80תחום עבודה:
Hz 20: קצב דגימה
ס''מ. 15-50יוק גבוהה למרחק בין החיישנים יכולים לתת מדידה ברמת ד
הרכב ניתן גם לקבל אינדיקציה השני בחלק האחורי שלבקדמת הרכב והממוקמים כך שאחד חיישנים, שניבעזרת
לול כמתואר באיור הבא:למס סלזווית הרכב ביח
-מחושבת באופן הבא ית וכאשר זו1
0.5
Xtg
,| 1 2 |X X X )ההפרש בין המדידות(
חישוב זווית – 1איור
X2
50 cm
מיקום החיישנים
X1
X
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
11עמוד S.A.M - Smart Autonomous Maneuvering
תוצאות ומסקנות של שיטת יישום זו
ס"מ מהקיר, במרחק 45-ל 20נמצא שהמרחק האופטימלי לעבודה הוא בין ומבדיקות נוספות לאחר כיול החיישנים
מין לגבי מיקום הרכב.זה קריאות החיישנים היו יציבות ומדויקות וניתן היה לקבל מידע א
ס''מ מקיר הייחוס, כך נקבל מדידות מדויקות לשגיאות לשני הכיוונים. 30-לכן בחרנו לנסוע במרחק של כ
.( 47עמ' – 2ראה נספח מספר ) הכיול הבוצע במעבדה נמצא תואם לגרף כיול הנמצא במפרט החיישן
בודד מושפע מהפרעות ואינו יציב, ולכן הוחלט מבדיקות נוספות של החיישנים נמצא כי האות המתקבל מחיישן
:לעבוד בקונפיגורציה הבאה
מאחור, באופן הזה ניתן לחשב ממוצע קריאות של כל זוג חיישנים ולקבל מדידה 2-בקדמת הרכב ו 2חיישנים 4
אמינה יותר ובכך לקבל מיקום וזוית מדויקים יותר.
מערכת בניסויים בחנו את תגובת היעות וניסויים עם הרכב, חיישנים אלו נרכשו והיו בשימוש, ביצענו מספר נס
למדידות שהתקבלו מהחיישנים.
ית ומרחק ו, מתנאיי התחלה שונים )זוקיר הייחוסמ בקו ישר, במרחק קבוע ת הרכב בנסיעהבדקנו את תגובתחילה
ות על מערכת הבקרה.סימולציהרכב מתכנס למסלול כפי שצפינו מ אםיתה לראות ימטרת הבדיקה ה ביחס לקיר(.
דו במצלמת וידאו.הניסויים תוע, וקמ''ש 3-5 ,במהירות נמוכה הנסיעות הראשונות בוצעו
הצילומים ניתן היה לראות כי הרכב לא מתכנס למסלול ואינו מתנהג בצורה המתאימה לתוצאות הסימולציות, תוך מ
הרכב הגיב בצורה חזקה מידי לתנאי ההתחלה.
ת המדמות יותר את תוצאו מידול המערכת אינו מדויק, ועל מנת לקבליסוי הנסיעה היו כי מנ הראשונות המסקנות
את מאמץ הבקרה כך שהרכב לא יגיב בצורה קיצונית לתנאי ההתחלה. הסימולציות יש להקטין
הגענו למסקנה כי הבעיה אינה במידול המערכת או במאמץ בתוצאות, לאחר מספר רב של ניסויים ללא שיפור
ה.הבקר
בשלב זה חזרנו לבצע ניסויים על החיישנים.
טיב המדידה של מרחק מקיר ייחוס, וזווית ביחס לקיר הייחוס.התמקדנו בהחיישנים בניסויי
אך בווקטור המצב קיימת גם הנגזרת של זווית הרכב ביחס לקיר.
2הנגזרת מחושבת על פי הגדרה, הפרש המדידות חלקיי הזמן בין המדידות 1
t
.
שניות, כל שגיאת מדידה מוכפלת פי אלף. 0.001-קצר מאוד, כ מכיוון שהזמן בין המדידות
ר סדרי גודל מהנגזרת האמתית.ל כחצי מעלה באחת מהמדידות, נקבל נגזרת הגדולה במספשגיאה שלכן ב
שגיאה גדולה בנגזרת הזווית.אומנם שגיאה של כחצי מעלה נסבלת מבחינתנו במציאת זווית הרכב, הדבר גורם ל
בשלב זה החלטנו להוריד את מדידת נגזרת הזווית מווקטור המצב.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
12עמוד S.A.M - Smart Autonomous Maneuvering
ניסוי נסיעה במהירויות נמוכות, ללא התייחסות לנגזרת הזווית.בשלב הבא חזרנו על
בדקנו את תגובת הרכב בנסיעה בקו ישר, במרחק קבוע מקיר הייחוס, מתנאיי התחלה שונים.
אל המסלול התכנסיה קטנה מאוד והרכב י, המרחק הרצוי מהקיר נשמר בסטתה טובה מאודיובת הרכב היתג
תוצאות הניסוי היו דומות מאוד לתוצאות שצפינו מהסימולציות על .מתנאיי ההתחלה השונים המוגדר במהירות
מערכת הבקרה.
על מנת לראות את תגובת הרכב ומלייםרנדלעקום בעל קימורים לוללאחר מכן החלטנו לשנות במעט את המס
. לצורך כך הקמנו מערך "קירות" אלסטיים מפלקטים המחולקים לשינויים במסלול ולא רק לתנאי התחלה
למקטעים ובעזרתם יצרנו את המסלול הרצוי.
גם לשינויי עקמומיות בה מאוד הרכב הגיב בצורה טוות באותן מהירויות כמו קודם לכן, ראינו שלאחר מספר נסיע
.המסלול
ולבדוק שוב את שתי הרכב בעקבות התוצאות הטובות ומתוקף דרישות הנסיעה, החלטנו להעלות את מהירות
תצורות המסלול.
קמ''ש הרכב נסע בצורה טובה מאוד, כאשר הוא מתגבר על תנאי ההתחלה השונים ועל שינויי 8-מהירות של כעד
.ולל שינויים מקעירות לקמירות לאורך המסלולעקמומיות כהרדיוס
קמ''ש הרכב נכשל במעבר המסלול. 10-במהירות של כ
רכב מיקום ה קריאת התברר כי ישנה השהיה בין בצילומי הניסוי התבוננות באמצעותניתוח הנסיעות הנ"ל ב
הדרוש לא התבצע תיקוןגם הולכן כלומר מערכת הבקרה קיבלה את המידע על הרכב באיחור, המיקום האמיתיו
. והרכב התבדר מהמסלול בזמן
למרות ניסיונות לשינוי פרמטרי הבקרה על מנת לגרום למערכת להגיב באופן הרצוי, תגובת המערכת לא השתפרה.
חזרנו פעם נוספת לביצוע ניסויים על החיישנים, כאשר הפעם בדקנו את קצב העבודה שלהם על מנת לבדוק אם
מדידה עצמה או במערכת הרכב.ההשהיה נוצרת ב
מסלול קימורים – 2איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
13עמוד S.A.M - Smart Autonomous Maneuvering
תוצאות הניסוי היו חד משמעיות, החיישנים אינם עובדים בקצב מספק עבור נסיעה במהירויות גבוהות.
]20 -החיישנים עובדים בתדר של כ ]Hz 10לכן בקצב נסיעה של[ / ]m s מקבלים אינדיקציה למיקום הרכב רק
יקת.מנת לעקוב אחר המסלול בצורה מדו על איטי מידי קצב דגימה זהי מטר. כל חצ
בו הרכב , כך שבמקרה ס''מ 30 -קטן, כ הואחיישנים אלו המרחק שנשמר מהקיר עקב תחום העבודה של בנוסף
היא הנסיעהמהירות ון( ית המקסימלית המתאפשרת בשימוש בסרוו הנתוו)הזו 30בזווית מקסימלית שלנמצא
10[ / ]m s ס''מ אל עבר הקיר לפני עדכון המיקום הבא. 25 -נקבל התקדמות של כ
כך שלא ניתן לעקוב אחר המסלול במהירויות גבוהות תחת עמידה באילוצי הבטיחות.
המחשה:
.פה לחיישנים אלומהסיבות הנ"ל הוחלט למצוא חלוכתוצאה
דרישה לחלופה:
]100 -) גדול מ יעבדו בקצב דגימה מהירש חיישני מרחק ]Hz על מנת לקבל אינדיקציה טובה מספיק.)
תחום עבודה גדול יותר על מנת לשמור על מרחק גדול יותר מהקיר לצורכי בטיחות הנסיעה.
°30
30 cm
V=10 m/s
קיר ייחוס
רכב
וית מקסימלית ביחס לקירזו – 3איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
14עמוד S.A.M - Smart Autonomous Maneuvering
ר מסתובבוחיישן לייזחיישני מרחק מבוססי לייזר ב שימוש .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איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
15עמוד S.A.M - Smart Autonomous Maneuvering
מאפייני מנוע סרוו:
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איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
16עמוד S.A.M - Smart Autonomous Maneuvering
מסתובב על מנת לתגבר את הרכב בחיישן לייזר שנבחרו חיישני המרחק מבוססי הלייזר נשקלה אפשרות לאחר
להרחיב את יכולות הרכב.
לקבל תמונה מרחבית טובה יותר, יכולת לזהות מכשולים כך ניתןן זה יעבוד בנוסף לחיישני הלייזר והוחלט שחייש
במרחב, ובנוסף החיישן יעזור בביצוע מעבר הפינה.
חיישן לייזר מסתובב:
מאפיינים:
RPLIDAR - A1M1דגם:
RoboPeakיצרן:
(סורק)מנוע, V 5 מתח עבודה:
סורק()מנוע, mA 100 ,130 זרם:
מ"מ 100 :אורך
מ"מ 70 :רוחב
מ"מ 58גובה:
m 0.2-6תחום עבודה:
]10קצב דגימה: ]Hzup to
וית ביחס והמאפשר לקבל מידע על מרחק מן העצם והז,Triangulation Ranging על עיקרון מבוססחיישן זה
ן:לחייש
מיטר )יחידת הארה(.בשיטה זאת העינית ממוקמת במרחק קבוע לצד הא
ידוע, והזווית בין האימטר לעינית המרחק. האמיטר, העינית והנקודה בה פוגעת קרן הלייזר יוצרות ביחד משולש
.יתוולזו מרחקמיקום פגיעת הקרן בעינית נותן אינדיקציה ל
. °360 -החיישן בעל מנוע מובנה המאפשר סריקה ב
, 10Hz -נמוך יחסית עד כ הקצב העבודה של חיישן ז
אך מאפשר לקבל מידע רב על הסביבה מכיוון שהסריקה היא היקפית.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
17עמוד S.A.M - Smart Autonomous Maneuvering
בעזרת חיישני הלייזר נסיעההתכן שלבי
1שלב
אשר נעים בעזרת LiDAR liteהרכב ישמור על מרחק רצוי מהקיר בעזרת מדידות משני חיישני לייזר קווים מסוג
מנועי סרוו.
המחשה:
רכב קיר ייחוס
מדידת מרחק ע"י חיישני לייזר – 7איור
מיקום חיישני לייזר על גבי הרכב – 6איור
חיישן לייזר מסתובב
לוח אם-מיקום הלוח הראשי של הרכב
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
18עמוד S.A.M - Smart Autonomous Maneuvering
2שלב
תהליך זיהוי ומעקף הפינה ייעשה לפני הגעה לפינה וייעשה בעזרת שני חיישני הלייזר וחיישן לייזר מסתובב
המחשה:
3שלב
אל נקודת העגינה תוך נה אל קדמת הרכב בעזרת מנוע סרוו, וכך ימדוד את המרחק חיישן הלייזר הקווי הקדמי יפ
ות ושינוי מהירות על פי הצורך עד עצירה בטוחה בתחנת העגינה. כדי התקדמ
רכב קיר ייחוס
זיהוי פינה בעזרת חיישן לייזר מסתובב – 8איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
19עמוד S.A.M - Smart Autonomous Maneuvering
בקרת מסלול
.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 .
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
20עמוד S.A.M - Smart Autonomous Maneuvering
הצבה לתוך פונקציית המחיר תיתן לנו את הביטוי הבא:
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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
21עמוד S.A.M - Smart Autonomous Maneuvering
דרישות הבקרה:
.חזרה מהירה ככל שניתן למסלול (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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
22עמוד S.A.M - Smart Autonomous Maneuvering
. Xזהה לכל וקטור המצב Wקטור משקלים ונתחיל מו
קת מספיק למדוד את שנכון לרגע זה לא מצאנו דרך מדוימכיוון 0-כאשר את המשקל על שינוי קצב הזווית נציב כ
פרמטר זה.
1 1 1 0 1W
1קום והזווית בזמן, סימולציה תיאור שגיאות המי – 1 גרף
1תיאור מאמץ הבקרה בזמן, סימולציה – 2 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
23עמוד S.A.M - Smart Autonomous Maneuvering
, אם נאריך את הסימולציה שגיאת הזוויתל ביחס לאטלראות כי שגיאת המיקום לא מתכנסת בגרף הראשון ניתן
.שניות 30-נראה התכנסות לאחר כ
צה להעלות את משקל שגיאת המיקום.לכן נר
בנוסף גרף שגיאת המיקום מאוד חלק, לכן נרצה גם להקטין את חשיבות קצב שינוי שגיאת המיקום.
מעלות( 30לא סטינו ממאמץ הבקרה המקסימלי של המערכת ) בגרף השני ניתן לראות כי
יים לא נשנה את משקל מאמץ הבקרה.לכן בנת
סימולציה שנייה:
עבור וקטור המשקלים הבא: 5 0.5 1 0 1W
2תיאור שגיאות המיקום והזווית בזמן, סימולציה – 3 גרף
2תיאור מאמץ הבקרה בזמן, סימולציה – 4 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
24עמוד S.A.M - Smart Autonomous Maneuvering
את המיקום וגם שגיאת הזווית מתכנסות בפחות ניתן לראות בבירור את השיפור מהסימולציה הראשונה, גם שגי
משתי שניות.
שניות גרף שגיאת המיקום אינו חלק. בנקודה זאת שגיאת הזווית הייתה גדולה ולכן 0.2ניתן לראות כי לאחר
נעשה תיקון לכיוון ההפוך. ניתן לראות זאת גם מגרף מאמץ הבקרה.
קלי השגיאות. על חלקות גרף שגיאת המיקום נרצה לשמור, בשביל להימנע ממצב זה נקטין חזרה את היחס בין מש
לכן נשמר את יחס המשקלים בין שגיאת המיקום ונגזרתה.
אות לכן לא נשנה את משקלו. מאמץ הבקרה עדיין נשאר בתחום שבו נרצה
סימולציה שלישית:
עבור וקטור המשקלים הבא: 1.1 0.05 1 0 1W
3תיאור שגיאות המיקום והזווית בזמן, סימולציה – 5 גרף
3ימולציה תיאור מאמץ הבקרה בזמן, ס – 6 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
25עמוד S.A.M - Smart Autonomous Maneuvering
ניתן לראות בבירור את השיפור בתוצאות. שגיאת המיקום מתכנסת בצורה חלקה בדומה לתנועה של כלי רכב אשר
משתלב בנתיב בתנועת "גלישה".
אם נמצא דרך למדוד את קצב שינוי זווית הרכב בצורה מדוייקת נוכל לשפר עוד יותר את ביצועי הנסיעה.
סימולציה רביעית:
בסימולציה הבאה ניתן משקל גם לחלקות התכנסות שגיאת הזווית.
1.1 0.05 1 0.02 1W
ההתכנסות קצת יותר איטית. אך אנחנו נמנעים מתגובת יתר בהתכנסות שתי השגיאות מה שמדמה בצורה הטובה
ביותר את השתלבות רכב למסלול נסיעה.
4תיאור מאמץ הבקרה בזמן, סימולציה – 7 גרף 4תיאור שגיאות המיקום והזווית בזמן, סימולציה – 8 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
26עמוד S.A.M - Smart Autonomous Maneuvering
תכן אלקטרוני
וגי לדיגיטליכרטיס ממיר אנל
)ממיר מאנאלוגי לדיגיטלי( ADCלוח תכננואדום פרהלקרוא את קריאות חיישני האינעל מנת
ניסות אנאלוגיות של כ מספקתקריאות אנאלוגיות, מכיוון שאין לנו במעבד כמות 32אדום נותנים החיישני האינפר
בנינו את המעגל.
תפקוד המעגל:
.MCP3208לדיגיטלי מדגם כל חיישן מחובר לממיר אנאלוגי
אל המעבד.אשר מחובר הממירים מחוברים לפורט
. כאשר המעבד בוחר מאיזה חיישן לקרוא, ואיזה SPIהתקשורת בין המעבד לרכיבים מתבצעת בעזרת תקשורת
ערוץ לקרוא מתוך החיישן.
כולל מחברים ותושבות: ADC -לוח ה
48עמ' – 4*לשרטוט הלוח ראה נספח מספר
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
27עמוד S.A.M - Smart Autonomous Maneuvering
כרטיסי תקשורת
מטרת התקשורת האלחוטית היא לשמור את תוצאות הניסויים כך שנוכל לנתח אותן ולמצוא איפה היו תקלות ומה
היו גורמים.
בנוסף לאפשר יכולת לשליטה מרחוק על המערכת.
:בעזרתו ניתן היה לבצע מספר פעולות Xbeeמימשנו תקשורת בעזרת רכיב תחילה
תחילת נסיעה וקביעת המהירות לתת פקודת (1
שינוי מהירות תוך כדי נסיעה (2
פקודת בלימת חירום (3
שינוי ערכיי הבקר (4
התגלה כלא אמין, סבל מניתוקים רבים ולא עמד בטווח בו מוצהר במפרט. Xbeeלאחר מספר שימושים ה
יתן לקבל את תוצאות בנוסף לא הייתה יכולת להעביר מידע מהמעבד על הרכב אל המחשב. ובצורה זאת לא היה נ
הניסוי ולעבד אותן.
.NRF24L01כרגע אנחנו עובדים עם כרטיסי תקשורת מסוג
בעזרת כרטיסים אלו אנו יכולים לשלוח מידע אל הרכב ולקבל חזרה אל קופסת הפיקוד.
הרכב מקבל פקודה הכוללת את המהירות הרצויה והמרחק הרצוי
ים:הנתונים הבאאת ומשיב חזרה אל הקופסא
וקטור המצב )שגיאת מיקום וזווית( (1
מהירות הרכב (2
זמן ריצה (3
מרחק שעבר (4
מאמץ הבקרה )זווית ההיגוי( (5
מתח הסוללה (6
נתונים אלו נשמרים בכל סוף ניסוי וכך ניתן לנתח את תוצאות הניסוי בצורה מדויקת ולזהות באילו נקודות הרכב
נתקל בבעיות בעקבה אחר המסלול.
רכב גם את קואורדינטות המסלול בזמן אמת.בעתיד ניתן להעביר ל
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
28עמוד S.A.M - Smart Autonomous Maneuvering
בטיחותי מנגנונ
על מנת למנוע תאונות הכנסנו מספר מנגנוני הגנה שייעצרו את נסיעת הרכב במקרה ואחד מהרכיבים מפסיק פעולה
במהלך הנסיעה.
תקינות האנקודר. .1
תקשורת פעילה בין הרכב לקופסת הפיקוד. .2
.נמוךמתח סוללה .3
הנסיעה.הגבלת מרחק .4
תקינות אנקודר:
נטגרציה לקבל גם את מרחק הנסיעה.תפקיד האנקודר במערכת הוא לתת אינדיקציה על מהירות הנסיעה ובעזרת אי
קריאת האנקודר קריטית מכיוון שהרכב ייעלה את מאמץ הבקרה במטרה להגיע למהירות הרצויה עד שקריאת
האנקודר תראה כי המהירות הושגה.
מקרה בו האנקודר לא מתפקד בצורה תקינה הרכב ייעלה את מאמץ הבקרה עד למצב בו הרכב נוסע במהירות ב
קמ''ש. כמובן שמצב זה מסוכן מאוד כפי שניתן לראות בתמונה למעלה. 70-מקסימלית של כ
הרכב ברמת מנגנון הכשל כרגע מתפקד ברמת התוכנה, כאשר בעתיד מתוכנן להוסיף רכיב בדיקת מתח שינטרל את
החומרה.
הרכב לאחר תאונה – 10איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
29עמוד S.A.M - Smart Autonomous Maneuvering
אופן תפקוד המנגנון:
סיעה מרגע הפקודה למהירות הרצויה.ביצענו ניסוי למצוא את הקשר בין הזמן הלוקח לרכב להתחיל נ
להלן התוצאות:
בתוך התוכנה יצרנו קטע הבודק האם הרכב התחיל לנסוע בזמן המתאים למהירות שהכנסנו.
כב נוסע אזי הוא אינו מתפקד באופן תקין ולכן נפסיק את הנסיעה.אם האנקודר לא מזהה כי הר
מימוש הקוד:
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 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
30עמוד S.A.M - Smart Autonomous Maneuvering
בדיקת תקשורת:
בשביל שתמיד תהיה לנו היכולת להפסיק את נסיעת הרכב באופן פתאומי מוכרחת להיות תקשורת פעילה בין
קופסת הפיקוד והרכב.
אופן רציף. לכן מימוש המנגנון פשוט למדי.קיימת תקשורת פעילה הלוך ושוב בין הקופסה לרכב ב
אם לא מתקיימת תקשורת במשך יותר מעשירית שנייה נפסיק את פעילות הרכב באופן מידי. )התקשורת מתבצעת
בקצב מאוד גבוה לכן עשירית שנייה זה זמן מספק לבדיקה(.
שידור של כרטיס התקשורת.כך הרכב ייבצע עצירת חירום במקרה בו התקשורת נפלה או שהרכב ייצא מטווח ה
מימוש הקוד:
תח סוללה:בדיקת מ
כרגע מתח הסוללה נמדד עם רכיב ייעודי אשר מודד את המתח ומתחיל לצפצף ברגע שהמתח יורד מתחת לסף.
בעתיד מתוכנן רכיב מודד מתח שיעביר את קריאת המתח למעבד וכך נוכל ברמת התוכנה לעצור את הרכב כשמתח
וללה יורד.הס
השיפור יהיה בכך שהרכב יעצור מידית כאשר המתח ירד ובנוסף תהיה לנו אינדיקציה קבועה למתח הסוללה ונוכל
ללמוד את התנהגות הסוללה לאורך זמן.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
31עמוד S.A.M - Smart Autonomous Maneuvering
הגבלת מרחק הנסיעה:
בעזרת קריאת האנקודר ניתן בעזרת אינטגרציה לקבל את מרחק הנסיעה.
ה גם ללא קופסת הפיקוד והרכב ייעצר לאחר המרחק הרצוי.בצורה זאת ניתן לבצע נסיע
המרחק מחושב בצורה הבאה:
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
מימוש הקוד:
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
32עמוד S.A.M - Smart Autonomous Maneuvering
קופסת פיקוד
בזמן אמת, לתקשר עם הרכב ממרחק ולבצע את כל הפעולות הנדרשות ממרחק, כגון שליחת פקודות לרכב מנתל ע
התחלת נסיעה, הפסקת נסיעה, הזנת מהירות, איסוף נתונים ושמירתם, תוכננה קופסת פיקוד לרכב.
בקופסת הפיקוד ישנו מסך, שמאפשר קריאת נתונים בזמן אמת על הרכב, כגון מצב סוללה, מהירות, תאוצה, מאמץ
ופעולות נוספות באמצעות , המשמש להזנת הנתונים הנדרשיםו לוח מקשים וסף ישנבקרה ועוד נתונים כרצוננו. בנ
. י לחיצה עם ניצרת בטיחות ואנטנהשל המערכת כולה, שני כפתורי לחיצה, שני כפתור on/offהמקשים, מפתח
עת כל התקשורת בין צבאמצעות רכיב זה מתבתקשרת היא חלק חשוב בקופסא, האנטנה, שמגיעה כמובן עם כרטיס
הרכב לבין קופסת הפיקוד.
ע"מ לשמור את הנתונים שהרכב משדר, אך הוחלט להעבירו לרכב SDלהחזיק כרטיס זיכרון גם תוכננההקופסא
עצמו מסיבות שיפורטו בהמשך.
שניתנת להחלפה, ניתן לספק גם לקופסא מתח ע"י שנאי חיצוני. 9Vלקופסא מסופק מתח ע"י סוללה של
קופסת הפיקוד – 11איור
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
33עמוד S.A.M - Smart Autonomous Maneuvering
ם מנחים מאוד משמעותיים. תחילה הבנו כי על הקופסא במהלך תכנון הקופסא היה עלינו להתחשב במספר אלמנטי
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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
34עמוד S.A.M - Smart Autonomous Maneuvering
מודול תקשורת:
מאפיינים:
+nRF24L01 דגם
SparkFunיצרן
3.3-7Vמתח עבודה
0dB+עוצמת שידור
250Kbps-2Mbps קצב שידור
מטר 100 : מרחק מקסימלי
אל : אנטנה פנימית
-SPI תקשורת
מ"מ 20.3 :אורך
מ"מ 17.8 :רוחב
Nordicשל חברת +nRF24L01זהו מודול תקשורת )משדר/מקלט( המבוסס על רכיב
Semiconductor עם מחברSMA -RP לחיבור אנטנה חיצונית.
.חוטיים כמו מקלדות, עכברים וכדומה-נפוצים מאוד במוצרים אל Nordicרכיבים של
לאספקת מתח לרכיב וכל הקווים שלו יכולים להתממשק עם 3.3Vמייצב מתח של כרטיס זה כולל
.5Vבקרים הפועלים על
אנטנה:
מאפיינים:
2.4GHzתדר:
5dBi :הגבר
מ"מ 205 :אורך
מ"מ 13 :קוטר
האנטנה עוזרת לנו לשפר את טווח הקליטה והשידור בין הקופסא לרכב עצמו.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
35עמוד S.A.M - Smart Autonomous Maneuvering
מפסק עם מנעול:
, אין אספקת מתח לקופסא, מטרתו העיקרית למנוע הפעלה לא מכוונת של offכל אימת שמפסק זה נמצא על מצב
הקופסא ובכך שליחת נתונים ופקודות לרכב ולגרום שלא במכוון להפעלתו. בנוסף, ברכב עצמו ישנו מנגנון
בטיחות שכאשר הוא לא מזהה תקשורת עם הקופסא הוא מיד מפסיק לפעול.
כפתורים: 12מקשים לוח
מאפיינים:
מ"מ 72אורך:
מ"מ 52רוחב:
12מספר מקשים:
באמצעות לוח מקשים זה המשתמש יכול לשלוט על ממשק הקופסא ולנווט בתפריט ההפעלה וכמובן לבצע את שלל
הפעולות שבאפשרותו לבצע.
:LCDמסך
מאפיינים:
20 :מספר תווים
4 :מספר שורות
5x8 pixelsגודל תווים:
שחור :צבע תווים
ירוק :צבע רקע
5Vמתח עבודה:
I2C :תקשורת
מ"מ 105 :אורך
מ"מ 60 :רוחב
תווים המוצגים בצבע 20שורות של 4עם יכולת תקשורת טורית. מסך זה הינו בעל LCDזהו מסך
שחור על רקע ירוק ומיחידת תקשורת טורית על מעגל אחד.
ונים שאנו מקבלים מהרכב כגון: מהירות, תאוצה, מצב סוללה ועוד.באמצעות מסך זה אנו מציגים נת
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
36עמוד S.A.M - Smart Autonomous Maneuvering
: 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
לוח אם לקופסא:
להימנע מחוטים המעבד שלה ו ם של הקופסא לביןותר בין הרכיבילאפשר חיבורים נוחים י מנתל ע
יעודים לכל רכיב.בתוך הקופסא תוכנן לוח אם לקופסא שעליו יושב המעבד ועליו יש חיבורים חופשיים
מגעים וללא ניתוקים אם מאפשר חיבורים קבועים ועמידים לאורך זמן, ללא בעיותהלוח בנוסף
פתאומיים.
מתח של המייצב צב מתח אותו בחרנו להוסיף משום שמתח של המעבד לרכיבים עובר דרך מייהקו
ת בין מתח שמסופק מהמעבד למנוע התנגשויו ל מנתמסוגל לספק מספיק זרם, בנוסף, עהמעבד עצמו אינו
לבין מתח שמסופק ממקור חיצוני )במידה ובמחובר( שמנו דיודה שלא מאפשר אספקת מתח משניהם.
להלן שירטוט של הלוח:
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
37עמוד S.A.M - Smart Autonomous Maneuvering
ניסויים
הניסויים היוו כלי למידה מרכזי בכל הנוגע לתפקוד הבקרה נסיעות. ניסויי במהלך העבודה ביצענו עשרות
ן לא הייתה תקשורת לא הייתה יכולת לשמור על התוצאות מלבד תיעוד במצלמת הוידאו. מכיוון שעדייוהחיישנים.
י הליכה עם מחשב נייד לצד הרכב(.)מספר קטן של נסיעות במהירויות איטיות כן נשמרו על יד
ניסוי שינוי מהירות
למהירות אפס. הזמן הנדרש לחזורו מהירות תגובת הרכב לדרישת מהירות שונה, בדיקת - מטרת הניסוי
תוצאות הניסוי:
מתואר אות הרפרנס אליו אנחנו רוצים להגיע. בכתום
מסקנות:
ניתן לראות כי תחילת נסיעה לוקחת יותר זמן משינוי מהירות.
קמ''ש ניתנת לאחר שלוש שניות, כפי שניתן לראות לרכב לוקח כשנייה להגיב לפני 5-פקודת תחילת הנסיעה ב
מתחיל להאיץ. פעולת ההאצה מהירה וההתכנסות למהירות קבועה מהירה. כאשר מהירות הרכב תונדת סביב שהוא
קמ''ש. 1-המהירות המוגדרת ב
בצורה כמעט מידית. יי המהירויות תוך כדי נסיעה נעשית שינו
בהתחשב בכך שהרכב במהלך הניסוי באוויר.קמ''ש לעצירה נעשית בפחות משתי שניות. תוצאה טובה 8-האטה מ
הרכב וחיכוך עם הרצפה. אין האטה ממשקל
יש לסייג את תוצאות הניסוי מכיוון שהרכב אינו על הקרקע. הגלגלים למעשה באוויר ולכן אין התנגדות.
.על הקרקעמכיוון שלרכב לא חסר כח במהירויות אלו לא נצפה להבדלים גדולים בנסיעה
מהירות הרכב בקמ"ש כתלות בזמן – 10 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
38עמוד S.A.M - Smart Autonomous Maneuvering
יישניםניסויי ח
חיישני אינפרה אדום
.כיול חיישן 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 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
39עמוד S.A.M - Smart Autonomous Maneuvering
. קריאת חיישן במצב סטטי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 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
40עמוד S.A.M - Smart Autonomous Maneuvering
ית ממערך החיישניםו. קריאת זו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 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
41עמוד S.A.M - Smart Autonomous Maneuvering
ית משתנה(וית ממערך החיישנים במצב דינאמי )זוו. קריאת זו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 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
42עמוד S.A.M - Smart Autonomous Maneuvering
חיישני לייזר
כיול חיישני לייזר
ר את המרחק כתלות בקריאת החיישן.כיול החיישנים על מנת לקבל עקום המתא – מטרה
תוצאות:
בעזרת עקום הכיול ניתן להמיר בין הקריאה למרחק כפי שבוצע בתוכנית )קוד( הרכב.
(. 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 גרף
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
43עמוד S.A.M - Smart Autonomous Maneuvering
מקורות
פרויקט גמר קודם .1 , תכן בקרת מהירות.29ראה עמ' PIDלהרחבה בנושא בקר המהירות מסוג
, בקרת מיקום רוחבי.21ראה עמ' LQRלהרחבה בנושא מודל בקרת
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
44עמוד S.A.M - Smart Autonomous Maneuvering
נספחים:
. כיול חיישן לייזר שני1
שיפוע הגרף כמעט זהה לחלוטין לחיישן הקדמי.
יימת הסטה בייחס לחיישן הקדמי, ההסטה נוצרת כנראה ממדידה ממיקום שונה בשני הניסויים. כאשר נשים לב כי ק
בניסוי הראשון המדידה נעשתה מפני העדשה ובניסוי השני מבסיס החיישן.
יש לסדר את בעיית ההסטה לפני תחילת נסיעה עם החיישנים.
y = 1.014x - 7.2089
R² = 0.998
0
50
100
150
200
250
0 50 100 150 200 250
םרי
טמ
טיסנ
בק
חרמ
קריאת החיישן
כיול חיישן לייזר אחורי
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
45עמוד S.A.M - Smart Autonomous Maneuvering
כיול חיישן אינפרה אדום ע"י היצרן. 2
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
46עמוד S.A.M - Smart Autonomous Maneuvering
. דיאגרמת חיבור חיישני לייזר3
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
47עמוד S.A.M - Smart Autonomous Maneuvering
ADCשרטוט לוח .4
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
48עמוד S.A.M - Smart Autonomous Maneuvering
דיאגרמת חיבורים כרטיסי תקשורת .5
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
49עמוד S.A.M - Smart Autonomous Maneuvering
מודל קופסת פיקוד .6
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
50עמוד S.A.M - Smart Autonomous Maneuvering
קודים
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 EADC2=23; //ADC 2 CS pin
int EADC3=24; //ADC 3 CS pin
int EADC4=25; //ADC 4 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
change in position rate
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;
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
51עמוד S.A.M - Smart Autonomous Maneuvering
unsigned long time2=1;
unsigned long time3=1;
unsigned long time4=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(EADC3,OUTPUT);
pinMode(EADC4,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(EADC3,HIGH);
digitalWrite(EADC4,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++)
{
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
52עמוד S.A.M - Smart Autonomous Maneuvering
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);
}
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
53עמוד S.A.M - Smart Autonomous Maneuvering
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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
54עמוד S.A.M - Smart Autonomous Maneuvering
digitalWrite(SPICLOCK,LOW);
digitalWrite(SPICLOCK,HIGH);
digitalWrite(SPICLOCK,LOW);
//read bits from adc
for (int i=11; i>=0; i--)
{
adcvalue+=digitalRead(DATAIN)<<i;
//cycle clock
digitalWrite(SPICLOCK,HIGH);
digitalWrite(SPICLOCK,LOW);
}
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();
byte Speedbytes2=Serial.read();
byte Speedbytes3=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");
}
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
55עמוד S.A.M - Smart Autonomous Maneuvering
. קוד קופסת פיקוד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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
56עמוד S.A.M - Smart Autonomous Maneuvering
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]);
}
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
57עמוד S.A.M - Smart Autonomous Maneuvering
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;
}
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
58עמוד S.A.M - Smart Autonomous Maneuvering
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();
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
59עמוד S.A.M - Smart Autonomous Maneuvering
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*/
{
DataOut.MotorSpeedD1=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.
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
60עמוד S.A.M - Smart Autonomous Maneuvering
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(" ");
dataFile.print(Data.e1dot);
dataFile.print(" ");
dataFile.print(Data.e2);
dataFile.print(" ");
dataFile.print(Data.e2dot);
dataFile.print(" ");
dataFile.print(Data.Speed);
dataFile.print(" ");
dataFile.print(Data.WheelDist);
dataFile.print(" ");
dataFile.print(Data.CtrlEff);
dataFile.print(" ");
dataFile.println(Data.Voltage);
}
}
if (millis()>DispTime+1000)
{
lcd.setCursor(0,0);
lcd.print("Speed: ");
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
61עמוד S.A.M - Smart Autonomous Maneuvering
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;
DataOut.MotorSpeedD1=0;
delay(3000);
lcd.clear();
}
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
62עמוד S.A.M - Smart Autonomous Maneuvering
. קוד מרכזי 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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
63עמוד S.A.M - Smart Autonomous Maneuvering
#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
change in position rate
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
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
64עמוד S.A.M - Smart Autonomous Maneuvering
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();
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
65עמוד S.A.M - Smart Autonomous Maneuvering
/***************************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 ***************************/
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
66עמוד S.A.M - Smart Autonomous Maneuvering
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);
}
}
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
67עמוד S.A.M - Smart Autonomous Maneuvering
/****************** 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(" ");
טכניון מכון טכנולוגי לישראל
הפקולטה להנדסת מכונות
פרויקט גמר
68עמוד S.A.M - Smart Autonomous Maneuvering
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);
}
}
}