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)
}
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)
}
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)
}
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)
}
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)
}
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)
}
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)
}
