ドローンの離陸をマニュアルで試みるのは、なかなか困難だ。私が不器用なのか、歳をとって手の運動神経が老化しているのもあるだろう。そこで、離陸はプログラムで制御するようにしている。
これは、私の部屋にちょっとした離陸ベースを置いてプログラムで飛ばしたものだ。コマンドは、１行ずつ実行する。途中に、最後で定義されているマクロが挟まれる。$で始まる名前は、変数だ。^で始まるものは関数。@で始まるものがマクロだ。その他、細かいプログラミングルールは、こちらに書いておいた。
離陸直前のスロットルの状態をすこい維持して、その間にプロペラの回転バランスをとるように、モータースピードを調整するマクロ（@setspeedadjustment）が挟んである。
#-------------------------
# pidscale ver.0 2022年2月28日
# プログラムオリジナル作成
# pidscale ver.1 2022年3月20日
# ホバリング高度に到達した時点でPID制御に入る
# pidscale ver.1 2022年3月27日
# モータースピードの調整を自動化するマクロ追加
#
# Attitudeの姿勢制御ループに対するコマンドは、
# 繰り返しの時間を念頭に置かないとコマンドが有効にならない
#-------------------------
#基本定数
# テスト用
#$takeoffthrottle = 30
#$hoveringthrottle_max = 30;
#$hoveringthrottle_min = 30;
# 本番用
$takeoffthrottle = 39
$hoveringthrottle_max = 43;
$hoveringthrottle_min = 37;
#
^motor(on)
# motor on の有効化を0.1秒待つ：必須
^sleep(0.1)
# Arming
^throttle(20)
# sleep の引数は実数値可
^sleep(1.5)
^throttle(30)
# 回転数が上がる時間を稼ぐ
^sleep(0.7)
# モータースピード調整マクロの実行
# このスピードで、1秒使われる
@setspeedadjustment
# 調整に馴染ませる時間をとる
^sleep(0.5)
# 離陸
^print(離陸開始します)
^throttle($takeoffthrottle)
##############################
# 離陸高度チェックループ 開始
# ループを正しく抜けれるかどうか事前テストすること
##############################
$count = 0
$flag = -1
loop $flag < 0
$altitude = ^getTakeoffAltitude()
^print(離陸：No.+$count+ 現在高度 [ +$altitude+ ])
# 高度が指定の高さ以上になるまで空回りする
if $altitude > 30
^print(離陸：指定高度の30cmを超えたのでループを抜けます)
$flag = 1
endif
# 10msec 停止する
^sleep(0.01)
$count = $count + 1;
# 指定高度に0.5秒で到達しなかったら強制的にループを抜ける
if $count > 50
^print(離陸：規定チェック回数を超えたのでループを抜けます)
$flag = 1
endif
endloop
##############################
# 離陸高度チェックループ 終了
##############################
^print(PID制御を開始します)
# PID制御に入る
# 最高高度に入る直前だと思われる
^pidscale(50,20,50)
^throttle($hoveringthrottle_max)
^sleep(0.7)
^throttle($hoveringthrottle_min)
##############################
# ホバリングループ 開始
# $hoveringthrottle_min では、下降してしまうので
# スロットルを調整する
##############################
# 変数再初期化
$count = 0
$flag = -1
loop $flag < 0
# 60cm 以上の高度を想定していないので、こちらの高度センサーを使う
$altitude = ^getTakeoffAltitude()
^print(ホバリング：No.+$count+ 現在高度 [ +$altitude+ ])
# 高度が下がったら再上昇を試みる
if $altitude < 45
^print(ホバリング：高度が45cm以下になったのでスロットルを上昇させます)
^throttle($hoveringthrottle_max)
endif
if $altitude > 55
^print(ホバリング：高度が55cm以上になったのでスロットルを低下させます)
^throttle($hoveringthrottle_min)
endif
# 10msec 停止する
^sleep(0.01)
$count = $count + 1;
# ホバリングを指定時間試みたらループを抜ける
if $count > 100
^print(ホバリング：1秒経過したのでループを抜けます)
$flag = 1
endif
endloop
##############################
# ホバリングループ 終了
##############################
# ^sleep(0.5)
#########################
# 着陸に入る
# スロットルを少しずつ下げる
#########################
^print(着陸します)
^throttle($hoveringthrottle_min)
^sleep(0.3)
^throttle(30)
^sleep(0.4)
^throttle(25)
^sleep(0.4)
# throttleの有効化の前にmotor off にならないように待つ。0.1秒以上
# また、モーターが完全に停止するまでのログを取るという意味もある。それで3秒にしている
^print(モーターを停止させます)
^throttle(0)
^sleep(2)
^motor(off)
# メインプログラム終了defmacro setspeedadjustment
#################
# スピード調整設定マクロ
# スロットル関係の関数は使っていないので、
# ここに入る前にスロットルは調整すべき
#################
^print(スピード調整設定マクロを実行します)
# 調整スピードをゼロに設定：初期化
^motorSpeedAjustment(0.0,0.0,0.0,0.0)
# 各プロペラ回転数の合計
$psum_0 = 0
$psum_1 = 0
$psum_2 = 0
$psum_3 = 0
^print(調整スピードをゼロに設定：初期化 ループの開始)
# 誤差取得のループ
$flag = -1
$count = 0
loop $flag < 0
#プロペラ回転数を取得する
^getPropellaRpm()
^print(No.+$count+ 回転数 + $getPropellaRpm_0 +,+ $getPropellaRpm_1 +,+ $getPropellaRpm_2 +,+ $getPropellaRpm_3)
# 取得した回転数を加える
$psum_0 = $psum_0 + $getPropellaRpm_0
$psum_1 = $psum_1 + $getPropellaRpm_1
$psum_2 = $psum_2 + $getPropellaRpm_2
$psum_3 = $psum_3 + $getPropellaRpm_3
$count = $count + 1
if $count >= 100
# 1秒測る
$paverage_0 = $psum_0 / $count
$paverage_1 = $psum_1 / $count
$paverage_2 = $psum_2 / $count
$paverage_3 = $psum_3 / $count
^print(1秒経過で測定終了 平均： + $paverage_0 +,+ $paverage_1 +,+ $paverage_2 +,+ $paverage_3)
# 終了フラグを立てる
$flag = 1
endif
^sleep(0.01)
endloop
# ４つのモーターについてさらに平均を求める
$totalaverage = ($paverage_0 + $paverage_1 + $paverage_2 + $paverage_3)/4
^print(全体平均回転数 + $totalaverage + rpm)
# 平均との差から調整値を求める
# 調整係数
$adjusttingdeflator = 200
$adjust_0 = ($paverage_0 - $totalaverage)/$adjusttingdeflator
$adjust_1 = ($paverage_1 - $totalaverage)/$adjusttingdeflator
$adjust_2 = ($paverage_2 - $totalaverage)/$adjusttingdeflator
$adjust_3 = ($paverage_3 - $totalaverage)/$adjusttingdeflator
# 調整値をセットする
^print(モータースピード調整値： + $adjust_0 +,+ $adjust_1 +,+ $adjust_2 +,+ $adjust_3)
^motorSpeedAjustment($adjust_0,$adjust_1,$adjust_2,$adjust_3)
# マクロの終了
^print(スピード調整設定マクロを終了します)
endmacro
0 件のコメント:
コメントを投稿