ps3.go

/*
How to setup
You must be using a PS3 or compatible controller, along with a DJI Tello drone to run this example.

You run the Go program on your computer and communicate wirelessly via WiFi with the Tello.

How to run

        go run examples/tello_ps3.go
*/

package main

import (
        "log"
        "sync/atomic"
        "time"

        "gobot.io/x/gobot"
        "gobot.io/x/gobot/platforms/dji/tello"
        "gobot.io/x/gobot/platforms/joystick"
)

type pair struct {
        x float64
        y float64
}

var leftX, leftY, rightX, rightY atomic.Value

const offset = 32767.0

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
}

func main() {
        fastModeActivated := false
        hasTakenOff := false

        joystickAdaptor := joystick.NewAdaptor()
        stick := joystick.NewDriver(joystickAdaptor, "dualshock3")

        drone := tello.NewDriver("8888")

        increaseChannel := make(chan int)
        increaseStopChannel := make(chan int, 1)

        decreaseChannel := make(chan int)
        decreaseStopChannel := make(chan int, 1)

        rotateClockwiseChannel := make(chan int)
        rotateClockwiseStopChannel := make(chan int, 1)

        rotateCounterClockwiseChannel := make(chan int)
        rotateCounterClockwiseStopChannel := make(chan int, 1)

        go func() {
                for {
                        select {
                        case <-increaseChannel:
                                log.Println("Increasing height")
                                for len(increaseStopChannel) <= 0 {
                                        drone.Up(30)
                                }
                        case <-increaseStopChannel:
                                log.Println("Stop increasing")
                        }
                }
        }()

        go func() {
                for {
                        select {
                        case <-decreaseChannel:
                                log.Println("Decreasing height")
                                for len(decreaseStopChannel) <= 0 {
                                        drone.Down(30)
                                }
                        case <-decreaseStopChannel:
                                log.Println("Stop decreasing")
                        }
                }

        }()

        go func() {
                for {
                        select {
                        case <-rotateClockwiseChannel:
                                log.Println("Increasing Clockwise rotation")
                                for len(rotateClockwiseStopChannel) <= 0 {
                                        drone.Clockwise(30)
                                }
                        case <-rotateClockwiseStopChannel:
                                log.Println("Stop Clockwise rotation")
                        }
                }
        }()

        go func() {
                for {
                        select {
                        case <-rotateCounterClockwiseChannel:
                                log.Println("Increasing CounterClockwise rotation")
                                for len(rotateCounterClockwiseStopChannel) <= 0 {
                                        drone.CounterClockwise(30)
                                }
                        case <-rotateCounterClockwiseStopChannel:
                                log.Println("Stop CounterClockwise rotation")
                        }
                }
        }()

        work := func() {
                leftX.Store(float64(0.0))
                leftY.Store(float64(0.0))
                rightX.Store(float64(0.0))
                rightY.Store(float64(0.0))

                stick.On(joystick.TrianglePress, func(data interface{}) {
                        if hasTakenOff {
                                log.Println("Landing")
                                drone.Land()
                                hasTakenOff = false
                        } else {
                                log.Println("Taking off")
                                drone.TakeOff()
                                hasTakenOff = true
                        }
                })

                stick.On(joystick.CirclePress, func(data interface{}) {
                        log.Println("Aborting landing")
                        hasTakenOff = true
                        drone.StopLanding()
                })

                stick.On(joystick.UpPress, func(data interface{}) {
                        log.Println("Doing FrontFlip")
                        drone.FrontFlip()
                })

                stick.On(joystick.DownPress, func(data interface{}) {
                        log.Println("Doing BackFlip")
                        drone.BackFlip()
                })

                stick.On(joystick.RightPress, func(data interface{}) {
                        log.Println("Doing RightFlip")
                        drone.RightFlip()
                })

                stick.On(joystick.LeftPress, func(data interface{}) {
                        log.Println("Doing LeftFlip")
                        drone.LeftFlip()
                })

                stick.On(joystick.SelectPress, func(data interface{}) {
                        if fastModeActivated {
                                log.Println("Activating Slow Mode")
                                drone.SetSlowMode()
                                fastModeActivated = false
                        } else {
                                log.Println("Activating Fast Mode")
                                drone.SetFastMode()
                                fastModeActivated = true
                        }
                })

                stick.On(joystick.SquarePress, func(data interface{}) {
                        log.Println("Activating Throw&Go")
                        drone.ThrowTakeOff()
                        hasTakenOff = true
                })

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

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

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

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

                // stick.On(joystick.R2Press, func(data interface{}) {
                //         log.Println("R2Press")
                //         increaseChannel <- 1

                // })
                // stick.On(joystick.R2Release, func(data interface{}) {
                //         log.Println("R2Release")
                //         increaseStopChannel <- 1
                // })

                // stick.On(joystick.L2Press, func(data interface{}) {
                //         log.Println("L2Press")
                //         decreaseChannel <- 1
                // })
                // stick.On(joystick.L2Release, func(data interface{}) {
                //         log.Println("L2Release")
                //         decreaseStopChannel <- 1
                // })

                // stick.On(joystick.R1Press, func(data interface{}) {
                //         log.Println("R1Press")
                //         rotateClockwiseChannel <- 1
                // })
                // stick.On(joystick.R1Release, func(data interface{}) {
                //         log.Println("R1Release")
                //         rotateClockwiseStopChannel <- 1
                // })

                // stick.On(joystick.L1Press, func(data interface{}) {
                //         log.Println("L1Press")
                //         rotateCounterClockwiseChannel <- 1
                // })
                // stick.On(joystick.L1Release, func(data interface{}) {
                //         log.Println("L1Release")
                //         rotateCounterClockwiseStopChannel <- 1
                // })

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

                        switch {
                        case rightStick.y < -10:
                                drone.Forward(tello.ValidatePitch(rightStick.y, offset))
                        case rightStick.y > 10:
                                drone.Backward(tello.ValidatePitch(rightStick.y, offset))
                        default:
                                drone.Forward(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)
                        }
                })

                gobot.Every(10*time.Millisecond, func() {
                        leftStick := getLeftStick()
                        switch {
                        case leftStick.y < -10:
                                drone.Up(tello.ValidatePitch(leftStick.y, offset))
                        case leftStick.y > 10:
                                drone.Down(tello.ValidatePitch(leftStick.y, offset))
                        default:
                                drone.Up(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("tello",
                []gobot.Connection{joystickAdaptor},
                []gobot.Device{stick, drone},
                work,
        )

        robot.Start()
}