การคำนวนระยะทางของ robot

ผมมีจุดที่ robot จะวิ่งไปดังรูป

สั่งให้ robot วิ่งจากจุด 1 ไปยังจุด 2 ,3,4,6 แล้วกลับมาจะ 1 ใหม่ วนอย่างนี้ไปจนกว่า sensor จะเป็นตัวกำหนดให้ robot หยุด
ในระหว่างที่ robot วิ่งไปตามจุดต่างๆ ผมก็สั่ง protocol read ค่า XY เข้ามาเก็บใน array ไว้
เมื่อ robot หยุดแล้วผมจะได้ข้อมูลมาก้อนหนึ่ง ซึ่งผมจะเอามาคำนวนระยะทางที่ robot วิ่งไปได้ โดยใช้สมการ  

                     
                    ค่า XY ที่ได้เอามาแสดงเป็น XY graph

คำนวนค่าใน array แล้วนำมารามกันก็จะได้ระยะทางทั้งหมด
ปัญหาคือ ตอนที่ robot ทำ arc จุด 1,2,3 (4,5,6)การคำนวนของข้อมูลช่วงนี้มันจะมี error
ช่วงข้อมูลจุด 3 ไป 4 และ จุด 6 ไป 1 ไม่มีปัญหา
ผมไม่สามารถ sampling ให้ให้ระเอียนกว่านี้ได้ ติดปัญหาที่ baud rate ไม่สามารถทำได้เยอะกว่านี้แล้ว

คำถามคือ จะมีวิธีการไหนบ้างที่จะคำนวนได้แม่นยำกว่านี้ครับ เพราะวิธีของผมมัน error ไป 8cm
แสดงความคิดเห็น
โปรดศึกษาและยอมรับนโยบายข้อมูลส่วนบุคคลก่อนเริ่มใช้งาน อ่านเพิ่มเติมได้ที่นี่