/
joysphero.go
67 lines (57 loc) · 1.46 KB
/
joysphero.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
package main
import (
"fmt"
"math"
"github.com/hybridgroup/gobot"
"github.com/hybridgroup/gobot-joystick"
"github.com/hybridgroup/gobot-sphero"
)
// Angle
func Angle(x, y int) uint16 {
angle := math.Atan2(float64(x), float64(y)) / (math.Pi * 2) * 360
direction := math.Mod((360 + 180 + angle), 360)
return uint16(direction)
}
func main() {
joystickAdaptor := new(gobotJoystick.JoystickAdaptor)
joystickAdaptor.Name = "x52"
joystickAdaptor.Params = map[string]interface{}{
"config": "./saitek-x52.json",
}
joystick := gobotJoystick.NewJoystick(joystickAdaptor)
joystick.Name = "x52"
spheroAdaptor := new(gobotSphero.SpheroAdaptor)
spheroAdaptor.Name = "Sphero"
spheroAdaptor.Port = "/dev/tty.Sphero-PWG-RN-SPP"
sphero := gobotSphero.NewSphero(spheroAdaptor)
sphero.Name = "Sphero"
var (
x, y int
direction uint16
speed uint8
)
x = 1
y = 1
direction = 0
speed = 0
work := func() {
gobot.On(joystick.Events["right_x"], func(data interface{}) {
x = int(data.(int16))
})
gobot.On(joystick.Events["right_y"], func(data interface{}) {
y = int(data.(int16))
})
gobot.Every("0.01s", func() {
direction = Angle(x, y)
speed = uint8(math.Sqrt(float64(x*x+y*y)) / 128) //255
fmt.Println(x, y, speed, direction)
sphero.Roll(speed, direction)
})
}
robot := gobot.Robot{
Connections: []gobot.Connection{joystickAdaptor, spheroAdaptor},
Devices: []gobot.Device{joystick, sphero},
Work: work,
}
robot.Start()
}