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

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