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

     1  // +build example
     2  //
     3  // Do not build by default.
     4  
     5  /*
     6   How to setup
     7   You must be using a PS3 or compatible controller, along with
     8   one of the Parrot Mambo drones to run this example.
     9  
    10   You run the Go program on your computer and communicate
    11   wirelessly with the Mambo.
    12  
    13   How to run
    14   Pass the Bluetooth name or address as first param:
    15  
    16  	go run examples/minidrone_mambo_ps3.go "Mambo_1234"
    17  
    18   NOTE: sudo is required to use BLE in Linux
    19  */
    20  
    21  package main
    22  
    23  import (
    24  	"os"
    25  	"sync/atomic"
    26  	"time"
    27  
    28  	"gobot.io/x/gobot"
    29  	"gobot.io/x/gobot/platforms/ble"
    30  	"gobot.io/x/gobot/platforms/joystick"
    31  	"gobot.io/x/gobot/platforms/parrot/minidrone"
    32  )
    33  
    34  type pair struct {
    35  	x float64
    36  	y float64
    37  }
    38  
    39  var leftX, leftY, rightX, rightY atomic.Value
    40  
    41  const offset = 32767.0
    42  
    43  func main() {
    44  	joystickAdaptor := joystick.NewAdaptor()
    45  	stick := joystick.NewDriver(joystickAdaptor,
    46  		"./platforms/joystick/configs/dualshock3.json",
    47  	)
    48  
    49  	droneAdaptor := ble.NewClientAdaptor(os.Args[1])
    50  	drone := minidrone.NewDriver(droneAdaptor)
    51  
    52  	work := func() {
    53  		leftX.Store(float64(0.0))
    54  		leftY.Store(float64(0.0))
    55  		rightX.Store(float64(0.0))
    56  		rightY.Store(float64(0.0))
    57  
    58  		clawOpen := false
    59  
    60  		stick.On(joystick.CirclePress, func(data interface{}) {
    61  			if clawOpen {
    62  				drone.ClawControl(0, minidrone.ClawClosed)
    63  				clawOpen = false
    64  			} else {
    65  				drone.ClawControl(0, minidrone.ClawOpen)
    66  				clawOpen = true
    67  			}
    68  		})
    69  
    70  		stick.On(joystick.R2Press, func(data interface{}) {
    71  			if clawOpen {
    72  				drone.ClawControl(0, minidrone.ClawClosed)
    73  				clawOpen = false
    74  			} else {
    75  				drone.ClawControl(0, minidrone.ClawOpen)
    76  				clawOpen = true
    77  			}
    78  		})
    79  
    80  		stick.On(joystick.TrianglePress, func(data interface{}) {
    81  			drone.HullProtection(true)
    82  			drone.TakeOff()
    83  		})
    84  
    85  		stick.On(joystick.XPress, func(data interface{}) {
    86  			drone.Land()
    87  		})
    88  
    89  		stick.On(joystick.LeftX, func(data interface{}) {
    90  			val := float64(data.(int16))
    91  			leftX.Store(val)
    92  		})
    93  
    94  		stick.On(joystick.LeftY, func(data interface{}) {
    95  			val := float64(data.(int16))
    96  			leftY.Store(val)
    97  		})
    98  
    99  		stick.On(joystick.RightX, func(data interface{}) {
   100  			val := float64(data.(int16))
   101  			rightX.Store(val)
   102  		})
   103  
   104  		stick.On(joystick.RightY, func(data interface{}) {
   105  			val := float64(data.(int16))
   106  			rightY.Store(val)
   107  		})
   108  
   109  		gobot.Every(10*time.Millisecond, func() {
   110  			rightStick := getRightStick()
   111  
   112  			switch {
   113  			case rightStick.y < -10:
   114  				drone.Forward(minidrone.ValidatePitch(rightStick.y, offset))
   115  			case rightStick.y > 10:
   116  				drone.Backward(minidrone.ValidatePitch(rightStick.y, offset))
   117  			default:
   118  				drone.Forward(0)
   119  			}
   120  
   121  			switch {
   122  			case rightStick.x > 10:
   123  				drone.Right(minidrone.ValidatePitch(rightStick.x, offset))
   124  			case rightStick.x < -10:
   125  				drone.Left(minidrone.ValidatePitch(rightStick.x, offset))
   126  			default:
   127  				drone.Right(0)
   128  			}
   129  		})
   130  
   131  		gobot.Every(10*time.Millisecond, func() {
   132  			leftStick := getLeftStick()
   133  			switch {
   134  			case leftStick.y < -10:
   135  				drone.Up(minidrone.ValidatePitch(leftStick.y, offset))
   136  			case leftStick.y > 10:
   137  				drone.Down(minidrone.ValidatePitch(leftStick.y, offset))
   138  			default:
   139  				drone.Up(0)
   140  			}
   141  
   142  			switch {
   143  			case leftStick.x > 20:
   144  				drone.Clockwise(minidrone.ValidatePitch(leftStick.x, offset))
   145  			case leftStick.x < -20:
   146  				drone.CounterClockwise(minidrone.ValidatePitch(leftStick.x, offset))
   147  			default:
   148  				drone.Clockwise(0)
   149  			}
   150  		})
   151  	}
   152  
   153  	robot := gobot.NewRobot("minidrone",
   154  		[]gobot.Connection{joystickAdaptor, droneAdaptor},
   155  		[]gobot.Device{stick, drone},
   156  		work,
   157  	)
   158  
   159  	robot.Start()
   160  }
   161  
   162  func getLeftStick() pair {
   163  	s := pair{x: 0, y: 0}
   164  	s.x = leftX.Load().(float64)
   165  	s.y = leftY.Load().(float64)
   166  	return s
   167  }
   168  
   169  func getRightStick() pair {
   170  	s := pair{x: 0, y: 0}
   171  	s.x = rightX.Load().(float64)
   172  	s.y = rightY.Load().(float64)
   173  	return s
   174  }