package main import ( "github.com/hybridgroup/gobot" "github.com/hybridgroup/gobot/ardrone" "github.com/hybridgroup/gobot/joystick" "math" ) type pair struct { x float64 y float64 } func main() { joystickAdaptor := joystick.NewJoystickAdaptor() joystickAdaptor.Name = "ps3" joystickAdaptor.Params = map[string]interface{}{ "config": "../joytsitkc/configs/dualshock3.json", } joystick := joystick.NewJoystickDriver(joystickAdaptor) joystick.Name = "ps3" ardroneAdaptor := ardrone.NewArdroneAdaptor() ardroneAdaptor.Name = "Drone" drone := ardrone.NewArdroneDriver(ardroneAdaptor) drone.Name = "Drone" work := func() { offset := 32767.0 right_stick := pair{x: 0, y: 0} left_stick := pair{x: 0, y: 0} gobot.On(joystick.Events["square_press"], func(data interface{}) { drone.TakeOff() }) gobot.On(joystick.Events["triangle_press"], func(data interface{}) { drone.Hover() }) gobot.On(joystick.Events["x_press"], func(data interface{}) { drone.Land() }) gobot.On(joystick.Events["left_x"], func(data interface{}) { val := float64(data.(int16)) if left_stick.x != val { left_stick.x = val } }) gobot.On(joystick.Events["left_y"], func(data interface{}) { val := float64(data.(int16)) if left_stick.y != val { left_stick.y = val } }) gobot.On(joystick.Events["right_x"], func(data interface{}) { val := float64(data.(int16)) if right_stick.x != val { right_stick.x = val } }) gobot.On(joystick.Events["right_y"], func(data interface{}) { val := float64(data.(int16)) if right_stick.y != val { right_stick.y = val } }) gobot.Every("0.01s", func() { pair := left_stick if pair.y < -10 { drone.Forward(validatePitch(pair.y, offset)) } else if pair.y > 10 { drone.Backward(validatePitch(pair.y, offset)) } else { drone.Forward(0) } if pair.x > 10 { drone.Right(validatePitch(pair.x, offset)) } else if pair.x < -10 { drone.Left(validatePitch(pair.x, offset)) } else { drone.Right(0) } }) gobot.Every("0.01s", func() { pair := right_stick if pair.y < -10 { drone.Up(validatePitch(pair.y, offset)) } else if pair.y > 10 { drone.Down(validatePitch(pair.y, offset)) } else { drone.Up(0) } if pair.x > 20 { drone.Clockwise(validatePitch(pair.x, offset)) } else if pair.x < -20 { drone.CounterClockwise(validatePitch(pair.x, offset)) } else { drone.Clockwise(0) } }) } robot := gobot.Robot{ Connections: []gobot.Connection{joystickAdaptor, ardroneAdaptor}, Devices: []gobot.Device{joystick, drone}, Work: work, } robot.Start() } func validatePitch(data float64, offset float64) float64 { value := math.Abs(data) / offset if value >= 0.1 { if value <= 1.0 { return float64(int(value*100)) / 100 } else { return 1.0 } } else { return 0.0 } }