RustでPID制御ライブラリ「advanced-pid-rs」を作る

11 14日前に更新

Rust入門としてPID制御ライブラリを作ってみる

PID制御ライブラリを作成し、公開する。

要件

no_std対応
PID制御だけでなくPI-D制御,I-PD制御や速度型PID制御にも対応
min, maxを設定できる

作ったもの

https://crates.io/crates/advanced-pid
https://docs.rs/advanced-pid/

参考

11 17日前に更新

モジュール構造

trait

FromとInto

intoの使いみち

generics

Defalt trait

no_stdのcrate

features

11 17日前に更新

PIDのゲインとリミット

PID制御器を初期化するための構造体を作る。

src/config.rs

#[derive(Debug, Clone)]
pub struct Gain {
    pub kp: f32,
    pub ki: f32,
    pub kd: f32,
}

#[derive(Debug, Clone)]
pub struct Config {
    pub gain: Gain,
    pub min: f32,
    pub max: f32,
}

位置型Pid制御

src/pid.rs

use super::PidConfig;

#[derive(Debug, Clone)]
pub struct Pid {
    config: PidConfig,
    i_term: f32,
    pre_error: f32,
}

ルートファイルを作成

src/lib.rs

pub mod config;
pub mod pid;

pub type PidGain = config::Gain;
pub type PidConfig = config::Config;

11 17日前に更新

PID制御器トレイトを作成

src/lib.rs

 pub mod config;
 pub mod pid;

 pub type PidGain = config::Gain;
 pub type PidConfig = config::Config;

+pub trait PidController {
+    fn new(config: PidConfig) -> Self;
+    fn update(&mut self, set_point: f32, actual: f32, dt: f32) -> f32;
+    fn reset_config(&mut self, config: PidConfig)
+    where
+        Self: core::marker::Sized,
+    {
+        *self = Self::new(config);
+    }
+}

位置型PID制御にPID制御器トレイトを実装

src/pid.rs

 use super::PidConfig;
 use super::PidController;

 #[derive(Debug, Clone)]
 pub struct Pid {
     config: PidConfig,
     i_term: f32,
     pre_error: f32,
 }

+impl PidController for Pid {
+    fn new(config: PidConfig) -> Self {
+        Self {
+            config,
+            i_term: 0.0,
+            pre_error: f32::NAN,
+        }
+    }
+    fn update(&mut self, set_point: f32, actual: f32, dt: f32) -> f32 {
+        let error = set_point - actual;
+        self.i_term += error * dt;
+        let d_term = if self.pre_error.is_nan() {
+            0.0
+        } else {
+            (error - self.pre_error) / dt
+        };
+        let output = self.config.gain.kp * error
+            + self.config.gain.ki * self.i_term
+            + self.config.gain.kd * d_term;
+        self.pre_error = error;
+        output.clamp(self.config.min, self.config.max)
+    }
+}

11 17日前に更新

GainとConfigにコンストラクタを作成

src/config.rs

#[derive(Debug, Clone, Default)]
pub struct Gain {
    pub kp: f32,
    pub ki: f32,
    pub kd: f32,
}

#[derive(Debug, Clone)]
pub struct Config {
    pub gain: Gain,
    pub min: f32,
    pub max: f32,
}

impl Default for Config {
    fn default() -> Self {
        Self {
            gain: Default::default(),
            min: f32::NEG_INFINITY,
            max: f32::INFINITY,
        }
    }
}

impl Config {
    pub fn new(kp: f32, ki: f32, kd: f32) -> Self {
        Self {
            gain: Gain { kp, ki, kd },
            ..Default::default()
        }
    }

    pub fn with_limits(self, min: f32, max: f32) -> Self {
        Self { min, max, ..self }
    }
}

impl From<Gain> for Config {
    fn from(gain: Gain) -> Self {
        Self {
            gain,
            ..Default::default()
        }
    }
}

11 17日前に更新

速度型PID制御(不完全微分)を実装

src/vel_pid.rs

use super::PidConfig;
use super::PidController;

#[derive(Debug, Clone)]
pub struct VelPid {
    config: PidConfig,
    output: f32,
    pre_error: f32,
    pre_p_term: f32,
    d_term_lpf: f32,
}

impl Default for VelPid {
    fn default() -> Self {
        Self::new(PidConfig::default())
    }
}

impl PidController for VelPid {
    fn new(config: PidConfig) -> Self {
        Self {
            config,
            output: 0.0,
            pre_error: f32::NAN,
            pre_p_term: f32::NAN,
            d_term_lpf: 0.0,
        }
    }
    fn update(&mut self, set_point: f32, actual: f32, dt: f32) -> f32 {
        let error = set_point - actual;
        let p_term = if self.pre_error.is_nan() {
            0.0
        } else {
            (error - self.pre_error) / dt
        };
        let d_term = if self.pre_p_term.is_nan() {
            0.0
        } else {
            (p_term - self.pre_p_term) / dt
        };
        self.d_term_lpf += (d_term - self.d_term_lpf) / 8.0;
        let du = self.config.gain.kp * p_term
            + self.config.gain.ki * error
            + self.config.gain.kd * self.d_term_lpf;
        self.pre_error = error;
        self.pre_p_term = p_term;
        self.output = (self.output + du).clamp(self.config.min, self.config.max);
        self.output
    }
}

11 16日前に更新

no_std対応

The Cargo bookを参考。

src/lib.rs

#![no_std]
#[cfg(feature = "std")]
#[allow(unused_imports)]
#[macro_use]
extern crate std;

FloatTypeをf32とf64で可変にする

src/lib.rs

#[cfg(all(feature = "f32", feature = "f64"))]
compile_error!("feature \"f32\" and feature \"f64\" cannot be enabled at the same time");
#[cfg(feature = "f32")]
type FloatType = f32;
#[cfg(feature = "f64")]
type FloatType = f64;

Cargo.toml

[features]
default = ["std", "f32"]
std = []
f32 = []
f64 = []

11 14日前

結局FloatTypeは`f64`のトグル式にした

src/lib.rs

#[cfg(not(feature = "f64"))]
type FloatType = f32;
#[cfg(feature = "f64")]
type FloatType = f64;

Cargo.toml

[features]
default = ["std"]
std = []
f64 = []

11 16日前に更新

crates.ioに公開する

GitHubアカウントを使って https://crates.io/me/ にログインする。
https://crates.io/me/ からcrateを公開するためのAPI Access Tokenを生成。

cargo login <TOKEN>
cargo publish

11 15日前に更新

Examplesを書く

examples/simple.rs

use advanced_pid::{pid::Pid, PidController, PidGain};

fn main() {
    let gain = PidGain {
        kp: 1.0,
        ki: 0.1,
        kd: 0.1,
    };
    let mut pid = Pid::new(gain.into());

    println!("{:5.2}", pid.update(1.0, 0.0, 1.0));
    println!("{:5.2}", pid.update(1.0, 0.5, 1.0));
    println!("{:5.2}", pid.update(1.0, 0.8, 1.0));
}

examples/vel_pid.rs

use advanced_pid::{vel_pid::VelPid, PidConfig, PidController};

fn main() {
    let config = PidConfig::new(1.0, 0.1, 0.1).with_limits(-1.0, 1.0);
    let mut pid = VelPid::new(config);

    let target = 1.0;
    let dt = 1.0;

    println!("{:5.2}", pid.update(target, 0.0, dt));
    println!("{:5.2}", pid.update(target, 0.1, dt));
    println!("{:5.2}", pid.update(target, 0.3, dt));
}

examples/以下においた内容は下記で実行できる。

cargo run --example <FILE_NAME>
# cargo run --example simulation

11 13日前に更新

CI

.github/workflows/ci.yaml

name: CI
on:
  push:
    branches:
      - main
  pull_request:
    branches:
      - main

env:
  CARGO_TERM_COLOR: always

jobs:
  rust-checks:
    name: Rust Checks
    runs-on: ubuntu-22.04
    timeout-minutes: 10
    strategy:
      fail-fast: false
      matrix:
        action:
          - command: build
            args: --release
          - command: fmt
            args: --all -- --check --color always
          - command: clippy
            args: --workspace -- -D warnings
    steps:
      - name: Checkout repository
        uses: actions/checkout@v4
      - name: Enable caching
        uses: Swatinem/rust-cache@v2
      - name: Run command
        run: cargo ${{ matrix.action.command }} ${{ matrix.action.args }}

TODO

~~テストを実行する~~
~~自動でリリースを作成してpublish~~

11 14日前

テストを追加した

.github/workflows/ci.yaml

+          - command: test
+            args: --workspace --release

11 14日前に更新

testを書く

実装したファイルの中に下記のような感じで書くらしい。
cfg(test)とすることでtest時のみビルドされる。またmod testsとuse super::*;によってテストをtests内に切り離しつつテスト対象はそのまま使用できるようになっている。

src/some_module.rs

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_some_test() {
        assert_eq!(1 + 2, 3);
    }
}

cargo testでテストを実行。

11 14日前に更新

ドキュメントを書く

実装ファイル内に下記の感じでドキュメントを書いていく。Markdown形式が使える。またドキュメント内の別のアイテムにリンクを貼ることもできる。

src/some_module.rs

//! これはモジュールに対するドキュメント

/// これは`Hoge`に対するドキュメント
struct Hoge {
}

cargo docでドキュメントをビルド。
生成物はtarget/doc/以下に設置される。

11 14日前に更新

バージョンアップ時の便利scriptを作成

scripts/bump-version.bash

#!/bin/bash
set -euo pipefail
cd "${BASH_SOURCE[0]%/*}"/..

# How to use
# ./scripts/bump-version.bash
# or if you want to specify the version
# VERSION="0.2.1" ./scripts/bump-version.bash

# 環境変数でバージョンが指定されたとき、Cargo.tomlを上書き
if [ -n "${VERSION:-}" ]; then
  echo "Bump Cargo.toml: ${VERSION}"
  TARGET=("Cargo.toml")
  sed -i -e "/version/s/\"[0-9]\+\.[0-9]\+\.[0-9]\+[^\"]*\"/\"${VERSION}\"/" "${TARGET[@]}"
fi

# TARGETのversionを更新する
TARGET=("README.md" "src/lib.rs")
# Cargo.toml のバージョンを取得
VERSION=$(cargo metadata --no-deps --format-version 1 | jq -r '.packages[] | select(.name == "advanced-pid") | .version')
echo "Version: ${VERSION:?"cann't read version."}, Target: ${TARGET[*]}"
# 正規表現でバージョンを置換
# "0.1.12-alpha" のようなバージョン表記を"${VERSION}"に置換する
sed -i -e "/advanced-pid/s/\"[0-9]\+\.[0-9]\+\.[0-9]\+[^\"]*\"/\"${VERSION}\"/" "${TARGET[@]}"

参考

11 13日前に更新

自動でcargo publish

.github/workflows/ci.yaml

name: CI
on:
  push:
    branches:
      - main
  pull_request:
    branches:
      - main

env:
  CARGO_TERM_COLOR: always

jobs:
  rust-checks:
    name: Rust Checks
    runs-on: ubuntu-22.04
    timeout-minutes: 10
    strategy:
      fail-fast: false
      matrix:
        action:
          - command: build
            args: --release
          - command: fmt
            args: --all -- --check --color always
          - command: clippy
            args: --workspace -- -D warnings
          - command: test
            args: --workspace --release
    steps:
      - name: Checkout repository
        uses: actions/checkout@v4
      - name: Enable caching
        uses: Swatinem/rust-cache@v2
      - name: Run command
        run: cargo ${{ matrix.action.command }} ${{ matrix.action.args }}

  create-release:
    name: Create Release
    needs: rust-checks
    if: ${{ needs.rust-checks.result == 'success' && github.event_name == 'push' && github.ref == 'refs/heads/main'}}
    runs-on: ubuntu-22.04
    timeout-minutes: 10
    steps:
    - name: Checkout repository
      uses: actions/checkout@v4
    - name: Create Tag
      id: create-tag
      uses: mathieudutour/github-tag-action@v6.1
      with:
        github_token: ${{ secrets.GITHUB_TOKEN }}
        default_bump: false
        dry_run: true  # tagはRelease作成時つける
    - name: Bump Version
      run: ./scripts/bump-version.bash
      env:
        VERSION: ${{ steps.create-tag.outputs.new_version }}
    - name: Push version
      run: |
        git config user.name "github-actions[bot]"
        git config user.email "github-actions[bot]@users.noreply.github.com"
        git switch -c ${{ env.branch }}
        git add .
        git commit -m "Bump version: ${{ steps.create-tag.outputs.new_tag }}"
        git tag -a ${{ steps.create-tag.outputs.new_tag }} -m "Bump version: ${{ steps.create-tag.outputs.new_tag }}"
        git push -u origin ${{ env.branch }}
        git push origin ${{ steps.create-tag.outputs.new_tag }}
      env:
        branch: "release/${{ steps.create-tag.outputs.new_tag }}"
    - name: Create and merge Pull Request
      run: |
        gh pr create --base ${GITHUB_REF_NAME} --head ${{ env.branch }} --assignee ${GITHUB_ACTOR} -l "bump version" \
            --title "Bump version: ${{ steps.create-tag.outputs.new_tag }}" --body 'Automated changes by [CI](https://github.com/teruyamato0731/advanced-pid-rs/actions/workflows/ci.yaml) GitHub action'
        gh pr merge ${{ env.branch }} --merge --delete-branch
      env:
        GH_TOKEN: ${{ secrets.GITHUB_TOKEN }}
        branch: "release/${{ steps.create-tag.outputs.new_tag }}"
    - name: Publish
      run: cargo publish
      env:
        CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }}
    - name: Create a GitHub release
      uses: ncipollo/release-action@v1
      with:
        tag: ${{ steps.create-tag.outputs.new_tag }}
        body: ${{ steps.create-tag.outputs.changelog }}

11 13日前に更新

良さげなactionを発見

    - name: Push verion
      uses: EndBug/add-and-commit@v9
      with:
        committer_name: github-actions[bot]
        committer_email: github-actions[bot]@users.noreply.github.com
        fetch: false
        message: "Bump version: ${{ steps.create-tag.outputs.new_tag }}"
        new_branch: release/${{ steps.create-tag.outputs.new_tag }}
        tag: ${{ steps.create-tag.outputs.new_tag }}

11 13日前に更新

CI/CDをローカルでテストする

nektos/actを使用して毎回pushせずにCI/CDを実行できるようにする。

https://nektosact.com/introduction.html

Install nektos/act

下記コマンドで./bin/actにInstallされる。各自でPATHを通すなどしてください。

curl --proto '=https' --tlsv1.2 -sSf https://raw.githubusercontent.com/nektos/act/master/install.sh | sudo bash

Dockerに依存しているため、DockerのInstallも必要。なお後述する方法でDockerを使用せずにself-hosted runnerを指定することもできる。

Push時のworkflowを実行

pushは省略可。

act push

初回実行時はactで使用するDockerイメージのサイズを聞かれる。Largeは18Gもあるので慎重に選ぼう。なおMediumだとcargoが入っていなかった。毎回docker pullが走って嫌な場合は以下のoptionをつけよう。Defaultではtrueになっている。

act --pull=false

またdryrunモードでも実行できる。
dryrunの場合はactionが成功する前提で、どのworkflow が実行されるのか確認できる模様。

act -n

Dockerなしで実行

self-hosted runnerを指定すればDockerなしで動かせる。大抵のactionはnode等のツールに依存するので自分で用意する必要がある。

act -P ubuntu-22.04=-self-hosted

Node.js 20のInstall

curl -fsSL https://deb.nodesource.com/setup_20.x | sudo -E bash - && sudo apt-get install -y nodejs

イメージの指定も可能。

act -P <platform>=<docker-image>

Sercrets

$GITHUB_TOKENがないのでいくつかのactionで文句を言われる。
Sercretsを設定すれば実行できる。
GITHUB_TOKENが必要な場合、PATを生成して渡す必要がある。
適当なファイルを作成し、--secret-fileとして渡す。

.act.secrets

GITHUB_TOKEN=XXXXXXXXXXXXXXXXXXXX

act --secret-file .act.secrets

事前設定

~/.actrcに設定を書いておくこともできるらしい。

~/.actrc

-P ubuntu-latest=catthehacker/ubuntu:act-latest
-P ubuntu-22.04=catthehacker/ubuntu:act-22.04
-P ubuntu-20.04=catthehacker/ubuntu:act-20.04
-P ubuntu-18.04=catthehacker/ubuntu:act-18.04
--secret-file .act.secrets
--env-file .act.env

11 13日前に更新

Docker内でactを走らせる

act/Dockerfile

FROM node:18-bookworm-slim

RUN apt-get update && apt-get install -y \
  curl \
  gcc \
  pkg-config \
  jq \
  && rm -rf /var/lib/apt/lists/*

RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y
RUN curl --proto '=https' --tlsv1.2 -sSf https://raw.githubusercontent.com/nektos/act/master/install.sh | sh -s -- -b /usr/local/bin

ENV PATH /root/.cargo/bin:$PATH

COPY docker-entrypoint.sh /usr/local/bin/
ENTRYPOINT ["docker-entrypoint.sh"]

CMD [ "act" ]

act/docker-entrypoint.sh

#!/bin/sh
set -e

# Run command with act if the first argument contains a "-" or is not a system command. The last
# part inside the "{}" is a workaround for the following bug in ash/dash:
# https://bugs.debian.org/cgi-bin/bugreport.cgi?bug=874264
if [ "${1#-}" != "${1}" ] || [ -z "$(command -v "${1}")" ] || { [ -f "${1}" ] && ! [ -x "${1}" ]; }; then
  set -- act "$@"
fi

exec "$@"

act/docker-compose.yml

services:
  nektos-act:
    build:
      context: .
      dockerfile: Dockerfile
    volumes:
      # Working
      - ..:/workspaces/advanced-pid-rs:cached
    working_dir: /workspaces/advanced-pid-rs
    tty: true
    command: act --pull=false -P ubuntu-22.04=-self-hosted --secret-file act/.act.secrets

Rust入門としてPID制御ライブラリを作ってみる

作ったもの

参考

PIDのゲインとリミット

位置型Pid制御

ルートファイルを作成

PID制御器トレイトを作成

位置型PID制御にPID制御器トレイトを実装

GainとConfigにコンストラクタを作成

速度型PID制御(不完全微分)を実装

no_std対応

FloatTypeをf32とf64で可変にする

Cargo.toml

結局FloatTypeはf64のトグル式にした

crates.ioに公開する

Examplesを書く

CI

TODO

testを書く

ドキュメントを書く

バージョンアップ時の便利scriptを作成

参考

自動でcargo publish

CI/CDをローカルでテストする

Install nektos/act

Push時のworkflowを実行

Dockerなしで実行

Sercrets

事前設定

Docker内でactを走らせる

結局FloatTypeは`f64`のトグル式にした