gobot.io/x/gobot@v1.16.0/examples/ardrone_ps3.go (about)

     1  // +build example
     2  //
     3  // Do not build by default.
     4  
     5  package main
     6  
     7  import (
     8  	"sync/atomic"
     9  	"time"
    10  
    11  	"gobot.io/x/gobot"
    12  	"gobot.io/x/gobot/platforms/joystick"
    13  	"gobot.io/x/gobot/platforms/parrot/ardrone"
    14  )
    15  
    16  type pair struct {
    17  	x float64
    18  	y float64
    19  }
    20  
    21  var leftX, leftY, rightX, rightY atomic.Value
    22  
    23  const offset = 32767.0
    24  
    25  func main() {
    26  	joystickAdaptor := joystick.NewAdaptor()
    27  	stick := joystick.NewDriver(joystickAdaptor, "dualshock3")
    28  
    29  	ardroneAdaptor := ardrone.NewAdaptor()
    30  	drone := ardrone.NewDriver(ardroneAdaptor)
    31  
    32  	leftX.Store(float64(0.0))
    33  	leftY.Store(float64(0.0))
    34  	rightX.Store(float64(0.0))
    35  	rightY.Store(float64(0.0))
    36  
    37  	work := func() {
    38  		stick.On(joystick.SquarePress, func(data interface{}) {
    39  			drone.TakeOff()
    40  		})
    41  
    42  		stick.On(joystick.TrianglePress, func(data interface{}) {
    43  			drone.Hover()
    44  		})
    45  
    46  		stick.On(joystick.XPress, func(data interface{}) {
    47  			drone.Land()
    48  		})
    49  
    50  		stick.On(joystick.LeftX, func(data interface{}) {
    51  			val := float64(data.(int16))
    52  			leftX.Store(val)
    53  		})
    54  
    55  		stick.On(joystick.LeftY, func(data interface{}) {
    56  			val := float64(data.(int16))
    57  			leftY.Store(val)
    58  		})
    59  
    60  		stick.On(joystick.RightX, func(data interface{}) {
    61  			val := float64(data.(int16))
    62  			rightX.Store(val)
    63  		})
    64  
    65  		stick.On(joystick.RightY, func(data interface{}) {
    66  			val := float64(data.(int16))
    67  			rightY.Store(val)
    68  		})
    69  
    70  		gobot.Every(10*time.Millisecond, func() {
    71  			leftStick := getLeftStick()
    72  
    73  			switch {
    74  			case leftStick.y < -10:
    75  				drone.Forward(ardrone.ValidatePitch(leftStick.y, offset))
    76  			case leftStick.y > 10:
    77  				drone.Backward(ardrone.ValidatePitch(leftStick.y, offset))
    78  			default:
    79  				drone.Forward(0)
    80  			}
    81  
    82  			switch {
    83  			case leftStick.x > 10:
    84  				drone.Right(ardrone.ValidatePitch(leftStick.x, offset))
    85  			case leftStick.x < -10:
    86  				drone.Left(ardrone.ValidatePitch(leftStick.x, offset))
    87  			default:
    88  				drone.Right(0)
    89  			}
    90  		})
    91  
    92  		gobot.Every(10*time.Millisecond, func() {
    93  			rightStick := getRightStick()
    94  
    95  			switch {
    96  			case rightStick.y < -10:
    97  				drone.Up(ardrone.ValidatePitch(rightStick.y, offset))
    98  			case rightStick.y > 10:
    99  				drone.Down(ardrone.ValidatePitch(rightStick.y, offset))
   100  			default:
   101  				drone.Up(0)
   102  			}
   103  
   104  			switch {
   105  			case rightStick.x > 20:
   106  				drone.Clockwise(ardrone.ValidatePitch(rightStick.x, offset))
   107  			case rightStick.x < -20:
   108  				drone.CounterClockwise(ardrone.ValidatePitch(rightStick.x, offset))
   109  			default:
   110  				drone.Clockwise(0)
   111  			}
   112  		})
   113  	}
   114  
   115  	robot := gobot.NewRobot("ardrone",
   116  		[]gobot.Connection{joystickAdaptor, ardroneAdaptor},
   117  		[]gobot.Device{stick, drone},
   118  		work,
   119  	)
   120  
   121  	robot.Start()
   122  }
   123  
   124  func getLeftStick() pair {
   125  	s := pair{x: 0, y: 0}
   126  	s.x = leftX.Load().(float64)
   127  	s.y = leftY.Load().(float64)
   128  	return s
   129  }
   130  
   131  func getRightStick() pair {
   132  	s := pair{x: 0, y: 0}
   133  	s.x = rightX.Load().(float64)
   134  	s.y = rightY.Load().(float64)
   135  	return s
   136  }