-
Notifications
You must be signed in to change notification settings - Fork 7
/
gimbal.go
82 lines (68 loc) · 1.97 KB
/
gimbal.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
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
package systems
import (
"github.com/EngoEngine/ecs"
"github.com/EngoEngine/engo"
"github.com/brunoga/robomaster/sdk"
"github.com/brunoga/robomaster/sdk/examples/robotcontrol/components"
"github.com/brunoga/robomaster/sdk/modules/gimbal"
)
type systemGimbalEntity struct {
ecs.BasicEntity
*components.Speed
}
type Gimbal struct {
entity *systemGimbalEntity
client *sdk.Client
mirrorClients []*sdk.Client
}
func NewGimbal(client *sdk.Client,
mirrorClients []*sdk.Client) *Gimbal {
return &Gimbal{
&systemGimbalEntity{
ecs.NewBasic(),
&components.Speed{},
},
client,
mirrorClients,
}
}
func (g *Gimbal) New(world *ecs.World) {
g.client.GimbalModule().Recenter()
for _, mirrorClient := range g.mirrorClients {
mirrorClient.GimbalModule().Recenter()
}
}
func pixelsToDegrees(pixels float32, resolutionPixels, fovDegrees int) float64 {
return (float64(pixels) * float64(fovDegrees)) / float64(resolutionPixels)
}
func pixelsToYawDegrees(pixels float32) float64 {
return pixelsToDegrees(pixels, sdk.CameraVerticalResolutionPoints,
sdk.CameraVerticalFOVDegrees)
}
func pixelsToPitchDegrees(pixels float32) float64 {
return pixelsToDegrees(pixels, sdk.CameraHorizontalResolutionPoints,
sdk.CameraHorizontalFOVDegrees)
}
func (g *Gimbal) Update(dt float32) {
mouseXDelta := engo.Input.Axis("MouseXAxis").Value()
mouseXSpeed := pixelsToYawDegrees(mouseXDelta) * 30
mouseYDelta := engo.Input.Axis("MouseYAxis").Value()
mouseYSpeed := -pixelsToPitchDegrees(mouseYDelta) * 30
if g.entity.SpeedX != mouseXSpeed ||
g.entity.SpeedY != mouseYSpeed {
g.client.GimbalModule().SetSpeed(gimbal.NewSpeed(mouseYSpeed,
mouseXSpeed), true)
for _, mirrorClient := range g.mirrorClients {
mirrorClient.GimbalModule().SetSpeed(gimbal.NewSpeed(
mouseYSpeed, mouseXSpeed), true)
}
g.entity.SpeedX = mouseXSpeed
g.entity.SpeedY = mouseYSpeed
}
}
func (g *Gimbal) Remove(e ecs.BasicEntity) {
// Do nothing.
}
func (g *Gimbal) Priority() int {
return 10
}