コード例 #1
0
ファイル: waypoint_test.go プロジェクト: nzmichaelh/nppilot
func TestDistance(t *testing.T) {
	w, _, _, s := setup()
	ref := s.GPS

	// Positioned to the north by 50 m
	s.GPS.Latitude = ref.Latitude + 50*DegreesToM
	demand := w.Step(s)

	tx.CheckClosef2(t, demand.Throttle, 50, 0.3)
	tx.CheckClosef(t, demand.Steering, math.Pi)

	// To the south by 20 m
	s.GPS.Latitude = ref.Latitude - 20*DegreesToM
	demand = w.Step(s)

	tx.CheckClosef2(t, demand.Throttle, 20, 0.3)
	tx.CheckClosef(t, demand.Steering, 0)

	s.GPS.Latitude = ref.Latitude

	// To the east by 10 m
	s.GPS.Longitude = ref.Longitude + 10*DegreesToM
	demand = w.Step(s)

	tx.CheckClosef2(t, demand.Throttle, 10, 0.3)
	tx.CheckClosef(t, demand.Steering, -math.Pi/2)

	// To the west by 30 m
	s.GPS.Longitude = ref.Longitude - 30*DegreesToM
	demand = w.Step(s)

	tx.CheckClosef2(t, demand.Throttle, 30, 0.3)
	tx.CheckClosef(t, demand.Steering, math.Pi/2)
}
コード例 #2
0
ファイル: pid_test.go プロジェクト: nzmichaelh/nppilot
func TestKi(t *testing.T) {
	pid := PID{Ki: 4, UMin: -100, UMax: 100, TiLimit: 20}

	// Increases.
	tx.CheckClosef(t, pid.Step(10, dt), 8)
	tx.CheckClosef(t, pid.Step(10, dt), 16)
	// Stops at the TiLimit.
	tx.CheckClosef(t, pid.Step(10, dt), 20)
}
コード例 #3
0
ファイル: heading_test.go プロジェクト: nzmichaelh/nppilot
func TestHeading(t *testing.T) {
	c := &HeadingController{
		PID: &PID{Kp: 1, UMax: 10, UMin: -10}}

	s := &Status{}
	s.Input.Switch = 1
	s.GPS.Track = 0

	// Save the heading as the set point.
	c.Step(s)
	s.Input.Switch = 2
	// On track.
	s.GPS.Track = 0
	tx.CheckClosef(t, c.Step(s).Steering, 0)
	// Actual heading is to the west.  Expect turn to the left.
	s.GPS.Track = 1
	tx.CheckClosef(t, c.Step(s).Steering, -1)
	// Actual heading is to the east.  Expect turn to the right.
	s.GPS.Track = -1
	tx.CheckClosef(t, c.Step(s).Steering, 1)
}
コード例 #4
0
ファイル: heading_test.go プロジェクト: nzmichaelh/nppilot
func TestWrap(t *testing.T) {
	c := &HeadingController{
		PID: &PID{Kp: 1, UMax: 10, UMin: -10}}

	s := &Status{}
	s.Input.Switch = 1
	s.GPS.Track = 2

	// Save the heading as the set point.
	c.Step(s)
	s.Input.Switch = 2

	s.GPS.Track = 1
	tx.CheckClosef(t, c.Step(s).Steering, 1)
	s.GPS.Track = -1
	tx.CheckClosef(t, c.Step(s).Steering, 3)
	// Still quicker to turn left.
	s.GPS.Track = -1.14
	tx.CheckClosef(t, c.Step(s).Steering, 3.14)

	s.GPS.Track = 3
	tx.CheckClosef(t, c.Step(s).Steering, -1)
	// Wrap around.  Pi-2 to the wrap point, then Pi-3 to -3.
	s.GPS.Track = -3
	tx.CheckClosef(t, c.Step(s).Steering, -(1.1415 + 0.1415))
	// Almost on the bounary.
	s.GPS.Track = -1.2
	tx.CheckClosef(t, c.Step(s).Steering, -3.083)
}
コード例 #5
0
ファイル: pid_test.go プロジェクト: nzmichaelh/nppilot
func TestULimits(t *testing.T) {
	pid := PID{Kp: 5, UMin: -50, UMax: 150}

	tx.CheckClosef(t, pid.Step(-9, dt), -45)
	tx.CheckClosef(t, pid.Step(-10, dt), -50)
	tx.CheckClosef(t, pid.Step(-11, dt), -50)

	tx.CheckClosef(t, pid.Step(29, dt), 145)
	tx.CheckClosef(t, pid.Step(30, dt), 150)
	tx.CheckClosef(t, pid.Step(31, dt), 150)
}
コード例 #6
0
ファイル: pid_test.go プロジェクト: nzmichaelh/nppilot
func TestKp(t *testing.T) {
	pid := PID{Kp: 3, UMin: -100, UMax: 100}

	tx.CheckClosef(t, pid.Step(10, dt), 30)
	tx.CheckClosef(t, pid.Step(-30, dt), -90)
}
コード例 #7
0
ファイル: clip_test.go プロジェクト: nzmichaelh/nppilot
func TestClipf(t *testing.T) {
	tx.CheckClosef(t, Clipf(0.7, 0.5, 1.0), 0.7)
	tx.CheckClosef(t, Clipf(1.1, 0.5, 1.0), 1.0)
	tx.CheckClosef(t, Clipf(0.3, 0.5, 1.0), 0.5)
}