כיוון עם Visual Servoing

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

באמצעות מעקב ראייה בקצב פריימים גבוה, כעת ניתן להשתמש בצינור הראייה ישירות כ"חיישן" בלולאת בקרת PID כדי להנחות את הרובוט או הצריח שלכם. כדי לבדוק את הרעיון הזה הוספנו limelight לרובוט FRC 2017 שלנו וגרמנו לו לכוון למטרות ראייה באמצעות מערכת ההנעה ונתוני טבלת הרשת שמדווחים על ידי ה-limelight בלבד.

בדוגמה זו, מועמד הבדיקה שלנו היה רובוט FRC 2017 שמשתמש במערכת הנעה של 6 גלגלים עם גלגלי colson. הנה תמונה שלנו מוסיפים limelight לרובוט כדי לבצע את הבדיקה הזו.

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

float Kp = -0.1f;  // קבוע בקרה פרופורציונלי

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = tx;
    steering_adjust = Kp * tx;
    
	left_command+=steering_adjust;
    right_command-=steering_adjust;
}

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

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

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

float Kp = -0.1f;
float min_command = 0.05f;

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = -tx;
	float steering_adjust = 0.0f;
	if (Math.abs(heading_error) > 1.0) 
	{
		if (heading_error < 0) 
		{
			steering_adjust = Kp*heading_error + min_command;
		} 
		else 
		{
			steering_adjust = Kp*heading_error - min_command;
		}
	} 
	left_command += steering_adjust;
	right_command -= steering_adjust;
}

היזהרו, אם תגדירו Kp או min_command גבוה מדי, הרובוט שלכם יכול להיות לא יציב ויכול להתנדנד קדימה ואחורה כשהוא חורג מהמטרה:

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