Projects << Previous Next >> Printer

Control

Essentials of Arduino Boards Programming (電子書)

1. 在 Modelica 建立控制器:

model SimplePIDController
  input Real position;
  output Real torque;

  parameter Real Kp = 2.0;
  parameter Real Kd = 0.5;

  Real error;
  Real derror;
initial equation
  error = 0;
  derror = 0;
equation
  error = 0 - position;
  derror = der(error);
  torque = Kp * error + Kd * derror;
end SimplePIDController;

2. 匯出為 FMU

用 OpenModelica，匯出此模型為 FMU：

使用 OMEdit ➜ 選擇 Export ➜ FMI

勾選：

✔ FMU for Co-Simulation

或 ✔ FMU for Model Exchange（自行利用 PyFMI 提供 solver）

會產生一個 .fmu 檔（zip 檔格式）。

3. Webots 中撰寫 Python 控制器:

以便讓 Webots 的機器人由 FMU 控制。

from controller import Robot
from pyfmi import load_fmu

# Webots 設定
robot = Robot()
timestep = int(robot.getBasicTimeStep())

# 載入 FMU 控制器
controller = load_fmu("SimplePIDController.fmu")
controller.initialize()

# 獲取感測器與馬達
joint_sensor = robot.getPositionSensor("my_joint_sensor")
joint_sensor.enable(timestep)

motor = robot.getMotor("my_joint_motor")

while robot.step(timestep) != -1:
    position = joint_sensor.getValue()
    
    # 設定 FMU 輸入
    controller.set("position", position)
    
    # 執行一步 FMU 模型
    controller.do_step(current_t=robot.getTime(), step_size=timestep / 1000.0)
    
    # 取得輸出
    torque = controller.get("torque")
    
    # 輸出至 Webots 馬達
    motor.setTorque(torque)

Projects << Previous Next >> Printer