②telloを飛ばそう・ソース(映像表示、録画、ジョイスティック)
==================================================================
①操作方法
モード1です
録画開始を押すと以下の画像が現れる
==================================================================
②telloを飛ばそう・ソース
/* ############################################################################### file name tello_xxxx.go PC変更によりjoystickが動かなくなった為変更した。 ① PCとtello をwifiで接続する ② /c/msys64/mingw64.exe を実行する $ go run tello_xxxx.go <--- 実行する PC画面にカメラ映像が出るはず joystickの操作で飛行開始(下記参照) ③ 飛行後は必ずプログラムを終了させること(joystickで12番を2回クリック) 以下はスタート時の画面のコピー ================================================================================== ================================================================================== yyyy@DESKTOP-15DPINR MINGW64 /f/go/tello_2 $ go run tello_20191217_good.go 2019/12/17 16:31:50 Initializing connections... 2019/12/17 16:31:50 Initializing devices... 2019/12/17 16:31:50 Initializing device Tello-216C9B011274873 ... 2019/12/17 16:31:50 Robot tello initialized. 2019/12/17 16:31:50 Starting Robot tello ... 2019/12/17 16:31:50 Starting connections... 2019/12/17 16:31:50 Starting devices... 2019/12/17 16:31:50 Starting device Tello-216C9B011274873... 2019/12/17 16:31:50 Starting work... Slow=2 録画表示が出ない時は[options]スイッチオン 録画のON/OFFは[options]スイッチO N/OFF ファイル名チェック中: tello_1.mp4 f_name_A= tello_20191217163150.mp4 <--- ファイル名が重なる場合は左記のように年月日時分秒とする Joystick 0 connected Joystick 1 connected [13628 ms] Button:8 state:1 <===== ボタン12を2回クリックでプログラム終了 [13754 ms] Button:8 state:0 [13871 ms] Button:8 state:1 [13989 ms] Button:8 state:0 END-------------- exit status 1 ================================================================================== ================================================================================== ④ 主な機能等 ジョイスチック機能/telloのカメラ映像をPCに映し出し機能 tello映像を記録する機能/「録画」文字をPC画面に表示/非表示する機能 ジョイスティックの操作画面表示 ---------------------------------------------------------------- ⑤ 実行に必要なファイル等 tello_xxxx.go ==> コンパイルで tello_xxxx.exe 飛行プログラム rokuga_img.go ==> コンパイルで rokuga_img.exe 【録画】表示プログラム img_out.go ==> コンパイルで img_out.exe 【joystick操作画面】表示プログラム joystick_tec2.png ==> 画像データjoystick操作画面 button1.png ==> 画像データ button2.png ==> 画像データ tello_1.mp4 ==> 録画した映像データ tello_20191217153329.mp4 ==> 録画した映像データ(ファイル名を変えている) joystickでの操作 --------------------------------------------------------------- | 12 11 | | |~| |~| 9 | | ~ ~ |~~| | | | | | | 5 1 ~~ | | o o |~~| |~~| | | | | | | | | | | | | ~~ ~~ | | 8 o----+----o 7 3 o----+----o 4 |~~| | | | | | | | | | | ~~ | | o o 10 | | 6 2 | |_______________________________________________________________| 1: 上昇 2: 下降 3: 左移動 4: 右移動 5: 前進 6: 後退 7: 右回転 8: 左回転 9: 2回クリックでtaleoff 10: 2回クリックでLAND 11: 1回録画ONで開始 トグルで録画ストップ 12: 2回クリックでプログラム終了 ---------------------------------------------------------------------------------------------- パスの設定 $ echo $PATH (:で改行させている) /mingw64/bin: /mingw64/bin: /usr/local/bin:/usr/bin: /bin: /c/Windows/System32: /c/Windows: /c/Windows/System32/Wbem: /c/Windows/System32/WindowsPowerShell/v1.0/: /usr/bin/site_perl: /usr/bin/vendor_perl: /usr/bin/core_perl: /c/Go/bin: /c/Program Files (x86)/MPlayer for Windows: /c/Program Files/ffmpeg/bin: /c/Program Files (x86)/mingw-w64/i686-8.1.0-posix-dwarf-rt_v6-rev0/mingw32/bin: /c/Program Files/CMake/bin: /c/Users/yyyy/go/src/gocv.io/x/gocv: /c/opencv/build/bin 以下の設定も必要かも $ pwd /c/msys64/etc file : profile の最終行に追加 PATH=$PATH:/c/Go/bin PATH=$PATH:/c/"Program Files (x86)"/"MPlayer for Windows" PATH=$PATH:/c/"Program Files"/ffmpeg/bin PATH=$PATH:/c/"Program Files (x86)"/mingw-w64/i686-8.1.0-posix-dwarf-rt_v6-rev0/mingw32/bin PATH=$PATH:/c/"Program Files"/CMake/bin PATH=$PATH:/c/Users/yyyy/go/src/gocv.io/x/gocv PATH=$PATH:/c/opencv/build/bin export PATH ----------------------------------------------------------------------------------------------- その他注意 $ pwd /c/users/yyyy/go/src/github.com yyyy@DESKTOP-15DPINR MINGW64 /c/users/yyyy/go/src/github.com $ ls -l 合計 0 drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 bmizerany drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 cpuguy83 drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:02 donovanhide drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:01 eclipse drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 go-ble drwxr-xr-x 1 yyyy yyyy 0 12月 11 14:59 gobuffalo drwxr-xr-x 1 yyyy yyyy 0 12月 11 14:59 gofrs drwxr-xr-x 1 yyyy yyyy 0 12月 12 15:46 go-gl drwxr-xr-x 1 yyyy yyyy 0 12月 11 14:48 golang drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:01 gorilla drwxr-xr-x 1 yyyy yyyy 0 12月 17 15:28 hajimehoshi <----- 要確認 drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 hashicorp drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:02 hybridgroup drwxr-xr-x 1 yyyy yyyy 0 12月 13 11:53 lxn drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:02 nats-io drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 pkg drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 sigurn drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:00 urfave drwxr-xr-x 1 yyyy yyyy 0 12月 11 15:01 veandco <------ 要確認 ----------------------------------------------------------------------------------------------- ----------------------------------------------------------------------------------------------- ----------------------------------------------------------------------------------------------- */ package main import ( "fmt" "time" "os" "os/exec" "github.com/veandco/go-sdl2/sdl" "gobot.io/x/gobot" "gobot.io/x/gobot/platforms/dji/tello" "sync/atomic" "math" "bufio" "strings" "strconv" "regexp" ) type pair struct { x float64 y float64 } var leftX, leftY, rightX, rightY atomic.Value var Mark = 0 const offset = 32767.0 // joystickの最大値(0-31767) var Slow = 2 var ctrl_point_RRL float64 = 1000 // ペジェ曲線の制御ポイント値 右スティックの左右 var ctrl_point_RUD float64 = 1000 // ペジェ曲線の制御ポイント値 右スティックの上下 var ctrl_point_LRL float64 = 1000 // ペジェ曲線の制御ポイント値 左スティックの左右 var ctrl_point_LUD float64 = 1000 // ペジェ曲線の制御ポイント値 左スティックの上下 const YYYYMMDDHHMISS1 = "20060102150405" // 固定値 const YYYYMMDDHHMISS2 = "06-1.23:4:5" var f_name_A string var f_name string = "tello_" var f_num int = 1 var f_mp4 string = ".mp4" // var f_mp4 string = ".mov" var ret int = 0 var end_count = 0 // ジョイティック2回押しで終了させる時のカウンター var takeoff_cnt = 0 // ジョイティック2回押しでtakeoff var land_count = 0 // ジョイティック2回押しでLAND var share_count = 0 // share_count2回押しでプログラム終了 var toff_tm[2]string // takeoffで使用 var toff_int[2]int // takeoffで使用 var land_tm[2]string // LANDで使用 var land_int[2]int // LANDで使用 var fin_tm[2]string // プログラム終了で使用 var fin_int[2]int // プログラム終了で使用 var joysticks [16]*sdl.Joystick // ############################################################################ func main(){ var event sdl.Event var running bool sdl.Init(sdl.INIT_JOYSTICK) defer sdl.Quit() sdl.JoystickEventState(sdl.ENABLE) drone := tello.NewDriver("8888") cmd := exec.Command("./rokuga_img.exe") j_cmd := exec.Command("./img_out.exe") j_cmd.Start() // joystick画像の表示 // ---------------------------------------------------------------------------- work := func() { leftX.Store(float64(0.0)) leftY.Store(float64(0.0)) rightX.Store(float64(0.0)) rightY.Store(float64(0.0)) drone.On(tello.ConnectedEvent, func(data interface{}) { fmt.Println("Connected") drone.StartVideo() drone.SetVideoEncoderRate(4) gobot.Every(100*time.Millisecond, func() { drone.StartVideo() }) }) mplayer := exec.Command("mplayer", "-fps", "30", "-") mplayerIn, _ := mplayer.StdinPipe() if err := mplayer.Start(); err != nil { fmt.Println("mplayer-St ",err) return } drone.On(tello.VideoFrameEvent, func(data interface{}) { pkt := data.([]byte) if _, err := mplayerIn.Write(pkt); err != nil { fmt.Println("mplayer-Wr ",err) } }) // ffmpeg f_name_A = f_name + strconv.Itoa(f_num) + f_mp4 fmt.Printf("Slow=%d\n\n",Slow) fmt.Printf("録画表示が出ない時は[options]スイッチオン\n") fmt.Printf("録画のON/OFFは[options]スイッチO N/OFF\n") ret = f_chk(f_name_A) if ret == 1 { // ファイル名が存在した var time_stmp string get_timestamp(&time_stmp) // 引数のstringポインタにセット f_name_A = f_name + time_stmp + f_mp4 fmt.Println("f_name_A=",f_name_A) } c := []string {"ffmpeg", "-i", "-", "-acodec", "copy", "-vcodec", "copy", f_name_A} // スライスの定義 // -i(インプットファイル指定)の"-" は pipe:0(標準入力) と同じ(??) ffmpeg := exec.Command(c[0], c[1:]...) ffmpegIn, _ := ffmpeg.StdinPipe() // if err := ffmpeg.Start(); err != nil { fmt.Println("ffmpeg-St エラー ",err) os.Exit(1) } drone.On(tello.VideoFrameEvent, func(data interface{}) { if Mark == 1{ // fmt.Println("121--------------options_press -----録画開始",) pkt := data.([]byte) if _, err := ffmpegIn.Write(pkt); err != nil { fmt.Println("ffmpeg-Wr File Name OK? ",err) os.Exit(1) } } }) // ---------------------------------------------------------------------------- running = true for running { // ---------------------------------------------------------------------------- for event = sdl.PollEvent(); event != nil; event = sdl.PollEvent() { if takeoff_cnt > 0{ // TakeOff ボタンが押されている get_timestamp(&toff_tm[1]) // 引数のstringポインタにセット toff_int[0], _ = strconv.Atoi(toff_tm[0]) toff_int[1], _ = strconv.Atoi(toff_tm[1]) if (toff_int[1] - toff_int[0]) > 3 { takeoff_cnt = 0 } } if land_count > 0{ // ボタンが押されている get_timestamp(&land_tm[1]) // 引数のstringポインタにセット land_int[0], _ = strconv.Atoi(land_tm[0]) land_int[1], _ = strconv.Atoi(land_tm[1]) if (land_int[1] - land_int[0]) > 3 { land_count = 0 } } if share_count > 0{ // ボタンが押されている get_timestamp(&fin_tm[1]) // 引数のstringポインタにセット fin_int[0], _ = strconv.Atoi(fin_tm[0]) fin_int[1], _ = strconv.Atoi(fin_tm[1]) if (fin_int[1] - fin_int[0]) > 3 { share_count = 0 } } // イベント switch t := event.(type) { case *sdl.QuitEvent: running = false case *sdl.JoyDeviceAddedEvent: joysticks[int(t.Which)] = sdl.JoystickOpen(int(t.Which)) if joysticks[int(t.Which)] != nil { fmt.Printf("Joystick %d connected\n", t.Which) if int(t.Which) > 0 { fmt.Printf("Connect!!!--------------- %d \n",int(t.Which)) } } case *sdl.JoyDeviceRemovedEvent: if joystick := joysticks[int(t.Which)]; joystick != nil { joystick.Close() } fmt.Printf("Joystick %d disconnected\n", t.Which) case *sdl.JoyAxisEvent: // ジョイスチック ############################################################## switch { // **** right Stick 上下 **** case t.Axis == 5: switch { case t.Value < -10: fmt.Printf("Up %d\n",t.Value) drone.Up(validatePitch_SP( float64(t.Value) , offset , ctrl_point_RUD)) case t.Value > 10: fmt.Printf("Down %d\n",t.Value) drone.Down(validatePitch_SP(float64(t.Value), offset, ctrl_point_RUD)) default: drone.Up(0) } // **** right Stick 左右 **** case t.Axis == 2: switch { case t.Value < -10: fmt.Printf("LEFT %d\n",t.Value) drone.Left(validatePitch_SP(float64(t.Value), offset,ctrl_point_RRL) / Slow) case t.Value > 10: fmt.Printf("Right %d\n",t.Value) drone.Right(validatePitch_SP(float64(t.Value), offset,ctrl_point_RRL) / Slow) default: drone.Right(0) } // **** Left Stick 上下 **** case t.Axis == 1: switch { case t.Value < -10: fmt.Printf("Forward %d\n",t.Value) drone.Forward(validatePitch_SP( float64(t.Value) , offset,ctrl_point_LUD) / Slow) case t.Value > 10: fmt.Printf("Backward %d\n",t.Value) drone.Backward(validatePitch_SP(float64(t.Value), offset,ctrl_point_LUD) / Slow) default: drone.Forward(0) } // **** Left Stick 回転 **** case t.Axis == 0: switch { case t.Value > 20: fmt.Printf("Clockwise %d\n",t.Value) drone.Clockwise(validatePitch_SP(float64(t.Value), offset,ctrl_point_LRL) / Slow) case t.Value < 20: fmt.Printf("CounterClockwise %d\n",t.Value) drone.CounterClockwise(validatePitch_SP(float64(t.Value), offset,ctrl_point_LRL) / Slow) default: drone.Clockwise(0) } } // *** ball ************** case *sdl.JoyBallEvent: fmt.Printf("[%d ms] Ball:%d\txrel:%d\tyrel:%d\n",t.Timestamp, t.Ball, t.XRel, t.YRel) case *sdl.JoyButtonEvent: // *** ボタン ************ fmt.Printf("[%d ms] Button:%d\tstate:%d\n",t.Timestamp, t.Button, t.State) switch { // TakeOff *** 右ボタン上 **** case t.Button == 3: if t.State == 0{ if takeoff_cnt < 1 { get_timestamp(&toff_tm[0]) // 引数のstringポインタにセット takeoff_cnt = 1 }else { get_timestamp(&toff_tm[1]) // 引数のstringポインタにセット takeoff_cnt = 0 toff_int[0], _ = strconv.Atoi(toff_tm[0]) toff_int[1], _ = strconv.Atoi(toff_tm[1]) if (toff_int[1] - toff_int[0]) < 1 { fmt.Printf("--TakeOff--\n") drone.TakeOff() } } } // LAND *** 右ボタン下 **** case t.Button == 1: if t.State == 0{ if land_count < 1 { get_timestamp(&land_tm[0]) // 引数のstringポインタにセット land_count = 1 }else { get_timestamp(&land_tm[1]) // 引数のstringポインタにセット land_count = 0 land_int[0], _ = strconv.Atoi(land_tm[0]) land_int[1], _ = strconv.Atoi(land_tm[1]) if (land_int[1] - land_int[0]) < 1 { fmt.Printf("--LAND--\n") drone.Land() } } } // program STOP!! **** 左SHARE ボタン **** case t.Button == 8: if t.State == 0{ if share_count < 1 { get_timestamp(&fin_tm[0]) // 引数のstringポインタにセット share_count = 1 }else { get_timestamp(&fin_tm[1]) // 引数のstringポインタにセット share_count = 0 fin_int[0], _ = strconv.Atoi(fin_tm[0]) fin_int[1], _ = strconv.Atoi(fin_tm[1]) if (fin_int[1] - fin_int[0]) < 1 { // *** 録画ストップ ******** if Mark == 1 { fmt.Println("options_press -----録画STOP ") Mark = 0; cmd.Process.Kill() cmd.Wait() } // program end ************** fmt.Printf("END--------------\n") os.Exit(1) } } } // 録画**** 右Optionsボタン*** case t.Button == 9: if t.State == 0{ if Mark == 0 { Mark = 1 cmd.Start() fmt.Printf("\n実行中: pid=%d Mark=%d\n", cmd.Process.Pid,Mark) time.Sleep(1 * time.Second) }else { fmt.Println("options_press -----録画STOP ") Mark = 0; cmd.Process.Kill() cmd.Wait() fmt.Printf("Kill後:%d Mark=%d ", cmd.Process.Pid,Mark) } fmt.Println("ON-OFFで録画開始\n") } } /* case *sdl.JoyHatEvent: // ###ボタン ######################################################################################### fmt.Printf("[%d ms] Hat:%d\tvalue:%d\n", t.Timestamp, t.Hat, t.Value) if t.Value == 0 { fmt.Printf( " Value=(%d) exit \n",t.Value) os.Exit(1) } else{ fmt.Printf( " Value=.(%d)\n",t.Value) } */ default: fmt.Printf("Unknown event\n") } } sdl.Delay(8) // sdl.Delay(16) } } robot := gobot.NewRobot("tello", []gobot.Connection{}, []gobot.Device{drone}, work, ) robot.Start() } func getLeftStick() pair { s := pair{x: 0, y: 0} s.x = leftX.Load().(float64) s.y = leftY.Load().(float64) return s } // ========================================================= func getRightStick() pair { s := pair{x: 0, y: 0} s.x = rightX.Load().(float64) s.y = rightY.Load().(float64) return s } // ========================================================= // input 1-line func StrStdin() (stringInput string) { scanner := bufio.NewScanner(os.Stdin) scanner.Scan() stringInput = scanner.Text() stringInput = strings.TrimSpace(stringInput) return } // ====== ファイル名チェック================================ func f_chk(f_name string)(int) { // 戻り値はint f, _ := os.Open(f_name) defer f.Close() if fi, err := f.Stat(); err == nil { fmt.Printf("ファイル名チェック中: %s\n", fi.Name()) // fmt.Printf("ファイルサイズ(byte): %d\n", fi.Size()) // fmt.Printf("モード: %s\n", fi.Mode()) // fmt.Printf("ディレクトリ? :%t\n", fi.IsDir()) return 1 }else { return 0 } } // ====== タイムスタンプの作成============================ func get_timestamp(stmp *string ) { // t := time.Now() const YYYYMMDDHHMISS1 = "20060102150405" time_stmp := fmt.Sprintln(t.Format(YYYYMMDDHHMISS1)) // fmt.Printf("[85 : time_stmp=%s]",time_stmp) rep:= regexp.MustCompile(`\n`) time_stmp = rep.ReplaceAllString(time_stmp,"") // fmt.Printf("[88 : time_stmp=%s]\n",time_stmp) *stmp = time_stmp } // ====== ペジェ直線の作成============================ // --------------------------------------------------------------------- // fx2:x方向の位置 offset(終点) fy2:y方向点 // スピードアップのため float64=>int64 に変換している // 同2乗を関数math.Powでなく、積にて実施 // --------------------------------------------------------------------- func validatePitch_SP(fx2 float64, offset float64,fy2 float64) int { var y3 int64 = int64(offset) var y2 int64 = int64(fy2) var tt int64 = int64(math.Abs(fx2) / offset * 100) var t00 int64= 100 - tt // fmt.Printf("tt=%d---",tt) if tt >= 1 { if tt <= 100 { // yyy := math.Pow((1 - tt) , 2.0) + (2 * (1 - tt) * tt * y2) + (math.Pow(tt , 2.0) * y3) // yyy := (((100 - tt) * (100 - tt)) + (2 * (100 - tt) * tt * y2) + ((tt * tt) * y3)) / 100 yyy := ((t00 * t00) + (2 * t00 * tt * y2) + ((tt * tt) * y3)) / 100 zzz := int(int64(yyy / y3)) // 201908 fmt.Printf("tt=%d zzz=%d \n",tt,zzz) return zzz } return 100 } return 0 } // ----------------------------------------------------------------------------------- // EOF //---------------------------------------------------------- // --------------------------------------------------------