Esempio n. 1
0
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]),
	}
}
Esempio n. 2
0
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]),
	}
}
Esempio n. 3
0
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)
		}
	}
}