func debug() {
c := attitude.NewComplementary()
for {
data, err := navboard.NextData()
if err != nil {
continue
}
a := c.Update(data.Data)
fmt.Printf("\r%s -> %s", data, a)
}
}
func loop(m *motorboard.Motorboard) {
var (
p = 0.9
i = 0.4
d = 0.2
basespeed = 0.5
roll = pidctrl.NewPIDController(p, i, d)
pitch = pidctrl.NewPIDController(p, i, d)
yaw = pidctrl.NewPIDController(p, i, d)
filter = attitude.NewComplementary()
)
for {
data, err := navboard.NextData()
if err != nil {
continue
}
a := filter.Update(data.Data)
ro := roll.Update(a.Roll / 90)
po := pitch.Update(a.Pitch / 90)
yo := yaw.Update(a.Yaw/90) / 5
_ = yo
_ = basespeed
fmt.Printf("roll: %.2f pitch: %.2f yaw: %.2f - %s\r", ro, po, a)
//speeds := [4]float64{basespeed, basespeed, basespeed, basespeed}
speeds := [4]float64{0.8, 0, 0.8, 0}
//if ro > 0 {
//speeds[0], speeds[3] = speeds[0]+ro, speeds[3]+ro
//} else if ro < 0 {
//speeds[1], speeds[2] = speeds[1]-ro, speeds[2]-ro
//}
//if po > 0 {
//speeds[0], speeds[1] = speeds[0]+po, speeds[1]+po
//} else if po < 0 {
//speeds[2], speeds[3] = speeds[2]-po, speeds[3]-po
//}
//if yo > 0 {
//speeds[1], speeds[3] = speeds[1]+yo, speeds[3]+yo
//} else if yo < 0 {
//speeds[0], speeds[2] = speeds[0]-yo, speeds[2]-yo
//}
for i, speed := range speeds {
m.SetSpeed(i, speed)
}
}
}
func NewInstances(c Config) (i Instances, err error) {
i.log = log.DefaultLogger
i.navboard = navboard.NewNavboard(c.NavboardTTY, i.log)
i.motorboard, err = motorboard.NewMotorboard(c.MotorboardTTY)
if err != nil {
return
}
i.attitude = attitude.NewComplementary()
i.control = control.NewControl(c.RollPID, c.PitchPID, c.YawPID)
i.http = http.NewHandler(i.control, i.log)
return
}