以前、M5stickCからU加速度センサーの位置情報をUDPで送信してメカナムホイールラジコンを制御してみました。↓
今回はこれを応用して、ドローン（TELLO）を制御してみましたので、ここでまとめておこうと思います。
初めに
おおまかな概要
今回制御するTELLOの操作についての大まかな概要は下記のとおりです。
コントローラー（M5stickC) ⇒ UDP通信 ⇒ TELLO
M5stickCの加速度センサーを利用して、ピッチ角、ロー角の傾きに応じてその方向にTELLOが動くようにします。
TELLOについて
当初、TELLO側にもプログラムを書き込むものと思い込んでいたのですが、どうやら基本的には無理なようです。
代わりに、TELLOは電源投入後APモードで待ち受けてくれるため、コントローラー側からTELLOにWIFIで接続後、UDP通信で決められた文字列を送信すれば、文字列に応じて色んな動作をしてくれるようになっているようです。
その辺りの詳細につきましては、下記サイトを参考にさせて頂きました。↓
つまり、コントローラー側のみプログラムを作成すれ事でTELLOを操縦できます。
※今回プログラムにはpythonを使っています。
プログラム
M5stickC(micropython)
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 |
import network import time from socket import socket, AF_INET, SOCK_DGRAM from m5stack import * from m5ui import * from uiflow import * from m5stack import lcd import imu imu0 = imu.IMU() wlan = network.WLAN(network.STA_IF) wlan.active(True) if not wlan.isconnected(): print('connecting to network...') wlan.connect('TELLO-D436CD')#, '') while not wlan.isconnected(): pass print('network config:', wlan.ifconfig()) socket = socket(AF_INET, SOCK_DGRAM) tello_address = ('192.168.10.1' , 8889) socket.sendto('command'.encode('utf-8'),tello_address) print ('start') while(True): if btnA.wasPressed():#imu0.ypr[1] < -15 and -10 < imu0.ypr[2] < 10: #lcd.clear(lcd.BLACK) #lcd.clear(lcd.BLUE) socket.sendto('takeoff'.encode('utf-8'),tello_address) print ('takeoff') elif btnA.pressFor(0.8):#imu0.ypr[1] > 30 and -10 < imu0.ypr[2] < 10:#btnB.wasPressed(): #lcd.clear(lcd.BLACK) #lcd.clear(lcd.RED) socket.sendto('land'.encode('utf-8'),tello_address) print ('land') elif btnB.wasPressed(): socket.sendto('up 40'.encode('utf-8'),tello_address) print ('up') elif btnB.pressFor(0.8): socket.sendto('down 40'.encode('utf-8'),tello_address) print ('down') elif -10 < imu0.ypr[1] < 10 and 20 < imu0.ypr[2]:#btnC.wasPressed(): # ボタンAが押されているか lcd.clear(lcd.BLACK) lcd.clear(lcd.GREEN) socket.sendto('right 40'.encode('utf-8'),tello_address) print ('right') elif -10 < imu0.ypr[1] < 10 and -80 < imu0.ypr[2] < -20: lcd.clear(lcd.BLACK) lcd.clear(lcd.ORANGE) socket.sendto('left 40'.encode('utf-8'),tello_address) print ('left') elif imu0.ypr[1] < -15 and -10 < imu0.ypr[2] < 10: lcd.clear(lcd.BLACK) lcd.clear(lcd.BLUE) socket.sendto('forward 40'.encode('utf-8'),tello_address) print ('forward') elif imu0.ypr[1] > 30 and -10 < imu0.ypr[2] < 10:#btnB.wasPressed(): lcd.clear(lcd.BLACK) lcd.clear(lcd.RED) socket.sendto('back 40'.encode('utf-8'),tello_address) print ('back') elif 80 < imu0.ypr[2]: lcd.clear(lcd.BLACK) lcd.clear(lcd.RED) socket.sendto('flip r'.encode('utf-8'),tello_address) print ('flip r') elif -80 > imu0.ypr[2]: lcd.clear(lcd.BLACK) lcd.clear(lcd.RED) socket.sendto('flip l'.encode('utf-8'),tello_address) print ('flip l') elif -15 > imu0.ypr[1] and 80 > imu0.ypr[2] > 10: lcd.clear(lcd.BLACK) lcd.clear(lcd.YELLOW) socket.sendto('cw 90'.encode('utf-8'),tello_address) print ('cw 90') elif -15 > imu0.ypr[1] and -80 < imu0.ypr[2] < -10: lcd.clear(lcd.BLACK) lcd.clear(lcd.PINK) socket.sendto('ccw 90'.encode('utf-8'),tello_address) print ('ccw 90') else: lcd.clear(lcd.BLACK) socket.sendto('stop'.encode('utf-8'),tello_address) print ('stop') time.sleep(0.03) |
初めにTELLOのIPアドレスに接続しています。デフォルトでは192.168.10.1みたいです。解放されているポート番号は8889。
sendでTELLOに文字列を送信していますが、”command”を初めに送信する事で、文字列に対応した動作をするモードに入るみたいです。
takeoffなら離陸、forwardなら前進、といった感じです。スペース後の数字は〇〇cm動きます。
完成
こんな感じで動きます。↓
