75 lines
1.4 KiB
Go
75 lines
1.4 KiB
Go
package rover
|
|
|
|
import (
|
|
tx "juju.net.nz/testex"
|
|
_ "log"
|
|
"math"
|
|
"testing"
|
|
)
|
|
|
|
const (
|
|
DegreesToM = 1.0 / 111000
|
|
)
|
|
|
|
func setup() (w *WaypointController, gps *GPS, r *Recorder, s *Status) {
|
|
w = &WaypointController{
|
|
Heading: &PID{
|
|
Kp: 1.0, UMax: 100, UMin: -100,
|
|
},
|
|
Speed: &PID{
|
|
Kp: 1.0, UMax: 100, UMin: -100,
|
|
},
|
|
Distance: &PID{
|
|
Kp: 1.0, UMax: 100, UMin: -100,
|
|
},
|
|
TargetSize: 2,
|
|
}
|
|
|
|
gps = &GPS{Ok: true, Latitude: 45, Longitude: 8}
|
|
r = NewRecorder()
|
|
s = &Status{Recorder: r, GPS: *gps}
|
|
|
|
r.GPS(gps)
|
|
w.GPS(gps)
|
|
r.State = Run
|
|
r.Mark(&r.position)
|
|
w.Event(Run)
|
|
|
|
return
|
|
}
|
|
|
|
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)
|
|
}
|