ardrone_ps3_channels.go

package main

import (
	"math"
	"time"

	"gobot.io/x/gobot"
	"gobot.io/x/gobot/platforms/joystick"
	"gobot.io/x/gobot/platforms/parrot/ardrone"
)

type pair struct {
	x float64
	y float64
}

var rightStick, leftStick pair
var leftX, leftY, rightX, rightY chan float64

func main() {
	joystickAdaptor := joystick.NewAdaptor()
	stick := joystick.NewDriver(joystickAdaptor,
		"./platforms/joystick/configs/dualshock3.json",
	)

	ardroneAdaptor := ardrone.NewAdaptor()
	drone := ardrone.NewDriver(ardroneAdaptor)

	rightStick = pair{x: 0, y: 0}
	leftStick = pair{x: 0, y: 0}

	leftX = make(chan float64)
	leftY = make(chan float64)
	rightX = make(chan float64)
	rightY = make(chan float64)

	work := func() {
		offset := 32767.0

		stick.On(joystick.SquarePress, func(data interface{}) {
			drone.TakeOff()
		})

		stick.On(joystick.TrianglePress, func(data interface{}) {
			drone.Hover()
		})

		stick.On(joystick.XPress, func(data interface{}) {
			drone.Land()
		})

		stick.On(joystick.LeftX, func(data interface{}) {
			val := float64(data.(int16))
			leftX <- val
		})

		stick.On(joystick.LeftY, func(data interface{}) {
			val := float64(data.(int16))
			leftY <- val
		})

		stick.On(joystick.RightX, func(data interface{}) {
			val := float64(data.(int16))
			rightX <- val
		})

		stick.On(joystick.RightY, func(data interface{}) {
			val := float64(data.(int16))
			rightY <- val
		})

		gobot.Every(10*time.Millisecond, func() {
			getLeftStick()

			switch {
			case leftStick.y < -10:
				drone.Forward(validatePitch(leftStick.y, offset))
			case leftStick.y > 10:
				drone.Backward(validatePitch(leftStick.y, offset))
			default:
				drone.Forward(0)
			}

			switch {
			case leftStick.x > 10:
				drone.Right(validatePitch(leftStick.x, offset))
			case leftStick.x < -10:
				drone.Left(validatePitch(leftStick.x, offset))
			default:
				drone.Right(0)
			}
		})

		gobot.Every(10*time.Millisecond, func() {
			getRightStick()

			switch {
			case rightStick.y < -10:
				drone.Up(validatePitch(rightStick.y, offset))
			case rightStick.y > 10:
				drone.Down(validatePitch(rightStick.y, offset))
			default:
				drone.Up(0)
			}

			switch {
			case rightStick.x > 20:
				drone.Clockwise(validatePitch(rightStick.x, offset))
			case rightStick.x < -20:
				drone.CounterClockwise(validatePitch(rightStick.x, offset))
			default:
				drone.Clockwise(0)
			}
		})
	}

	robot := gobot.NewRobot("ardrone",
		[]gobot.Connection{joystickAdaptor, ardroneAdaptor},
		[]gobot.Device{stick, drone},
		work,
	)

	robot.Start()
}

func validatePitch(data float64, offset float64) float64 {
	value := math.Abs(data) / offset
	if value >= 0.1 {
		if value <= 1.0 {
			return float64(int(value*100)) / 100
		}
		return 1.0
	}
	return 0.0
}

func getLeftStick() {
	select {
	case leftStick.x = <-leftX:
	default:
	}
	select {
	case leftStick.y = <-leftY:
	default:
	}
}

func getRightStick() {
	select {
	case rightStick.x = <-rightX:
	default:
	}
	select {
	case rightStick.y = <-rightY:
	default:
	}
}