ปรับปรุงการอ่าน/คำนวนความเร็วของอีมอเตอร์

บทความนี้ต่อเนื่องจาก eMotor Phones Home ก่อนหน้านี้ ที่นำเสนอการควบคุมความเร็วรอบของ eMotor โดยตัวควบคุมพีไอดี วิธีการคำนวนความเร็วที่ใช้ในบทความนั้นคือ ตั้งโมดูล QEI ของ dsPIC ให้รีเซ็ตค่านับในรีจิสเตอร์ POSCNT ในทุกรอบโดยสัญญาณอินเด็กซ์ ข้อเสียของวิธีการนี้คือ

1. คาบเวลาในการอ่านค่านับจะต้องตั้งให้เหมาะสมโดยสัมพันธ์กับความเร็วรอบ จะต้องไม่เกิด overflow/underflow ทำให้เป็นเงื่อนไขบังคับด้านเวลากับคาบของไทเมอร์ที่ใข้อ่านและคำนวน การทำงานลักษณะนี้ถ้ามีการตั้งค่าสำหรับความเร็วรอบหนึ่งจะใช้งานได้ตามต้องการ แต่เมื่อปรับค่าความเร็วรอบในย่านที่กว้างจะเริ่มเกิดปัญหาขึ้น
2. ไม่สามารถใช้กับงานที่ต้องการรวบรวมค่านับมากกว่าหนึ่งรอบ เช่นการควบคุมแกนเชิงเส้นของเครื่องซีเอ็นซี หรือข้อต่อหุ่นยนต์ที่มีการทดรอบโดยเกียร์ เป็นต้น

ดังนั้นบทความนี้จะเสนอวิธีการคำนวนความเร็วรอบสำหรับกรณีสะสมค่านับจากเอนโคเดอร์โดยไม่มีการรีเซ็ต ส่วนของการคำนวนความเร็วจะง่ายกว่า จุดสำคัญที่ต้องการอธิบายในบทความนี้คือการเก็บค่าตำแหน่งจากมอเตอร์อย่างมีประสิทธิภาพและเชื่อถือได้

สร้างตัวนับตำแหน่งขนาด 32 บิต

ตัวนับที่เป็นฮาร์ดแวร์ในโมดูล QEI ของ dsPIC จะมีขนาด 16 บิต ดังนั้นจะเก็บค่านับได้ระหว่าง 32767 กับ -32768 ทำให้สามารถเกิด overflow/underflow ในเวลาไม่นานขึ้นกับจำนวนพัลซ์ของมอเตอร์ในหนึ่งรอบ (มักถูกคูณด้วย 4 เพื่อเพิ่มความละเอียด) และความเร็วรอบของมอเตอร์ ดังนั้นในการใช้งานจริงมักต้องการตัวนับตำแหน่งที่มีขนาดอย่างน้อย 32 บิต ในกรณีที่ไม่ใช้ฮาร์ดแวร์พิเศษเพิ่มเติม วิธีที่ทำได้คือขยายขนาดของตัวนับตำแหน่งโดยซอฟต์แวร์

วิธีการแบบง่ายๆ ที่ใช้ได้คือกำหนดตัวแปร global เป็นชนิด long (32 บิต) เมื่อมีการ overflow/underflow เกิดขึ้น ก็เพิ่มหรือลดค่าตัวแปรนี้ลงหนึ่งหน่วย ต้องการค่านับเมื่อใดก็เลื่อนบิตตัวแปรนี้ไปทางซ้าย 16 บิต และบวกกับค่าที่อ่านได้จาก POSCNT จะเห็นว่าวิธีการนี้มีประสิทธิภาพต่ำเพราะต้องใช้หลายคำสั่งในการอ่านแต่ละครั้ง วิธ๊การที่ดีกว่าคือประกาศโครงสร้างแบบ union ดังนี้

typedef union 
{
    int half[2];
    long full;
} enc_cnt;

สำหรับเก็บค่านับ การใช้ union เป็นวิธีสร้างตัวแปรหลายชนิดที่อยู่บนแอดเดรสเดียวกัน ดังนั้นเมื่อเราสร้างตัวแปรชนิด enc_cnt ที่ประกาศไว้

enc_cnt EncCnt;  

ค่า 32 บิต , 16 บิตล่าง, และ 16 บิตบน จะอ่านได้จาก EncCnt.full, EncCnt.half[0] และ EncCnt.half[1], ตามลำดับ ดังนั้นเมื่อใดที่ overflow/underflow เกิดขั้น เราก็จะอัพเดตค่าของ EncCnt.half[1] ขณะที่ค่านับใน POSCNT จะถูกก๊อปปี้ไปยัง EncCnt.half[0] จะเห็นได้ว่าวิธีการนี้ไม่จำเป็นต้องมีการเลื่อนและบวกข้อมูล เมื่อต้องการค่านับ 32 บิตก็สามารถอ่านได้จาก EncCnt.full โดยทันที อย่างไรก็ตามต้องคำนึงถึงปัญหา read-modify-write ด้วย โดยลำคับความสำคัญ (priority) ของเทรดไทเมอร์อินเตอร์รัพท์ที่เขียนค่าลงใน enc_cnt จะต้องสูงกว่าเทรดที่อ่าน

ส่วนที่เปลี่ยนหรือเพิ่มเติมในโมดูล QEI

ฟังก์ชันการตั้งค่าโมดูล QEI ส่วนใหญ่ยังคงเดิม เพียงแต่ในวิธีนี้เราไม่ต้องการรีเซ็ตค่านับโดยพัลซ์อินเด็กซ์ ดังนั้นคำสั่งที่เปลี่ยนไปคือ

QEICONbits.QEIM = 7; // X4 mode, without index pulse reset

คำถามต่อมาคือ ทำอย่างไรจะทราบว่ามี overflow/underflow เกิดขึ้น? โมดูล QEI จะเตรียมอินเทอร์รัพท์สำหรับเหตุการณ์นี้อยู่แล้ว เราเพียงแต่เปิดการทำงานเท่านั้น ซึ่งวิธีการตั้งให้อินเทอร์รัพท์ทำงานจะมีขั้นตอนเหมือนอินเทอร์รัพท์อื่นๆ ใน dsPIC (รวมถึง PIC ตระกูลอื่นๆ) คือ enable และรีเซ็ต flag ของอินเทอร์รัพท์ ดังนั้น 2 คำสั่งที่เพิ่มเติมในส่วนท้ายของฟังก์ชันตั้งค่า QEI คือ

    _QEIIE = 1;  // enable QEI interrupt
    _QEIIF = 0;  // reset the interrupt flag

งานสุดท้ายสำหรับส่วนของ QEI คือเขียนโค้ดในอินเทอร์รัพท์รูทีน เพื่ออัพเดตค่าในส่วน 16 บิตบนของค่านับทั้งหมด ทำได้ง่ายๆ โดยตรวจสอบบิต QEICONbits.UPDN เพื่อทราบทิศทางการหมุนของมอเตอร์และปรับค่าขึ้นหรือลงให้สอดคล้องกับทิศทาง ก่อนจะออกจากรูทีน อย่าลืมรีเซ็ตค่าแฟล็กเพื่อยอมให้เกิดอินเทอร์รัพท์ในครั้งต่อไปได้

void __attribute__((interrupt, auto_psv)) _QEIInterrupt(void)
{
    if (QEICONbits.UPDN) EncCnt.half[1]++;
    else EncCnt.half[1]--;
    _QEIIF = 0;
}

สำหรับการอัพเดทค่าของ EncCnt.half[0] โดยค่าใน POSCNT จะกระทำโดยเทรดไทเมอร์

การคำนวนความเร็ว

เมื่อใช้การเก็บค่านับ 32 บิตดังที่ได้กล่าวมา การคำนวนความเร็วจะง่ายมาก ในที่นี้เราใช้ไทเมอร์ 2 ที่ตั้งคาบเวลา 50 มิลลิวินาที (กำหนดโดยมาโคร DELTAT2) และจำนวนพัลซ์ของเอนโคเดอร์ที่คูณด้วย 4 แล้ว กำหนดไว้ใน ENCPPRx4 เนื่องจาก eMotor กำเนิดสัญญาณเอนโคเดอร์ 100 พัลซ์ต่อรอบ ดังนั้นENCPPRx4 = 100*4 - 1

#define DELTAT2 50  // milliseconds
#define ENCPPRx4 399 // 100*4 - 1
long EncCnt_old;   // global variable to keep previous position
void __attribute__((__interrupt__)) _T2Interrupt (void)
{

    IFS0bits.T2IF = 0; // Clear timer 2 interrupt flag

    dEnc = EncCnt.full - EncCnt_old;
    Velf = (double)dEnc*1000/(ENCPPRx4*DELTAT2);


    EncCnt_old = EncCnt.full;

}

ผลการทดลอง

รูปที่ 1 และ 2 ด้านล่างแสดงการเปรียบเทียบระหว่างลูปเปิดและลูปปิด เหมือนกับที่แสดงในบทความก่อนหน้านี้ แต่ในครั้งนี้ใช้การอ่านและคำนวนความเร็วแบบใหม่ที่นำเสนอ อัลกอริธิมพีไอดียังคงเป็นรูปแบบเดิม แต่ครั้งนี้เราปรับค่าพารามิเตอร์ใหม่ คือ Kp = 50, Ki = 3, and Kd = 0.5 นอกจากนั้นในการทดลองครั้งนี้ สวิทซ์ที่แทนการรบกวนแรงบิดจะถูกกดที่เวลาประมาณ 1/3 ของเวลาทั้งหมด และปล่อยสวิทซ์ที่เวลาประมาณ 2/3 เมื่อเปรียบเทียบกับผลในรูปที่ 3 และ 4 ในบทความ eMotor Phones Home พบว่าข้อมูลความเร็วรอบที่อ่านและคำนวนโดยวิธีการใหม่นี้จะมีความน่าเชื่อถือมากกว่า และใช้งานได้ดีในช่วงความเร็วรอบของ eMotor คือ 0 - 600 รอบ/นาที (ค่าคำสั่งและความเร็วที่พล็อตในรูปมีหน่วยเป็น รอบ/วินาที) ค่ายอดที่เกิดขึ้นในผลตอบสนองลูปปิดในช่วงที่มีการกดสวิทซ์เป็นผลจากการปรับค่าพีไอดี ซึ่งยังไม่ได้ปรับแต่งให้ได้ผลตอบสนองที่เหมาะสม หากทดลองลดค่าของ Ki ลง จะพบว่าค่ายอดลดลง แต่ผลตอบสนองจะเข้าสู่ค่าคำสั่งช้าลงด้วย เมื่อปรับค่า Kp เพิ่มขึ้นประมาณ 200 ระบบจะเสียเสถียรภาพ

รูปที่ 1: ผลตอบสนองลูปเปิดเมื่อมีการรบกวน รูปที่ 2: ผลตอบสนองลูปปิดเมื่อมีการรบกวน