ロボットの操作とサンプルコード解説
はじめに
ついにロボットが完成したので、サンプルプログラムをもとにロボットで遊んでいきたいと思います。
サンプルコードをいきなりすべて理解することは大変なので、試行錯誤しながら動かしていきます。
ロボットの操作
電源について
ロボットの電源はバッテリーでもUSBでもどちらでも利用可能です。
ただし、RaspberryPi4の場合は消費電力が高いためか、USB-Type Cの入力を利用しないと、プログラム利用中に再起動してしまいます。
プログラム起動
サンプルプログラム
Client（PC側）
GUI.pyについて
おおまかに2つの機能が動いています
- ロボット操作機能
- カメラ映像描画機能
- RaspberryPiへ接続した後に、RaspberryPiのカメラ映像を受信して表示する機能です。
気を付けなければいけない点として、ロボット操作機能はクライアント、カメラ映像描画機能はサーバとして別々の機能として動いています。
### server.py抜粋 ### ip_adr（RaspberryPi）へTCPで接続している。 def socket_connect(): #Call this function to connect with the server global ADDR,tcpClicSock,BUFSIZ,ip_stu,ipaddr ip_adr=E1.get() #Get the IP address from Entry SERVER_IP = ip_adr SERVER_PORT = 10223 #Define port serial BUFSIZ = 1024 #Define buffer size ADDR = (SERVER_IP, SERVER_PORT) tcpClicSock = socket(AF_INET, SOCK_STREAM) #Set connection value for socket
### server.py抜粋 ### 映像受信のために、TCP 5555番ポートで通信を待ち受けている。 def video_thread(): global footage_socket, font, frame_num, fps context = zmq.Context() footage_socket = context.socket(zmq.SUB) footage_socket.bind('tcp://*:5555') footage_socket.setsockopt_string(zmq.SUBSCRIBE, np.unicode(''))
参考：TCPサーバとクライアントについて
【図解】初心者にも分かる TCP/UDP 〜違いや共通点,使い分け,ポート番号,具体例について〜│SEの道標
Server（RaspberryPi側）
プログラム構造
RaspberryPi側はいろいろな機能が実装されているので、プログラムが複数に分かれています。
細かな説明は触りながら見ていきたいと思いますので、メイン機能部のserver.pyを見ていきます。
server/ ├── FPV.py ├── info.py ├── instraction.txt ├── Instruction.txt ├── Kalman_filter.py ├── LED.py ├── move.py ├── PID.py ├── raspi-config.py ├── server.py ├── serverTest.py ├── servo.py ├── SpiderG.py ├── switch.py └── test.py
server.pyについて
ロボットを実行する際に起動するプログラムです。
GUI.pyと逆で、ロボット操作用のサーバ機能、カメラ映像配信用のクライアント機能が動作します。
server.py起動時にはいくつかの初期設定が完了した後は、run関数でGUIの操作を待ち続けます。
run関数ではGUIから入力された値をもとにif分で分岐して対応した機能が実行されていました。
## server.py抜粋 def run(): <中略> while True: data = '' data = str(tcpCliSock.recv(BUFSIZ).decode()) if not data: continue elif 'forward' == data: print('1') SpiderG.walk('forward') elif 'backward' == data: SpiderG.walk('backward') elif 'DS' in data: if turn_command == 'no': SpiderG.servoStop()
サーボモータの制御について
ロボットはボタン一つで前後左右に動きますが、実際には12個のサーボモータを駆使して足を動かしています。
## server.py、SpiderG.py抜粋 ## Forwardを入力すると以下の関数が呼び出される。 SpiderG.walk('forward') ## SpiderG.walk関数を見てみると... ## walkで移動コマンドを指定した後、繰り返し処理の中で各サーボモータを動かしていることがわかります。 def walk(direction): global goal_command goal_command = direction Servo.resume() def goal_GenOut(position_input, left_direction, right_direction): def leg_FL(pos, direction_input): if pos == 1: goal_dict['FLB'] = int(FLB_init_pwm + (wiggle_middle)*FLB_direction) goal_dict['FLM'] = int(FLM_init_pwm + (wiggle_v - FL_height)*FLM_direction) goal_dict['FLE'] = int(FLE_init_pwm + (wiggle_v + 0)*FLE_direction) elif pos == 2: goal_dict['FLB'] = int(FLB_init_pwm + (wiggle_middle + wiggle_h*direction_input)*FLB_direction) goal_dict['FLM'] = int(FLM_init_pwm - FL_height*FLM_direction) <略>
まとめ
今回は完成したロボットをGUIで操作しつつ、サンプルプログラムの全体像を確認してみました。
次回はお試しとしてLEDの色を変更する機能を追加したいと思います。
