gobot.io/x/gobot/v2@v2.1.0/examples/ardrone_ps3.go (about)

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