func NewControl(rollPID, pitchPID, yawPID [3]float64) *Control {
return &Control{
roll: pidctrl.NewPIDController(rollPID[0], rollPID[1], rollPID[2]),
pitch: pidctrl.NewPIDController(pitchPID[0], pitchPID[1], pitchPID[2]),
yaw: pidctrl.NewPIDController(yawPID[0], yawPID[1], yawPID[2]),
}
}
func NewControl(roll, pitch, yaw, altitude [3]float64) *Control {
return &Control{
roll: pidctrl.NewPIDController(roll[0], roll[1], roll[2]),
pitch: pidctrl.NewPIDController(pitch[0], pitch[1], pitch[2]),
yaw: pidctrl.NewPIDController(yaw[0], yaw[1], yaw[2]),
altitude: pidctrl.NewPIDController(altitude[0], altitude[1], altitude[2]),
}
}
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)
}
}
}
