②telloを飛ばそう・ソース(映像表示、録画、ジョイスティック)
==================================================================
①操作方法
モード1です
録画開始を押すと以下の画像が現れる
==================================================================
②telloを飛ばそう・ソース
//---------------------------------------------------------- /* MEMO 2019/6/5 ファイル名:tello_001.go(tello_001.exe) ファイルエンコード:UTF-8 go言語 gobot DJI tello pkg(-d -u gobot.io/x/gobot) MSYS2 MinGW-w64 (64bit) joystick 操作 mplayer 表示用 ffmpeg 録画用 hajimehoshi/ebiten exec.Command("./rokuga_img.exe")の別ファンクションでの画像出力 gocv どこで使ったのは忘れました(画像表示でいろいろトライしたので) Cmake どこで使ったのは忘れました */ /* ************** joy-stick 情報(一部)************* for ps4 joystick Mode 1で設定 Left bar up/down :left_y left/right :left_x Right bar up/down :R2 left/right :L2 right bottan up-pos :square_press/square_release right-pos :triangle_press/triangle_release down-pos :circle_press/circle_release left-pos :x_press/x__release right up-bottan :Ooptions_Press/options_release left up-bottan :Shere_Press/shere_release left bottan up-pos :up_press/up__release right-pos :right_press/right_release down-pos :down_press/down_release left-pos :left_press/left_release left under L1 bottan :l1_press/l1__release left under L2 bottan :right_x/l2__release right under R1 bottan :r1_press/r1_release right under R2 bottan :right_y/r2__release ************************************************************* */ package main import ( "fmt" "gobot.io/x/gobot" "gobot.io/x/gobot/platforms/dji/tello" "gobot.io/x/gobot/platforms/joystick" "os" "os/exec" // 他exe(録画マーク表示)を呼ぶ "sync/atomic" "time" "bufio" "strings" "strconv" "regexp" "math" // for joystick ) type pair struct { x float64 y float64 } var leftX, leftY, rightX, rightY atomic.Value var Mark = 0 // 録画 on/off const offset = 32767.0 // joystickの最大値(0-31767) var ctrl_point_RRL float64 = 2000 // ペジェ曲線の制御ポイント値 右スティックの左右 var ctrl_point_RUD float64 = 2000 // ペジェ曲線の制御ポイント値 右スティックの上下 var ctrl_point_LRL float64 = 2000 // ペジェ曲線の制御ポイント値 左スティックの左右 var ctrl_point_LUD float64 = 2000 // ペジェ曲線の制御ポイント値 左スティックの上下 /* var ctrl_point_RRL float64 = 16350 // ペジェ曲線の制御ポイント値 右スティックの左右 var ctrl_point_RUD float64 = 16350 // ペジェ曲線の制御ポイント値 右スティックの上下 var ctrl_point_LRL float64 = 16350 // ペジェ曲線の制御ポイント値 左スティックの左右 var ctrl_point_LUD float64 = 16350 // ペジェ曲線の制御ポイント値 左スティックの上下 */ // ファイル名作成で使用 const YYYYMMDDHHMISS1 = "20060102150405" // 固定値 const YYYYMMDDHHMISS2 = "06-1.23:4:5" var ret int = 0 var f_name string = "tello_" var f_name_A string var f_num int = 1 var f_mp4 string = ".mp4" // ファイル名作成で使用 // var f_mp4 string = ".mov" // ファイル名作成で使用 var end_count = 0 // ジョイティック2回押しで終了させる時のカウンター // ========================================================= func main() { drone := tello.NewDriver("8888") // wifi joystickAdaptor := joystick.NewAdaptor() stick := joystick.NewDriver(joystickAdaptor, joystick.Dualshock4) cmd := exec.Command("./rokuga_img.exe") 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----------------- mplayer := exec.Command("mplayer", "-fps", "30", "-") mplayerIn, _ := mplayer.StdinPipe() if err := mplayer.Start(); err != nil { fmt.Println("mplayer-St ",err) return } //---drone.On---------------- 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 ret = f_chk(f_name_A) if ret == 1 { // ファイル名が存在した var sss1 string get_timestamp(&sss1) // 引数のstringポインタにセット fmt.Println("sss1= ",sss1) f_name_A = f_name + sss1 + 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) // return } //-------------------- drone.On(tello.VideoFrameEvent, func(data interface{}) { if Mark == 1{ // joystickでセット // fmt.Println("------options_press -----録画開始",) pkt := data.([]byte) if _, err := ffmpegIn.Write(pkt); err != nil { fmt.Println("ffmpeg-Wr File Name OK? ",err) os.Exit(1) } } }) //---TakeOff----------------- stick.On(joystick.SquarePress, func(data interface{}) { // fmt.Println("square_press") drone.TakeOff() }) //---Land stick.On(joystick.CirclePress, func(data interface{}) { fmt.Println("Land") drone.Land() }) //---LeftX stick.On(joystick.LeftX, func(data interface{}) { // fmt.Println("left_x", data) val := float64(data.(int16)) leftX.Store(val) }) //---LeftY stick.On(joystick.LeftY, func(data interface{}) { // fmt.Println("left_y", data) val := float64(data.(int16)) leftY.Store(val) }) //---RightX stick.On(joystick.L2, func(data interface{}) { // fmt.Println("L2", data) val := float64(data.(int16)) rightX.Store(val) }) //---RightY stick.On(joystick.R2, func(data interface{}) { // fmt.Println("R2", data) val := float64(data.(int16)) rightY.Store(val) }) //---UPpress stick.On(joystick.UpPress, func(data interface{}) { fmt.Println("2回プッシュで終了") if end_count == 0{ end_count = 1 return } // fmt.Println("END? Y or N") // p := StrStdin() // fmt.Println(p) // if p == "Y" || p == "y" { // } fmt.Println("ENDING") os.Exit(1) }) //---share_press stick.On(joystick.SharePress, func(data interface{}) { fmt.Println("SharePress",Mark,"F_NUM=",f_num) }) //---options_press stick.On(joystick.OptionsPress, func(data interface{}) { fmt.Println("options_press") if Mark == 0 { Mark = 1 cmd = exec.Command("./rokuga_img.exe") 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) } }) //---rightStick gobot.Every(10*time.Millisecond, func() { rightStick := getRightStick() switch { case rightStick.y < -10: drone.Up(validatePitch_SP(rightStick.y, offset , ctrl_point_RUD)) case rightStick.y > 10: drone.Down(validatePitch_SP(rightStick.y, offset, ctrl_point_RUD)) default: drone.Up(0) } switch { case rightStick.x > 10: drone.Right(validatePitch_SP(rightStick.x, offset,ctrl_point_RRL)) case rightStick.x < -10: drone.Left(validatePitch_SP(rightStick.x, offset,ctrl_point_RRL)) default: drone.Right(0) } }) //---leftStick gobot.Every(10*time.Millisecond, func() { leftStick := getLeftStick() // fmt.Printf("L.y = %d\n", leftStick.y) switch { case leftStick.y < -10: drone.Forward(validatePitch_SP(leftStick.y, offset,ctrl_point_LUD)) case leftStick.y > 10: drone.Backward(validatePitch_SP(leftStick.y, offset,ctrl_point_LUD)) default: drone.Forward(0) } switch { case leftStick.x > 20: drone.Clockwise(validatePitch_SP(leftStick.x, offset,ctrl_point_LRL)) case leftStick.x < -20: drone.CounterClockwise(validatePitch_SP(leftStick.x, offset,ctrl_point_LRL)) default: drone.Clockwise(0) } }) } // work := func() のEND robot := gobot.NewRobot("tello", []gobot.Connection{joystickAdaptor}, []gobot.Device{stick, drone}, work, ) robot.Start() } // func main() のEND // ========================================================= 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 { // fmt.Printf("ファイルが存在しません \n") return 0 } } // === タイムスタンプの作成 =============================== func get_timestamp(stmp *string ) { // t := time.Now() const YYYYMMDDHHMISS1 = "20060102150405" sss1 := fmt.Sprintln(t.Format(YYYYMMDDHHMISS1)) fmt.Printf("[85 : sss1=%s]",sss1) rep:= regexp.MustCompile(`\n`) sss1 = rep.ReplaceAllString(sss1,"") fmt.Printf("[88 : sss1=%s]\n",sss1) *stmp = sss1 } // === ペジェ直線の作成 =================================== // joystick のスティック動作を直線からペジェ曲線に変えるfunction // オリジナルは ValidatePitch(leftStick.x, offset) // -------------------------------------------------------- // 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)) // fmt.Printf("tt=%d zzz=%d \n",tt,zzz) return zzz } return 100 } return 0 } // --------------------------------------------------------