b8

8. Flow Chart

รูป 16 Rotary Inverted Pendulum Flow Chart

Pseudo Code การควบคุม Pendulum Rod
Algo: Rotary Inverted Pedulum
Input: Motor Encoder(A1), Magnetic Rotary Encoder(A2)
Output: Motor Degree(A1), Motor Volt(PWM)

Process:
While(Turn On)
     A1 <- Motor Encoder
     A2 <- Magnetic Rotary Encoder
     If (87<=A1 && A1<=93)
          Call Balancing Controller(LQR)
     Else
          Call Swing Up Controller(PID)
     End If
End While

การอ่าน Quadrature Encoder

รูป 17 การส่งข้อมูลจาก Megnetic Rotary Encoder และ Motor Encoder             

Megnetic Rotary Encoder และ Moter Encoder ใช้ การส่งข้อมูลแบบ Quad A/B-Mode เมื่อเกิดการหมุนและสามารถพิจารณาทิศทางการหมุน ได้จาก เฟสขาขึ้นของ A เทียบกับ B ดังนี้

1. หมุนทวนเข็มนาฬิกา เฟสขาขึ้นของ A จะเกิดขึ้นก่อนเฟสขาขึ้นของ B
2. หมุนตามเข็มนาฬิกา เฟสขาขึ้นของ A จะเกิดขึ้นหลังเฟสขาขึ้นของ B

Pseudo Code การอ่าน Encoder โดยใช้ Interrupt
Algo: Encoder Interrupt
Input: A,B
Output: Position
Process:
     If A = HIGH Then
         If B = HIGH Then
Incresses Position
         Else
Decresses Position
     Else
          If B = HIGH Then
Decresses Position
         Else
Incresses Position
 

การแปลงค่าจากพันซ์เป็นมุมองศา

Magnetic Rotary Encoder มีค่า 512 พันซ์ใน1รอบ
เมื่อ พันซ์เป็นค่าบวก หมุนทวนเข็มนาฬิกา

เมื่อ พันซ์เป็นค่าบวก หมุนทวนเข็มนาฬิกา

เมื่อ

A คือ มุมหน่วยองศา
P คือ พันซ์ที่ถูกนับในระบบ
S คือ มุมเริ่มต้นที่นับ พันซ์