นี่คือรายละเอียดการสอนเกี่ยวกับวิธีการตระหนักถึงหุ่นยนต์ที่เริ่มต้นจากศูนย์และให้ความสามารถในการนำทางด้วยตนเองในสภาพแวดล้อมที่ไม่รู้จัก
ข้อโต้แย้งทั่วไปที่เกี่ยวข้องกับวิทยาการหุ่นยนต์จะได้รับการคุ้มครอง: กลศาสตร์ , อิเล็กทรอนิกส์ และ การเขียนโปรแกรม .
หุ่นยนต์ทั้งหมดได้รับการออกแบบให้ทำโดยทุกคนที่บ้านโดยไม่มีเครื่องมือและอุปกรณ์ระดับมืออาชีพ (เช่นแพง)
กระดานสมอง (dsNav ) ใช้ Microchip DSPIC33 DSC พร้อมความสามารถในการเข้ารหัสและตัวควบคุมมอเตอร์ ตำแหน่งนั้นคำนวณโดย odometry (encoder) โดยไม่มีการอ้างอิงภายนอกใด ๆ (การคำนวณค่าตายตัว)
ในรุ่นสุดท้ายคอนโทรลเลอร์บางตัวใช้สำหรับควบคุมเซ็นเซอร์ (Arduino) และจัดการเซ็นเซอร์อะนาล็อก (PSoC)
วัสดุ:
ขั้นตอนที่ 1: แพลตฟอร์มพื้นฐาน
ตัวอย่างของวิธีการสร้างแพลตฟอร์มหุ่นยนต์ที่เรียบง่ายพร้อมส่วนประกอบและชิ้นส่วนที่หาได้ง่ายทุกที่โดยไม่ต้องใช้เครื่องมือหรืออุปกรณ์มืออาชีพและไม่มีทักษะพิเศษในการทำงานเชิงกล
ขนาดของฐานช่วยให้สามารถใช้ในการแข่งขันประเภทต่างๆของหุ่นยนต์ได้: Explorer, Line Follower, Can Collector, ฯลฯ
ขั้นตอนที่ 2: สิ่งที่เราต้องการได้รับ แล้วยังไง?
หุ่นยนต์นี้เป็นหุ่นยนต์ส่วนใหญ่ที่สร้างขึ้นโดย hobbysts โดยใช้ระบบบังคับเลี้ยวที่แตกต่างกันทำให้เราสามารถทราบพิกัดตำแหน่งของหุ่นยนต์ได้ทุกช่วงเวลาเพียงแค่รู้พื้นที่ที่ครอบคลุมของแต่ละล้อด้วยความแม่นยำที่เพียงพอ
ระบบการนำทางการคำนวณที่ไม่ทำงานนี้ได้รับผลกระทบจากข้อผิดพลาดสะสม ความแม่นยำในการวัดจะต้องสูงเพื่อให้แน่ใจว่าวงกลมข้อผิดพลาดเล็ก ๆ หลังจากเส้นทางยาว ดังนั้นหลังจากผลลัพธ์ที่ดีกับโปรแกรมเปลี่ยนรูปแบบโฮมเมดฉันตัดสินใจใช้สิ่งที่ดีกว่า: มอเตอร์ที่มีเกียร์ขนาด 12V-200 รอบต่อนาทีเชื่อมต่อกับตัวเข้ารหัส 300 Count Per Revolution (cpr) สองสามตัวซึ่งมีอยู่ในร้านขายหุ่นยนต์อินเทอร์เน็ตหลายแห่ง
หลักการพื้นฐาน
ในการตรวจจับพัลส์ทั้งหมดที่สร้างโดยตัวเข้ารหัส 300 cpr บนมอเตอร์ 3000 รอบต่อนาทีในวิธีการถอดรหัสแบบ 4x (120 kHz) เราจำเป็นต้องมีฮาร์ดแวร์เฉพาะสำหรับตัวเข้ารหัสแต่ละตัว (QEI = Quadrature Encoder Interface) หลังจากการทดลองกับ PIC18F2431 สองครั้งฉันได้พิจารณาแล้วว่าการอัพเกรดที่ถูกต้องนั้นเป็น dsPIC ที่จุดเริ่มต้นพวกเขาเป็นสองตัวควบคุมมอเตอร์ dsPIC30F4012 เพื่อควบคุมตำแหน่งล้อและความเร็วในการดำเนินการ odometry และเพื่อให้ข้อมูลของมอเตอร์ทั้งสองไปยัง dsPIC30F3013 DSC สำหรับวัตถุประสงค์ทั่วไปนี้มีประสิทธิภาพมากพอที่จะรับข้อมูลทำตรีโกณมิติบางอย่างเพื่อคำนวณพิกัดตำแหน่งและเก็บข้อมูลที่เกี่ยวข้องกับเส้นทางที่ครอบคลุมเพื่อรับแผนที่ของเขตข้อมูลทั้งหมดในอัตราที่สูงมาก
เมื่อบอร์ดและโปรแกรมใกล้จะเสร็จสมบูรณ์ Microchip นำเสนอ SPDIP 28 พินที่ทรงพลังในซีรีย์ dsPIC33F สำหรับทั้งตัวควบคุมมอเตอร์ (MC) และเวอร์ชันเอนกประสงค์ (GP) พวกเขาเร็วกว่า dsPIC30F มากมีหน่วยความจำโปรแกรมและ RAM ที่พร้อมใช้งานจำนวนมาก (มีประโยชน์สำหรับการทำแผนที่ภาคสนาม) พวกเขาต้องการพลังงานน้อยกว่า (ดีสำหรับหุ่นยนต์ที่ทำงานด้วยแบตเตอรี่) และความสามารถ DMA ทำให้การดำเนินงาน I / O ง่ายขึ้น
ที่สำคัญที่สุดคือตัวควบคุมมอเตอร์ Microchip ตัวแรกที่มี QEIs สองตัวบนชิปตัวเดียวกัน มาเริ่มพอร์ตใหม่กันเถอะ! ตรรกะบล็อกไดอะแกรมคล้ายกับบอร์ดสำหรับบอร์ดก่อนหน้า แต่ฮาร์ดแวร์และซอฟต์แวร์นั้นง่ายกว่ามาก ฉันสามารถใช้หนึ่ง DSC เพียงหนึ่งในสามเท่านั้น . ไม่จำเป็นต้องมีการสื่อสารความเร็วสูงระหว่างหัวหน้างานและผู้ควบคุมมอเตอร์เพื่อแลกเปลี่ยนพารามิเตอร์การนำทาง ทุกขั้นตอนง่าย ๆ ในการซิงโครไนซ์เพราะมันอยู่บนชิปตัวเดียวกัน ความสามารถในการเลือกพินอุปกรณ์ต่อพ่วงของซีรีย์ dsPIC33F ช่วยให้ PCB ง่ายขึ้นช่วยให้สามารถเชื่อมต่ออุปกรณ์ต่อพ่วงภายในและมีความยืดหยุ่นมากขึ้น
สิ่งนี้นำเราไปสู่“ บอร์ดควบคุมการนำทางด้วย dsPIC” หรือ dsNavCon สั้น ๆ. บอร์ดนี้ออกแบบมาเป็นส่วนหนึ่งของระบบที่ซับซ้อนยิ่งขึ้น ในหุ่นยนต์สำรวจที่สมบูรณ์บอร์ดอื่น ๆ จะควบคุมเสียงแสงเซ็นเซอร์ก๊าซเช่นเดียวกับกันชนและตัวค้นหาช่วงอัลตราโซนิกเพื่อค้นหาเป้าหมายและหลีกเลี่ยงอุปสรรค
ในฐานะคณะกรรมการอิสระ dsNavCon นอกจากนี้ยังสามารถใช้สำหรับหุ่นยนต์“ ผู้ติดตามเส้น” อย่างง่าย ๆ บางสิ่งที่ซับซ้อนกว่าเช่นหุ่นยนต์สำหรับการแข่งขัน Odometry และการคำนวณการตายหรือเรียกว่า“ can can robot” (สำหรับการรวบรวมการแข่งขัน) ยังมีหน่วยความจำของโปรแกรมมากมายให้เพิ่มรหัสสำหรับงานดังกล่าว หากมีการเปลี่ยนแปลงเล็กน้อยหรือไม่มีซอฟต์แวร์ก็สามารถใช้เป็นแบบสแตนด์อโลนสำหรับยานพาหนะที่มีการควบคุมระยะไกลได้โดยใช้โมเด็ม RF แบบสองทิศทางที่มีการควบคุมระยะไกลแบบอัจฉริยะบางประเภท รีโมตคอนโทรลนี้สามารถส่งคำสั่งที่ซับซ้อนเช่น“ เลื่อน FWD 1 ม.”,“ เลี้ยวซ้าย 15 °,”“ รัน FWD ที่ 50 ซม. / วินาที”“ ไปที่พิกัด X, Y พิกัด” หรือบางอย่างที่คล้ายกัน
บอร์ดและหุ่นยนต์ก็ออกแบบมาเพื่อให้ทุกคนในบ้านทำโดยไม่มีเครื่องมือและอุปกรณ์มืออาชีพ
ขั้นตอนที่ 3: ฮาร์ดแวร์โอเพ่นซอร์ส
บล็อกไดอะแกรม
ระบบย่อยการควบคุมการนำทางประกอบด้วย dsNav ในฐานะที่เป็นบอร์ด“ ฉลาด” ของระบบและบอร์ด H-bridge คู่ H29 ที่ใช้ L298 เพื่อควบคุมมอเตอร์ 12V ที่มีเกียร์ (Hsiang Neng HN-GH12-1634TR) ข้อเสนอแนะการเคลื่อนไหวมาจากสองสามเข้ารหัส 300 cpr (US ดิจิตอล e4p-300-079-ht)
การสื่อสารกับโลกภายนอกดำเนินการผ่านสองอินเตอร์เฟสแบบอนุกรมของ UART หนึ่งสำหรับ telemetry และอื่น ๆ เพื่อรับข้อมูลจากเซ็นเซอร์บอร์ด โมดูล XBee สามารถเชื่อมต่อกับ UART1 หรือ UART2 ผ่านจัมเปอร์ JP1 และ JP2 ซ็อกเก็ต J1 และ J16 พร้อมใช้งานสำหรับการเชื่อมต่อชนิดอื่น พอร์ต COMM1 (J16) สามารถใช้สำหรับการสื่อสาร I2C ด้วยความสามารถในการเลือกพินต่อพ่วงของซีรีย์ dsPIC33F
แผนภาพแผนผังดั้งเดิมในรูปแบบอีเกิ้ลสามารถพบได้ที่นี่:
http://www.guiott.com/Rino/dsNavCon33/dsNavCon33_Eagle_project/DsPid33sch.zip
อย่างที่คุณเห็นว่าแผนผังนั้นเรียบง่ายมากจนสามารถนำไปใช้งานบนกระดานอย่างที่ฉันทำ หากคุณไม่ต้องการใช้ระบบนี้และไม่ต้องการให้ตระหนักถึง PCB ของคุณเองคณะกรรมการการค้าที่ยึดตามงานต้นฉบับของฉันและสามารถใช้งานร่วมกับซอฟต์แวร์โอเพ่นซอร์สของฉันได้ที่: http: //www.robot-italy .com / product_info.php? products_id = 1564
ขั้นตอนที่ 4: ซอฟต์แวร์โอเพ่นซอร์ส
ซอฟต์แวร์ได้รับการพัฒนาด้วยMPLAB®ฟรี IDE และเขียนด้วยคอมไพเลอร์MPLAB® C30 (แม้แต่ในรุ่นฟรีหรือนักเรียน) ทั้งสอง (แน่นอน) โดย Microchip:
http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=81
โครงการทั้งหมดมีให้ในรูปแบบโอเพ่นซอร์สที่ Google Code
http://code.google.com/p/dspid33/
โปรดอ้างอิงถึงรุ่นล่าสุดความคิดเห็นคำอธิบาย ฯลฯ
โปรแกรมอธิบายทีละขั้นตอนภายในรหัส เพื่อให้มีความคิดเห็นในระดับสูงและรหัสที่อ่านได้ง่ายขึ้นในทุกจุดที่สำคัญจะมีตัวเลขในวงเล็บ (เช่น: 7) เป็นการอ้างอิงไปยังไฟล์ภายนอก (เช่น: descrEng.txt) ในโครงการ MPLAB .
แผนภาพแสดงสถาปัตยกรรมโดยรวมของขั้นตอนการควบคุมของบอร์ด dsNav และกลยุทธ์การนำทางที่ใช้ในโครงการ
ตัวควบคุมมอเตอร์สามารถมองเห็นเป็นกล่องดำที่ดูแลความเร็วของล้อ ส่วนหัวหน้างานของโปรแกรมจะส่งความเร็วอ้างอิงให้ (VeldDesX: ความเร็วที่ต้องการ) โมดูล Input Capture ของไมโครคอนโทรลเลอร์รับพัลส์จากตัวเข้ารหัสที่เชื่อมต่อกับแกนมอเตอร์และรับความเร็วการหมุนของมอเตอร์ (VelMesX: ความเร็วที่วัดได้) เมื่อรวมค่าทุก ๆ 1 มิลลิวินาทีในการควบคุม PID "Speed PID" เราได้รับค่า PWM ที่เหมาะสมเพื่อรักษาความเร็วที่ต้องการของแต่ละล้อ
โมดูล QEI (Quadrature Encoder Interface) รับทั้งพัลส์ A และ B จากเอนโค้ดเดอร์และส่งกลับไปยังหัวหน้างานฟังก์ชั่นทิศทางการเดินทางและจำนวนพัลส์ในโหมด 4x (นับขอบสัญญาณที่เพิ่มขึ้นและลดลงของสัญญาณ A และสัญญาณ B: 2 x 2 = 4)
การคูณจำนวนพัลส์ด้วย K ที่บ่งบอกถึงพื้นที่ที่เดินทางไปยังชีพจรตัวเข้ารหัสแต่ละอันเราได้ระยะทางที่เดินทางด้วยล้อขวาและซ้ายทุก ๆ 10ms หัวหน้างานจะรวมข้อมูลการเดินทางนี้และใช้กระบวนการคำนวณการตายเพื่อให้ได้ตำแหน่งพิกัดที่วัดได้ของบอท: Xmes, Ymes, θMes (มุมการวางแนว)
หัวหน้างานได้รับคำสั่งการนำทางจากภายนอกโดยอินเตอร์เฟสอนุกรม (telemetry)
กลยุทธ์ที่แตกต่างสามารถนำไปใช้:
- เดินทางด้วยความเร็วที่กำหนดในทิศทางที่กำหนด (VelDes, θDes)
B - เดินทางไปยังจุดที่กำหนดพร้อมพิกัด XDes, YDes
C - เดินทางไปตามระยะทางที่กำหนดในทิศทางที่กำหนด (DistDes, θDes)
โหมด A : ด้วย "สวิตช์ควบคุมแบบลอจิคัล" ในตำแหน่งที่ 1 จะใช้เฉพาะฟังก์ชัน PID "Angle PID" ที่ควบคุมโดยฟังก์ชันหัวหน้างาน อันนี้รวมมุมที่ต้องการ withDes กับมุมที่วัดได้θMesที่คำนวณโดยขั้นตอนการทาง odometry เพื่อให้ได้ค่าของความเร็วเชิงมุมในการหมุน vehicle ของยานพาหนะรอบแกนแนวตั้งเพื่อแก้ไขข้อผิดพลาดในการปฐมนิเทศ
ค่าของ DeltaV เป็นสัดส่วนกับω มันถูกเพิ่มเข้าไปใน VelDes เพื่อให้ได้ความเร็วของล้อซ้ายและลบออกจาก VelDes เพื่อให้ได้ความเร็วของล้อขวาเพื่อให้ส่วนหัวสอดคล้องกับค่าθDesในขณะที่ศูนย์กลางของหุ่นยนต์ยังคงเคลื่อนที่ด้วยความเร็ว VelDes
โหมด B : ด้วย "สวิตช์ควบคุมแบบลอจิคัล" ในตำแหน่งที่ 2 ความเร็วที่ต้องการ VelDes จะถูกคำนวณโดยตัวควบคุม PID "Dist PID" และใช้ในโหมด A สัญญาณเข้าที่วัดได้สำหรับ PID นี้ (DistMes) จะถูกคำนวณเป็นฟังก์ชันของ พิกัดปัจจุบันและพิกัดปลายทาง มุมการวางแนวที่ต้องการθDesนั้นมาจากขั้นตอนเดียวกันและใช้เป็นอินพุตอ้างอิงสำหรับ "Angle PID" อินพุตอ้างอิงสำหรับ "Dist PID" คือ 0 หมายความว่าถึงปลายทางแล้ว เมื่อใช้ωและ VelDes การควบคุมความเร็วของล้อจะทำงานเหมือนในโหมด A
โหมด C : ด้วย "สวิตช์ควบคุมแบบลอจิคัล" ในตำแหน่งที่ 2 cordinates ปลายทาง Xdes, Ydes จะถูกคำนวณหนึ่งครั้งที่จุดเริ่มต้นเป็นฟังก์ชันของพารามิเตอร์อินพุต DistDes, θDes หลังจากนั้นทุกอย่างจะเข้าสู่โหมด B
ขั้นตอนที่ 5: รายละเอียดของซอฟต์แวร์: การควบคุมความเร็วและฟังก์ชั่นพื้นฐานอื่น ๆ
โปรแกรมเต็มแล้ว ขับเคลื่อนขัดจังหวะ . เมื่อเริ่มต้นหลังจากเริ่มต้นโปรแกรมจะเข้าสู่ main-loop ง่าย ๆ ซึ่งทำหน้าที่เป็นเครื่องสถานะ ใน main-loop โปรแกรมจะตรวจสอบสถานะที่เปิดใช้งานโดยเหตุการณ์ภายนอกและเข้าสู่สถานะสัมพันธ์ตามค่าของมัน
เนื่องจากมันเป็นความร่วมมือที่ง่ายมาก "ระบบปฏิบัติการแบบเรียลไทม์ , "แต่ละงานประจำจะต้องถูกดำเนินการในเวลาที่สั้นที่สุดที่เป็นไปได้ทำให้ระบบหยุดชะงักเพื่อดูแลการขัดจังหวะบ่อยครั้งมาก
ไม่มี "รอจนกระทั่ง" และไม่มีความล่าช้าในรหัส เมื่อใดก็ตามที่มีการใช้อินเทอร์รัปต์โดยเฉพาะอย่างยิ่งสำหรับการทำงานที่ช้าเช่นการส่งหรือการรับสายอักขระ การสื่อสาร UART ใช้ประโยชน์จากความสามารถ DMA ของ dsPIC33F เพื่อประหยัดเวลาของ CPU ในการทำงาน "สกปรก" ทั้งหมดในฮาร์ดแวร์
อุปกรณ์ต่อพ่วงที่ใช้กับ dsPIC33FJ128MC802:
- QEIs เพื่อคำนวณเส้นทางการเดินทาง
- Input Capture (IC) เพื่อคำนวณความเร็ว
- ตัวแปลง A / D เพื่ออ่านค่ากระแสของมอเตอร์
- ปรับปรุง PWMs เพื่อขับเคลื่อนมอเตอร์
- UART เพื่อสื่อสารกับโลกภายนอก
โมดูล QEI ถูกใช้เพื่อรู้ว่าล้อเดินทางไปในทิศทางใด ค่านี้ถูกรวมในพีชคณิตแบบพีชคณิตทุก 1 มิลลิวินาทีและส่งไปยังฟังก์ชั่นหัวหน้างานตามคำขอ หลังจากส่งค่าแล้วตัวแปรจะถูกรีเซ็ต
วัดความเร็วที่ชีพจรของเครื่องเข้ารหัสทุกตัวดังที่อธิบายไว้ด้านล่าง ทุก 1 มิลลิวินาทีมันจะคำนวณความเร็วเฉลี่ยโดยการหาค่าเฉลี่ยตัวอย่างดำเนินการอัลกอริธึม PID และแก้ไขความเร็วมอเตอร์ให้สอดคล้องกับผลลัพธ์ของมันเปลี่ยนวัฏจักรหน้าที่ของ PWM สำหรับคำอธิบายโดยละเอียดของแอปพลิเคชันไลบรารี C30 PID ดูตัวอย่างรหัส Microchip: CE019 - การใช้ตัวควบคุม Proportional Integral Derivative (PID) ในระบบควบคุมวงปิด http://ww1.microchip.com/downloads/en/DeviceDoc/CE019_PID.zip
ความเร็วที่แตกต่างกันของมอเตอร์จะถูกดำเนินการอย่างราบรื่นเร่งหรือชะลอตัวลงด้วยทางลาดเอียงขึ้นหรือลงเพื่อหลีกเลี่ยงความเครียดทางกลหนักและการลื่นไถลของล้อซึ่งอาจทำให้เกิดข้อผิดพลาดใน Odometry การลดความเร็วจะเร็วขึ้นจากนั้นเร่งความเร็วเพื่อหลีกเลี่ยงการชนกับสิ่งกีดขวางระหว่างการเบรก
เข้าใจแล้ว โมดูลจับการป้อนข้อมูลถูกใช้เพื่อวัดเวลาที่ผ่านไประหว่างพัลส์สองอันที่สร้างโดยเครื่องเข้ารหัสซึ่งหมายถึงเมื่อล้อเดินทางในพื้นที่คงที่ซึ่งเป็นที่รู้จักกันดี SPACE_ENC ) เชื่อมต่อแบบขนานกับ QEA (ภายใน DSC ด้วยความสามารถของ Peripheral Pin Select ของ dsPIC33F) พวกเขาจับเวลาที่ผ่านไปของสัญญาณเข้ารหัสที่เพิ่มขึ้น TIMER2 ใช้ในโหมดฟรีรัน ในแต่ละการขัดจังหวะ IC ค่าปัจจุบันของ TMR2 จะถูกเก็บไว้และค่าก่อนหน้านี้จะถูกหักออกจากค่านั้น นี่คือช่วงเวลาการเต้นของชีพจร จากนั้นค่าปัจจุบันจะกลายเป็นค่าก่อนหน้าเพื่อรอการขัดจังหวะการถัดไป ต้องตรวจสอบการตั้งค่าสถานะของ TMR2 เพื่อดูว่ามีการล้นเกิดขึ้นในการลงทะเบียน 16 บิตหรือไม่ ถ้าใช่จะต้องเพิ่มความแตกต่างระหว่าง 0xFFFF และตัวอย่างก่อนหน้านี้ในค่าปัจจุบัน ตัวอย่างถูกเพิ่มเข้ามาในพีชคณิต IcPeriod ตัวแปรตาม _UPDN บิตเพื่อกำหนดทิศทางของความเร็ว นี่เป็นหนึ่งในวิธีที่แนะนำใน แอปพลิเคชัน Microchip note AN545 .
ตัวแปร IcIndx มีจำนวนตัวอย่างที่เพิ่มเข้ามา IcPeriod .
ทุก 1ms จะคำนวณความเร็วเฉลี่ยเป็น V = อวกาศที่ / เวลา
ที่ไหน พื้นที่ = SPACE_ENC • IcIndx
(= พื้นที่ครอบคลุมในหนึ่งชีพจรเข้ารหัส•จำนวนพัลส์)
และ เวลา = TCY • IcPeriod
(= ช่วงเวลา TMR เดียว•รวมช่วงเวลาที่เกิดขึ้น)
Single_TMR_period = TCY = 1 / FCY (ความถี่สัญญาณนาฬิกา)
ดังนั้น V = Kvel • (IcIndx / IcPeriod)
ที่ไหน Kvel = SPACE_ENC • FCY มีความเร็วเป็น m / s
เลื่อนไปทางซ้าย 15 บิต Kvel const ( KvelLong = Kvel << 15 ) ความเร็วจะถูกคำนวณแล้วในรูปแบบเศษส่วน (รวมถึงหากใช้เฉพาะตัวแปรจำนวนเต็ม) พร้อมที่จะใช้ในรูทีน PID ดูไฟล์ "descrEng.txt" ในโครงการ MPLAB สำหรับคำอธิบายโดยละเอียดเพิ่มเติม
ตัวแปลง A / D ทำการวัดกระแสมอเตอร์อย่างต่อเนื่องโดยเก็บค่าไว้ในบัฟเฟอร์ ADCBUF 16 ตำแหน่ง เมื่อบัฟเฟอร์เต็มจะเกิดการขัดจังหวะและค่าเฉลี่ยจะถูกคำนวณโดยประมาณทุกๆ 1 มิลลิวินาที
UARTs จะใช้ในการรับคำสั่งจากภายนอกและเพื่อส่งกลับผลลัพธ์ของการวัด ส่วนการสื่อสารของโปรแกรมทำงานเป็นเครื่องสถานะ ตัวแปรสถานะถูกใช้เพื่อดำเนินการตามลำดับ Interrupt Service Routines (ISR) ที่ง่ายและรวดเร็วมากรับหรือใส่ทุก ๆ ไบต์จากหรือไปยังบัฟเฟอร์และตั้งค่าสถานะที่ถูกต้องเพื่อให้สามารถใช้งานฟังก์ชันที่เหมาะสมได้
หากมีข้อผิดพลาดใด ๆ เกิดขึ้นระหว่างการรับ (UART, checksum, การแยกวิเคราะห์ข้อผิดพลาด) ตัวแปรสถานะจะถูกตั้งค่าเป็นจำนวนลบและไฟ LED สีแดงจะเปิดขึ้นเพื่อสื่อสารกับสภาพความผิดปกตินี้จากภายนอก ดูไฟล์“ descrEng.txt” ในโครงการ MPLAB เพื่อดูรายการข้อผิดพลาดทั้งหมด
โปรโตคอลที่ใช้สำหรับการจับมือกันคือ ชั้นกายภาพอิสระ และสามารถใช้กับบัส I2C หรือ RS485 ได้เช่นกันในการสื่อสาร
ชั้นแรก ถูกควบคุมโดยส่วนต่อพ่วง dsPIC ตรวจพบข้อผิดพลาดของเฟรมหรือโอเวอร์รัน (UART) หรือการชน (I2C) โดยการตั้งค่าสถานะที่เหมาะสม
ชั้นที่สอง ถูกจัดการโดยรูทีน ISR พวกเขาเติมบัฟเฟอร์ RX ด้วยไบต์ที่ได้รับจากอินเทอร์เฟซ พวกเขายังตรวจพบบัฟเฟอร์ล้นและคำสั่งเกิน
ฟังก์ชัน UartRx หรือ UartRx2 จัดการ ชั้นที่สาม . ตามที่อธิบายไว้แล้ว (ดูแผนภูมิการไหล) รูทีนเหล่านี้ทำหน้าที่เป็นเครื่องสถานะรับไบต์จากบัฟเฟอร์และถอดรหัสสตริงคำสั่ง
ไบต์มีการแลกเปลี่ยนระหว่างเลเยอร์ที่สองและสาม (ฟังก์ชัน ISR และ UartRx) ผ่านบัฟเฟอร์แบบวงกลม ISR ได้รับไบต์เก็บไว้ในอาร์เรย์และเพิ่มตัวชี้ไปยังอาร์เรย์ถ้าตัวชี้ไปถึงจุดสิ้นสุดของอาร์เรย์จะเริ่มต้นใหม่จนถึงจุดเริ่มต้น ฟังก์ชั่น UartRx มีตัวชี้ของตัวเองเพื่ออ่านอาร์เรย์เดียวกันเพิ่มขึ้น (ในลักษณะเป็นวงกลมด้วย) ทันทีที่ไบต์ถูกถอดรหัสในสถานะ RX ปัจจุบัน การวนซ้ำหลักเรียกฟังก์ชัน UartRx เมื่อใดก็ตามที่ตัวชี้ "ใน" แตกต่างจากตัวชี้ "ออก"
แต่ละแพ็คเก็ตคำสั่งประกอบด้วย:
0 - ส่วนหัว @
1 - ID 0-9 ASCII
2 - Cmd A-Z ASCII
3 - CmdLen N = 1-MAX_RX_BUFF # ของไบต์ต่อไปนี้ (รวมการตรวจสอบ)
4 - ข้อมูล …
…
N-1 - ข้อมูล
N - Checksum 0-255 ที่ได้รับเพียงเพิ่มขึ้นในตัวแปร 8 บิตทุกไบต์เขียนข้อความ (ยกเว้นการตรวจสอบตัวเอง)
เลเยอร์นี้ควบคุมข้อผิดพลาดการหมดเวลาและการตรวจสอบรวมถึงความสอดคล้องของแพ็กเก็ต (ส่วนหัวที่ถูกต้องความยาวที่ถูกต้อง) หากทุกอย่างโอเคจะเปิดใช้งานรูทีน Parser (ชั้นที่สี่ ) เพื่อถอดรหัสข้อความและเพื่อดำเนินการตามที่ต้องการ รูทีนนี้จะตั้งค่าสถานะข้อผิดพลาดที่เหมาะสมหากไม่ทราบรหัสข้อความที่ได้รับ
TMR1 สร้างนาฬิกาจับเวลา 1,000 Hz - การเต้นของโปรแกรม ในการขัดจังหวะของ TMR1 แต่ละครั้งตัวจับเวลาภายในจะได้รับการอัปเดต watchdog จะถูกล้างและตั้งค่าสถานะเพื่อเปิดใช้งานฟังก์ชั่นที่ขอค่าพื้นที่เดินทาง เปิดใช้งานฟังก์ชั่น“ All_Parameters_Ask” ทุก 10ms (ความเร็วตำแหน่งปัจจุบัน)
ขั้นตอนที่ 6: รายละเอียดของซอฟต์แวร์: Odometry และ Field Mapping = ฉันอยู่ที่ไหน?
เมื่อเรามีข้อมูลเกี่ยวกับระยะทางที่ล้อแต่ละล้อเดินทางในการอัปเดตแบบไม่ต่อเนื่อง (odometry) เราสามารถประเมินพิกัดตำแหน่งของหุ่นยนต์ด้วยระยะเวลาเดียวกันโดยไม่มีการอ้างอิงจากภายนอก (การคำนวณตาย)
ภูมิหลังทางทฤษฎีบางประการเกี่ยวกับการคำนวณการตายด้วย odometry สามารถพบได้ในหนังสือของ Johann Borenstein:
"ฉันอยู่ที่ไหน - เซ็นเซอร์และวิธีการสำหรับการจัดตำแหน่งหุ่นยนต์เคลื่อนที่"
และบนหน้าเว็บต่อไปนี้:
http://www.seattlerobotics.org/encoder/200010/dead_reckoning_article.html
พื้นหลังทางคณิตศาสตร์และคำอธิบายอย่างลึกซึ้งของวิธีการทั่วไปที่ใช้สามารถพบได้ใน G.W กระดาษของ Lucas A แบบฝึกหัดและรูปแบบการสอนเบื้องต้นสำหรับระบบบังคับเลี้ยวที่แตกต่างกันของ Actuators Wheel Robot มีให้บริการบนอินเทอร์เน็ต:
http://rossum.sourceforge.net/papers/DiffSteer/DiffSteer.html
อัลกอริทึมแบบง่าย ๆ บางอย่างสามารถพบได้ในเอกสารเดียวกันด้วยเช่นกันดังนั้นจึงเป็นไปได้ที่จะได้รับการประนีประนอมระหว่างความแม่นยำและความเร็วในการทำอย่างละเอียดโดยใช้ความสามารถทางคณิตศาสตร์ (ตรีโกณมิติ) ของซีรีย์ dsPIC33F
คำอธิบายของคณิตศาสตร์ที่ใช้ในการคำนวณตำแหน่งสามารถพบได้ในรูปภาพที่แนบมากับขั้นตอนนี้ คนแรกแสดงความหมายของสัญลักษณ์คนที่สองแสดงสูตรที่ใช้กับสัญลักษณ์เหล่านั้น คลิกที่กล่องถัดจากแต่ละขั้นตอนการคำนวณคำอธิบายสั้น ๆ จะปรากฏขึ้น
ในตอนท้ายเรารู้ว่าหุ่นยนต์เคลื่อนที่ในช่วงเวลานั้นในฐานะเดลต้าของการวางแนวเดลต้าบนแกน X และเดลต้าบนแกน Y ในฟิลด์อ้างอิงคาร์เพเทียน
การรวบรวมแต่ละค่าเดลต้าในตัวแปรของตัวเองเรารู้พิกัดปัจจุบัน (ตำแหน่งและการวางแนว) ของแพลตฟอร์ม
เพื่อหลีกเลี่ยงข้อผิดพลาดในการคำนวณ (หารด้วยศูนย์) และการเสียเวลาของตัวควบคุมจะต้องทำการตรวจสอบล่วงหน้าทั้งตัวแปร Sr และ Sl การกำหนดค่ากึ่งศูนย์ที่ดูแลการคำนวณเชิงกลและการคำนวณขั้นต่ำเราสามารถทำให้สูตรง่ายขึ้นหากหุ่นยนต์เคลื่อนที่เป็นเส้นตรง (พื้นที่ที่ครอบคลุมด้วยล้อขวาเกือบเท่ากับพื้นที่ที่เดินทางด้วยล้อซ้าย) หรือถ้ามันหมุนรอบแกนแนวตั้ง (พื้นที่ที่ครอบคลุมโดยล้อขวาเกือบเท่ากับพื้นที่ที่เดินทางโดยล้อซ้าย แต่ไปในทิศทางตรงกันข้าม) ดังรายละเอียดในแผนภูมิการไหลที่แสดงในภาพสุดท้าย
วิดีโอนี้แสดงตัวอย่างของความแม่นยำที่เราสามารถรับได้:
http://www.youtube.com/watch?v=d7KZDPJW5n8
การทำแผนที่ภาคสนาม
ด้วยข้อมูลที่คำนวณโดยฟังก์ชั่นก่อนหน้านี้จะทำการแมปฟิลด์
Xms ทุกตัวหลังจากการทำรายละเอียดตำแหน่งปัจจุบันการแมปฟิลด์จะดำเนินการโดยแบ่งเขตข้อมูลที่ไม่รู้จักในตารางเซลล์ขนาด 10 x 10 ซม. การกำหนดมิติของเขตข้อมูลสูงสุด 5 x 5m เราจะได้รับเมทริกซ์เซลล์ 50 x 50 = 2500 แต่ละเซลล์จะถูกกำหนดด้วย nibble โดยมีหน่วยความจำรวมที่ 1250 Bytes สามารถกำหนดค่าต่างกันสิบหกค่าให้กับแต่ละเซลล์:
n = 00 เซลล์ที่ไม่รู้จัก
n = 01 - 10 เซลล์ที่เยี่ยมชม n ครั้ง
n = พบสิ่งกีดขวาง 11 รายการ
n = พบเป้าหมายก๊าซ 12
n = พบ 13 เป้าหมายแสง
n = 14 พบเป้าหมายเสียง
หุ่นยนต์สามารถเริ่มจากตำแหน่งใด ๆ ในสนาม สิ่งเหล่านี้จะเป็นพิกัดอ้างอิง (0,0) ในระบบอ้างอิง
การทำแผนที่ภาคสนามมีประโยชน์ในการค้นหากลยุทธ์การสำรวจที่ดีที่สุดในสาขาที่ไม่รู้จัก หุ่นยนต์สามารถนำตัวเองไปยังส่วนที่สำรวจน้อยกว่าของฟิลด์ (ค่า“ n” ต่ำกว่า) สามารถประหยัดเวลาโดยไม่หยุดสองครั้งในเป้าหมายที่ค้นพบแล้วสามารถค้นหาเส้นทางที่ดีที่สุดในการเข้าถึงพิกัดที่กำหนดและอื่น ๆ
ขั้นตอนที่ 7: คอนโซลระยะไกล
เพื่อให้ง่ายต่อการพัฒนาและดีต่อการทำงานในระบบปฏิบัติการใด ๆ มันเขียนด้วย การประมวลผล ภาษา:
http://www.processing.org/
ซอร์สโค้ดสำหรับโปรแกรมนี้ก็มีให้เหมือนโอเพ่นซอร์สที่ Google Code:
http://code.google.com/p/dsnavconconsole/
กับ แผงหลัก (ภาพแรก) เราสามารถดำเนินการตรวจสอบระยะไกลโดยดูที่ตารางเส้นทางตามด้วยหุ่นยนต์ (ประเมินโดย odometry) ในฟิลด์ขนาดที่กำหนดค่าได้และค่าสำคัญอื่น ๆ ที่อ่านบน dsNav .
มาตรวัดแสดงค่าที่วัดได้:
- MesSpeed อยู่ในช่วง +/- 500 มม. / วินาทีซึ่งเป็นค่าเฉลี่ยของความเร็วของสองล้อ (ความเร็วของศูนย์กลางของแพลตฟอร์ม)
- กระแสที่วัดได้ใน mA (ผลรวมของกระแสจากมอเตอร์สองตัว)
- มุมที่วัดได้ซึ่งถูกขยายโดย odometry
พาเนลอื่นถูกใช้เพื่อกำหนดค่าพารามิเตอร์ของหุ่นยนต์และเพื่อจัดเก็บในเส้นทางที่กำหนดให้ทำตาม (ถ้าจำเป็น) ในหุ่นยนต์ แผงที่สำคัญอย่างน้อยในระหว่างการพัฒนาหุ่นยนต์ก็คือ แผงรายละเอียด (ภาพที่สอง) ที่แสดงความเร็วของแต่ละล้อแบบเรียลไทม์ซึ่งเป็นประโยชน์อย่างมากสำหรับการสอบเทียบพารามิเตอร์ทั้งหมด
มุมมองกริดกลางสามารถสลับกับมุมมองของเว็บแคมเพื่อควบคุมแม้ตามมุมมองเส้นทางที่หุ่นยนต์กำลังติดตาม
ตัวอย่างการใช้งานจริงสำหรับคอนโซลนี้แสดงในวิดีโอนี้:
http://www.youtube.com/watch?v=OPiaMkCJ-r0
