Ejemplo n.º 1
0
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)
	}
}
Ejemplo n.º 2
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)
		}
	}
}
Ejemplo n.º 3
0
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
}