search

ShunjiHashimoto / tang

収穫サポートロボットTANG
2 stars 1 forks source link

非接触回転センサでロボットの移動速度を推定する #184

Closed ShunjiHashimoto closed 2 years ago

ShunjiHashimoto commented 2 years ago

https://github.com/ShunjiHashimoto/tang/issues/183 IMU使っての速度推定がイマイチな結果になったので、この回転センサを使って速度推定を行う

ShunjiHashimoto commented 2 years ago
ShunjiHashimoto commented 2 years ago

現状

http://garakutakohbo.web.fc2.com/labo/OH182/OH182.htm 回路くんでやってみたら正常に動作した。

今後について

来週一週間で回転数から速度推定できるようにしたい。

ShunjiHashimoto commented 2 years ago
ShunjiHashimoto commented 2 years ago

モジュールM = ピッチ円の直径（D）/歯数（Z）= 33mm/16個 = 2.0・・・mm 以下の写真では、moduleが2mmのときはd = d0なのでd = 2mmでおけ つまり、ギアとセンサの間は2mmにするべし Screenshot from 2022-05-08 21-09-23

ShunjiHashimoto commented 2 years ago

LM339データシート https://datasheets-pdf.com/lm339-pdf-%E3%83%87%E3%83%BC%E3%82%BF%E3%82%B7%E3%83%BC%E3%83%88-pdf/

ShunjiHashimoto commented 2 years ago

回路のオペアンプの部分を理解しようとしたが、いまいちわからんな〜。もう少し解読する必要がありそうだ

ShunjiHashimoto commented 2 years ago

コンデンサ！

ShunjiHashimoto commented 2 years ago

コンデンサには放電のために安全抵抗をつけるらしい

ShunjiHashimoto commented 2 years ago

キャプチャ 静電容量 0.1μF = 100nF

ShunjiHashimoto commented 2 years ago

キャプチャ この部分は仕様書に書かれている。理由はよくわからん奴

ShunjiHashimoto commented 2 years ago

キャプチャ これがすべて

ShunjiHashimoto commented 2 years ago

分圧の式 (1330/11330)*5V = 0.58666V 5Kは保護回路用の抵抗です。

ShunjiHashimoto commented 2 years ago

回路理解したから次はこれを使って実際にモータの回転量調べる それができたら次は取付けてみたい！

ShunjiHashimoto commented 2 years ago

ラズパイから5Vを供給する いちいちTANGからラズパイを取り出して動作確認するのがめんどくさいのでmicro ssd買ってもう１台をデバッグ用とする

ShunjiHashimoto commented 2 years ago

OutPutのピンでは0.1Vから0.48Vを行き来している。どうやってHIGH、LOWにするのか

ShunjiHashimoto commented 2 years ago

Raspberry piのGPIO 電圧(THレベル)：Low 0～1.19V, High 1.34～3.3V

ShunjiHashimoto commented 2 years ago

Raspberry pi のGPIO 入力電圧 0 ~ 0.8V:LOW(OFF)として判断 0.8 ~1.3V : 不定（不安定でどっちになるかわからない） 1.3 ~ 3.3V: HIGH(ON)として判断 出力電圧 ON : 3.3V出力 Off : GND(出力)

ShunjiHashimoto commented 2 years ago

http://cba.sakura.ne.jp/board/QA010.htm こちらの記事ではOH182の出力をワンショットマルチバイブレータを使っている。 その後、GPIOピンに繋いでいる 動作時間を継続させるために使用するらしい

ShunjiHashimoto commented 2 years ago

ではどうやってOutPutのピンでは0.1Vから0.48Vを行き来している。どうやってHIGH、LOWにするのか？

ShunjiHashimoto commented 2 years ago

あれｍ、今見たら0.1Vから0.48Vではなく4.8Vにずっと張り付いている。なぜ 明日回路もう一度見直そう

ShunjiHashimoto commented 2 years ago

コンパレータ回路使って出力を0.0V⇔5.0Vになるよう変更でけた～、次はラズパイのGPIOで出力をひろって、回転数数えるぞい

ShunjiHashimoto commented 2 years ago

https://github.com/ShunjiHashimoto/tang/issues/184#issuecomment-1121298079 この人が正しかったorz

ShunjiHashimoto commented 2 years ago

ラズパイの予備セットアップ中 microSDカードにubuntu18.04書き込んで、起動しようとしている最中 電源とラズパイのHDMIが見つからぬ

ShunjiHashimoto commented 2 years ago

ラズパイのセットアップ完了

ShunjiHashimoto commented 2 years ago

回転数数えるプログラムとモータを回転させてカウントする仕組み作る

ShunjiHashimoto commented 2 years ago

今日のTODO

ShunjiHashimoto commented 2 years ago

勝った！！ モータの回転数を正確に調べることができた。 詰まったところとしては、制御周期を明示しない場合cntが不正確になってしまった。おそらく周期が早すぎて、なんかおかしなことになっていた、もしくは周期が不正確なので、ゆっくりとカウントされていたか、 多分後者な気がする。また調べよ！ 明日は残りのTODOを終えて価値とする！

https://twitter.com/23232871hashimo/status/1525780988957425665?s=20&t=iQ8prYs4CLCisVEx8f28Uw

ShunjiHashimoto commented 2 years ago

速度推定ってどうするの？ ギアがある時間にこれくらい回転したから速度はこれみたいな

ShunjiHashimoto commented 2 years ago

制御周期t = 0.01 １歯あたりの距離d カウント数 = 20 ギアが進んだ距離L = 20×d 速度 = 20d/t カウント数をリセットする ・・・・ をループすることで現在の速度を推定する

ShunjiHashimoto commented 2 years ago

基板は後回しにして、ユニバーサル基盤に実装していって、とりあえずそれを取り付ける んで、ハードウェアもいい感じに取り付けられるように設計をし直す。これが来週1週間でやること。

その次の１週間で統合テスト。 速度を求めてKFに渡す。

その次の１週間は予備日

その次の１週間で資料を作る

ShunjiHashimoto commented 2 years ago

image モーターカバーにセンサを取り付けて2本線をラズパイ側に伸ばせばいけそう

ShunjiHashimoto commented 2 years ago

ユニバーサル基板と、抵抗5kと3kを買ってくる 予備センサも買う

ShunjiHashimoto commented 2 years ago

image

ShunjiHashimoto commented 2 years ago

①カバーの内面の高さは300mm、ギアの高さは200mm つまり1cmくらい余裕があるポイ

ShunjiHashimoto commented 2 years ago

カバーの外寸は20mm

ShunjiHashimoto commented 2 years ago

センサを取り付けるには4mmくらい底上げする

ShunjiHashimoto commented 2 years ago

底上げ治具印刷した。 とりあえずテープでジグをくっつけて、そこにセンサを貼り付けて、カバーとクローラの隙間から2本線を出そうか

ShunjiHashimoto commented 2 years ago

底上げした。材料買ってきた ユニバーサル基板に部品をはんだ付けする。試しに１つだけでいい。 そのあとは、つなげて動作確認行う。

ShunjiHashimoto commented 2 years ago

KiCAD使うと時間がかかりそうなので、とりあえずブレットボード上で配線してみる。 その後ブレッドボード基板でハンダ付して動作確認する

ShunjiHashimoto commented 2 years ago

ブレッドボード基板でハンダ付でけた！ 次は配線してちゃんと回転数数えられるか動作確認！！ image

ShunjiHashimoto commented 2 years ago

センサをロボットに取り付けて、回転数をカウントすることができた。取り付けた位置は印付けてある。 次は、

  1. センサをしっかりと固定するために治具を作る
  2. もう片方のモータにもセンサを取り付けるか
ShunjiHashimoto commented 2 years ago
ShunjiHashimoto commented 2 years ago

image ２つ回路作れた、一部誤りがあったのをメモしておいた

ShunjiHashimoto commented 2 years ago

カバーへのセンサの取り付けは本来はCAD使ってちゃんと固定したいところだが、今回は一旦養生テープで我慢しておく

ShunjiHashimoto commented 2 years ago
ShunjiHashimoto commented 2 years ago

https://www.onosokki.co.jp/HP-WK/eMM_back/emm220.pdf https://www.keyence.co.jp/ss/products/measure-sys/machine-elements/gear/basic.jsp https://docs.google.com/presentation/d/1wiYviIgE20hC4sgee-OIXk0eCq3CGM38Qdrv87YEgmw/edit#slide=id.p 資料作成途中、頭回らんから今日はやめとく 両輪で回転数数えるところまできた、次は速度推定後、その情報をもとにKF

ShunjiHashimoto commented 2 years ago

回転数を割り込みで処理する。 GPIOのHIGH(立ち上がり)を検出したときにcntをカウントする。 pythonではイベントコールバックみたいにできるらしい、ROSであればSubscriberでもいいかも。いや、ROSでは回転角度を投げ続けるノードが必要。Pythonでいいか。

そのコールバックでcntをインクリメントして、その数を保持しておく。 そして、while文では常に速度を計算する。 このとき、速度を計算する処理では while(){ 　デルタ = 今のカウント - 前回のカウントを算出 　速度 = デルタ/制御周期×円ピッチ cnt = 0 # 初期化 }

ShunjiHashimoto commented 2 years ago 
import pprint

# 割り込みイベント設定
# bouncetimeは割り込みを行った後設定された時間は割り込みを検知しないという糸
GPIO.add_event_detect(right_gear_pin, GPIO.RISING, bouncetime=1000)
GPIO.add_event_detect(left_gear_pin, GPIO.RISING, bouncetime=1000)

# コールバック関数登録
GPIO.add_event_callback(pin, self.add_right_gear_count) 
GPIO.add_event_callback(pin, self.add_left_gear_count) 

cnt_list = [0, 0] # right = 0, left = 1

def add_right_gear_count():
        cnt_list[0]+=1

def add_left_gear_count():
        cnt_list[1]+=1

def calc_velocity(cnt):
      retunr cnt*円ピッチ/制御周期

def main():
     while(1):
        vel_r = calc_velocity(cnt_list[0])
        vel_l = calc_velocity(cnt_list[1])
        print( '\033[31m'+'右車輪の速度: %lf '+'\033[0m' + '\033[32m'+'左車輪の速度: %lf '+'\033[0m', vel_r, vel_l)
        cnt_list = [0, 0]
        loop_rate(10ms)
ShunjiHashimoto commented 2 years ago

今朝Jetsonでもギアの回転数を数えられることを確認した。 次は、

  1. 推定した速度が正しいか予備実験を行う、v = (vr+vl)/2、（推定した速度×制御周期）++して得られた距離5mと実際にロボットが動いた距離でどれくらい誤差があるか？Joystickでロボットを動かす。5m動かしたらpwmを切って強制停止させる。 そのときのスタート地点からゴール地点までの距離を計測、比較する。
    • [x] 実験用のスクリプト作成
      • [x] joystick入力で遠隔操作
      • [x] 進んだ距離が5mになれば強制終了
    • [x] 制御ボックスに入ったraspberrypiに最新のコードをgit pullする
    • [x] 実験
  2. human_detectionノードにこのギア回転数カウントスクリプトを移植する
  3. 速度をKFモジュールに移植する
ShunjiHashimoto commented 2 years ago

進んで距離に対して誤差が倍以上あったので、試行錯誤して、センサの距離がいまいちからだと思い、少し近づけると改善した。ただ、カバーが反対である右車輪のほうの回転数がまだいまいちなので、もう少し距離を調整するのと、逆回転の時に回転数がバグるのも直したい