[ble] Control stick flight control

Signed-off-by: deadprogram <ron@hybridgroup.com>

examples/minidrone_ps3.go

@@ -0,0 +1,141 @@
+package main
+
+import (
+	"math"
+	"time"
+	"os"
+	
+	"github.com/hybridgroup/gobot"
+	"github.com/hybridgroup/gobot/platforms/ble"
+	"github.com/hybridgroup/gobot/platforms/joystick"
+)
+
+type pair struct {
+	x float64
+	y float64
+}
+
+func main() {
+	gbot := gobot.NewGobot()
+
+	joystickAdaptor := joystick.NewJoystickAdaptor("ps3")
+	joystick := joystick.NewJoystickDriver(joystickAdaptor,
+		"ps3",
+		"./platforms/joystick/configs/dualshock3.json",
+	)
+
+	droneAdaptor := ble.NewBLEAdaptor("ble", os.Args[1])
+	drone := ble.NewBLEMinidroneDriver(droneAdaptor, "drone")
+
+	work := func() {
+
+		offset := 32767.0
+		rightStick := pair{x: 0, y: 0}
+		leftStick := pair{x: 0, y: 0}
+
+		recording := false
+
+		gobot.On(joystick.Event("circle_press"), func(data interface{}) {
+			if recording {
+				drone.StopRecording()
+			} else {
+				drone.StartRecording()
+			}
+			recording = !recording
+		})
+
+		gobot.On(joystick.Event("square_press"), func(data interface{}) {
+			drone.HullProtection(true)
+			drone.TakeOff()
+		})
+		gobot.On(joystick.Event("triangle_press"), func(data interface{}) {
+			drone.Stop()
+		})
+		gobot.On(joystick.Event("x_press"), func(data interface{}) {
+			drone.Land()
+		})
+		gobot.On(joystick.Event("left_x"), func(data interface{}) {
+			val := float64(data.(int16))
+			if leftStick.x != val {
+				leftStick.x = val
+			}
+		})
+		gobot.On(joystick.Event("left_y"), func(data interface{}) {
+			val := float64(data.(int16))
+			if leftStick.y != val {
+				leftStick.y = val
+			}
+		})
+		gobot.On(joystick.Event("right_x"), func(data interface{}) {
+			val := float64(data.(int16))
+			if rightStick.x != val {
+				rightStick.x = val
+			}
+		})
+		gobot.On(joystick.Event("right_y"), func(data interface{}) {
+			val := float64(data.(int16))
+			if rightStick.y != val {
+				rightStick.y = val
+			}
+		})
+
+		gobot.Every(10*time.Millisecond, func() {
+			pair := leftStick
+			if pair.y < -10 {
+				drone.Forward(validatePitch(pair.y, offset))
+			} else if pair.y > 10 {
+				drone.Backward(validatePitch(pair.y, offset))
+			} else {
+				drone.Forward(0)
+			}
+
+			if pair.x > 10 {
+				drone.Right(validatePitch(pair.x, offset))
+			} else if pair.x < -10 {
+				drone.Left(validatePitch(pair.x, offset))
+			} else {
+				drone.Right(0)
+			}
+		})
+
+		gobot.Every(10*time.Millisecond, func() {
+			pair := rightStick
+			if pair.y < -10 {
+				drone.Up(validatePitch(pair.y, offset))
+			} else if pair.y > 10 {
+				drone.Down(validatePitch(pair.y, offset))
+			} else {
+				drone.Up(0)
+			}
+
+			if pair.x > 20 {
+				drone.Clockwise(validatePitch(pair.x, offset))
+			} else if pair.x < -20 {
+				drone.CounterClockwise(validatePitch(pair.x, offset))
+			} else {
+				drone.Clockwise(0)
+			}
+		})
+	}
+
+	robot := gobot.NewRobot("minidrone",
+		[]gobot.Connection{joystickAdaptor, droneAdaptor},
+		[]gobot.Device{joystick, drone},
+		work,
+	)
+
+	gbot.AddRobot(robot)
+
+	gbot.Start()
+}
+
+func validatePitch(data float64, offset float64) int {
+	value := math.Abs(data) / offset
+	if value >= 0.1 {
+		if value <= 1.0 {
+			return int((float64(int(value*100)) / 100) * 100)
+		}
+		return 100
+	}
+	return 0
+}