① ジョイスティックから帰ってくる値を確認する
② ジョイスティックのスティック動作を直線からペジェ曲線に変える -------------------------------------------------------------------------------- ① ジョイスティックから帰ってくる値を確認する -------------------------------------------------------------------------------- joystick_ps4.goを実行してジョイスティックから帰ってくる値をプログラムにセットする 例えば、右スティックの左右はR2,右スティックの上下はL2、 プロブラム的には // RightX stick.On(joystick.L2, func(data interface{}) { fmt.Println("L2", data) val := float64(data.(int16)) rightX.Store(val) }) ====joystick_ps4.go 実行サンプル====== $ go run joystick_ps4.go 2019/05/15 17:34:03 Initializing connections... 2019/05/15 17:34:03 Initializing connection Joystick-3F7573B7AA13B66D ... 2019/05/15 17:34:03 Initializing devices... 2019/05/15 17:34:03 Initializing device Joystick-697DF95315C0C326 ... 2019/05/15 17:34:03 Robot joystickBot initialized. 2019/05/15 17:34:03 Starting Robot joystickBot ... 2019/05/15 17:34:03 Starting connections... 2019/05/15 17:34:03 Starting connection Joystick-3F7573B7AA13B66D... 2019/05/15 17:34:03 Starting devices... 2019/05/15 17:34:03 Starting device Joystick-697DF95315C0C326... 2019/05/15 17:34:03 Starting work... R2 -768 R2 -1536 L2 261 L2 3382 R2 261 R2 1821 L2 -24064 L2 -26880 left_y -512 left_y -2816 left_x 5202 left_x 15344 options_press options_press options_release options_release share_press share_press share_release share_release square_press square_release square_release triangle_press triangle_press triangle_release triangle_release circle_press circle_press circle_release circle_release x_press x_press x_release x_release up_press up_press up_release up_release right_press right_press right_release right_release down_press down_press down_release down_release left_press left_press left_release left_releas ====joystick_ps4.go プログラム====== /* ************** joy-stick info ************* for ps4 joystick Left bar up/down :left_y left/right :left_x Right bar up/down :R2 left/right :L2 right bottan up-posi :square_press/square_release right-pos :triangle_press/triangle_release down-pos :circle_press/circle_release let-pos :x_press/x__release right up-bottan :options_press/options_release left up-bottan :shere_press/shere_release left bottan up-posi :up_press/up__release right-pos :right_press/right_release down-pos :down_press/down_release let-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" "sync/atomic" "time" "bufio" "strings" ) type pair struct { x float64 y float64 } var leftX, leftY, rightX, rightY atomic.Value const offset = 32767.0 // ========================================================= func main() { drone := tello.NewDriver("8890") joystickAdaptor := joystick.NewAdaptor() stick := joystick.NewDriver(joystickAdaptor, joystick.Dualshock4) work := func() { leftX.Store(float64(0.0)) leftY.Store(float64(0.0)) rightX.Store(float64(0.0)) rightY.Store(float64(0.0)) mplayer := exec.Command("mplayer", "-fps", "35", "-") mplayerIn, _ := mplayer.StdinPipe() if err := mplayer.Start(); err != nil { fmt.Println(err) return } drone.On(tello.ConnectedEvent, func(data interface{}) { fmt.Println("Connected") drone.StartVideo() drone.SetVideoEncoderRate(4) gobot.Every(100*time.Millisecond, func() { drone.StartVideo() }) }) drone.On(tello.VideoFrameEvent, func(data interface{}) { pkt := data.([]byte) if _, err := mplayerIn.Write(pkt); err != nil { fmt.Println(err) } }) // 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("END? Y or N") p := StrStdin() fmt.Println(p) if p == "Y" || p == "y" { fmt.Println("ENDING") os.Exit(1) } else{ fmt.Println("not ENDING") // os.Exit(1) } }) // rightStick gobot.Every(10*time.Millisecond, func() { rightStick := getRightStick() switch { case rightStick.y < -10: drone.Up(tello.ValidatePitch(rightStick.y, offset)) case rightStick.y > 10: drone.Down(tello.ValidatePitch(rightStick.y, offset)) default: drone.Up(0) } switch { case rightStick.x > 10: drone.Right(tello.ValidatePitch(rightStick.x, offset)) case rightStick.x < -10: drone.Left(tello.ValidatePitch(rightStick.x, offset)) 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(tello.ValidatePitch(leftStick.y, offset)) case leftStick.y > 10: drone.Backward(tello.ValidatePitch(leftStick.y, offset)) default: drone.Forward(0) } switch { case leftStick.x > 20: drone.Clockwise(tello.ValidatePitch(leftStick.x, offset)) case leftStick.x < -20: drone.CounterClockwise(tello.ValidatePitch(leftStick.x, offset)) default: drone.Clockwise(0) } }) } // robot := gobot.NewRobot("joystickBot", // []gobot.Connection{joystickAdaptor}, // []gobot.Device{stick}, // work, // ) robot := gobot.NewRobot("tello", []gobot.Connection{joystickAdaptor}, []gobot.Device{stick, 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 } -------------------------------------------------------------------------------- ② ジョイスティックのスティック動作を直線からペジェ曲線に変える -------------------------------------------------------------------------------- tello のサンプルプログラム（WEBより）では drone.Up(tello.ValidatePitch(rightStick.y, offset)) となっており、tello.ValidatePitchを調べると単純直線の値を返していた。 if value >= 0.1 { if value <= 1.0 { return int((float64(int(value*100)) / 100) * 100) } return 100 } return 0
// yyy := math.Pow((1 - tt) , 2.0) + (2 * (1 - tt) * tt * y2) + (math.Pow(tt , 2.0) * y3) スピードアップの為、フロートをint64に変換して演算 var y3 int64 = int64(offset) var y2 int64 = int64(fy2) var tt int64 = int64(math.Abs(fx2) / offset * 100) var t00 int64= 100 - tt yyy := ((t00 * t00) + (2 * t00 * tt * y2) + ((tt * tt) * y3)) / 100 zzz := int(int64(yyy / y3)) 独自関数を以下のように作成した // ====== ペジェ直線の作成============================ // joystickのスティック動作を直線からペジェ直線に変えるfunction // oオリジナルは ValidatePitch(leftStick.x, offset) // yy = validatePitch_SP(x2,y3 ,y2 ) // // x=0 ～ x=32767 y=0 ～ y=32767 の範囲で右（ｘ）を0-32767に上っていく値を // ｙ方向点(y2)を変化させると曲線になる // y2が32767/2 で直線になる。それ以下で下側のしずむ // math.Powは2乗関数 // --------------------------------------------------------------------- var x float64 var y float64 var x1 float64 = 1 // 始点 x var x2 float64 = 16000 // 方向点 x var x3 float64 = 32767 // 終点 x var y1 float64 = 1 // 始点 y var y2 float64 = 16000 // 方向点 y var y3 float64 = 32767 // 終点 y var t float64 = 0.5 const offset = 32767.0 // joystickの最大値(0-31767) var ctrl_point_RRL float64 = 8000 // ペジェ曲線の制御ポイント値 右スティックの左右 var ctrl_point_RUD float64 = 8000 // ペジェ曲線の制御ポイント値 右スティックの上下 var ctrl_point_LRL float64 = 8000 // ペジェ曲線の制御ポイント値 左スティックの左右 var ctrl_point_LUD float64 = 8000 // ペジェ曲線の制御ポイント値 左スティックの上下 // --------------------------------------------------------------------- // fx2:x方向の位置 offset(終点） fy2:y方向点 // スピードアップのため float64=>int64 に変換している // 同２乗を関数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 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 := ((t00 * t00) + (2 * t00 * tt * y2) + ((tt * tt) * y3)) / 100 zzz := int(int64(yyy / y3)) return zzz } return 100 } return 0 } // ------呼び出しは以下--------------------------------------------------------------- // drone.CounterClockwise(tello.ValidatePitch(leftStick.x, offset)) // 変更前 drone.CounterClockwise(validatePitch_SP(leftStick.x, offset,ctrl_point_LRL))// 変更後