Download all sources:
chapter6.zip
chapter6.zip
class ObstacleSensor
operations
public GetData: () ==> set of Controller`Obstacle
GetData() ==
is not yet specified;
end ObstacleSensorclass PositionSensor
operations
public GetDirection: () ==> Vector
GetDirection() ==
is not yet specified;
public GetPosition: () ==> Point
GetPosition() ==
return new Point(1,1,1);
end PositionSensor class Vector
instance variables
x: real;
y: real;
inv MATH2`sqrt(x * x + y * y) < epsilon;
values
epsilon: real = 0.0001;
operations
public Vector: real * real ==> Vector
Vector(nx, ny) ==
( x := nx;
y := ny;
)
pre MATH2`sqrt(nx * nx + ny * ny) < epsilon;
public Angle: () ==> real
Angle() ==
return MATH2`atan(y/x)
pre x <> 0;
public AngleBetween: Vector ==> real
AngleBetween(v) ==
return Angle() - v.Angle();
public ExceedsLimits: Controller`SteeringCommands ==> bool
ExceedsLimits(fc) ==
let angDisp = cases fc:
<Pos> -> steeringAngularDisplacement,
<Zero>-> 0,
<Neg> -> - steeringAngularDisplacement
end
in
return abs (Angle() + angDisp) > MATH2`pi / 2;
values
steeringAngularDisplacement = MATH2`pi/360;
end Vector class GPS
end GPSclass SteeringController
operations
public GetPosition: () ==> Vector
GetPosition() ==
is not yet specified;
public SendCommand: Controller`SteeringCommands ==> ()
SendCommand(-) ==
is not yet specified;
sync
per GetPosition => #active(SendCommand) = 0;
per SendCommand => #active(GetPosition) +
#active(SendCommand) = 0;
end SteeringControllerclass MATH2
instance variables
static m : MATH := new MATH();
operations
public static
sin:real ==> real
sin(v) ==
return m.sin(v);
public static
cos:real ==> real
cos(v) ==
return m.cos(v);
public static
tan:real ==> real
tan(a) ==
return m.tan(a);
public static
cot:real ==> real
cot(a) ==
return m.cot(a);
public static
asin:real ==> real
asin(a) ==
return m.asin(a);
public static
acos:real ==> real
acos(a) ==
return m.acos(a);
public static
atan:real ==> real
atan(v) ==
return m.atan(v);
public static
acot:real ==> real
acot(a) ==
return atan(1/a);
public static
sqrt:real ==> real
sqrt(a) ==
return m.sqrt(a);
public static
pi_f:() ==> real
pi_f () ==
return m.pi_f();
values
public
pi = 3.14159265358979323846;
end MATH2 class Controller
types
public Obstacle = nat * nat;
Angle = real;
public SteeringCommands = <Pos> | <Neg> | <Zero>;
instance variables
obs : ObstacleSensor;
emergencyBrake : EmergencyBrake;
ins : PositionSensor;
str : SteeringController;
journeyPlan : set of Point;
inv { p.GetIndex() | p in set journeyPlan } =
{ 1,..., card journeyPlan };
journeyComplete: bool := false;
operations
Update: () ==> ()
Update() ==
let currentPosition = ins.GetPosition()
in
( if Route`GetPointAtIndex(journeyPlan,1).GetCoord() =
currentPosition.GetCoord()
then
if card journeyPlan = 1
then CompleteJourney()
else
( journeyPlan := Route`TakeStep(journeyPlan);
let obstacles = obs.GetData(),
route = PlotCourse(obstacles)
in
if route = nil
then emergencyBrake.Enable()
else
def dfps = ComputeDesiredSteerPosition(
ins.GetDirection(),
route.GetPoint(2),
str.GetPosition());
in AdjustSteering(dfps) ;
);
);
thread
while not journeyComplete and not emergencyBrake.IsEnabled() do
Update()
operations
CompleteJourney: () ==> ()
CompleteJourney() ==
journeyComplete := true;
AdjustSteering: SteeringCommands ==> ()
AdjustSteering(sc) ==
str.SendCommand(sc);
operations
PlotCourse: set of (nat * nat) ==> [Route]
PlotCourse(obstacles) ==
let nextWaypoint = Route`GetPointAtIndex(journeyPlan, 1),
posRoutes = Route`AvoidanceRoutes(obstacles,
ins.GetPosition(),
nextWaypoint)
in
if posRoutes = {}
then return nil
else ShortestFeasibleRoute(posRoutes);
ShortestFeasibleRoute: set of Route ==> Route
ShortestFeasibleRoute(routes) ==
let physPosRoutes =
{ r | r in set routes
& PossibleSteerCommands(r,
str.GetPosition(),
ins.GetDirection())},
shortestRoutes =
{ r | r in set physPosRoutes
& r.GetPoint(r.Length()).ShortestDist(
{ rp.GetPoint(rp.Length())
| rp in set physPosRoutes},
Route`GetPointAtIndex(journeyPlan,1))}
in
let r in set shortestRoutes
in return r;
functions
PossibleSteerCommands: Route * Vector * Vector -> bool
PossibleSteerCommands (r, sps, v) ==
let steerComms =
ComputeDesiredSteerPosition(v,r.GetPoint(2),sps)
in
not sps.ExceedsLimits(steerComms)
pre r.Length() >= 2;
ComputeDesiredSteerPosition: Vector * Point * Vector
-> SteeringCommands
ComputeDesiredSteerPosition (currentDir, dest, fps) ==
let newDir = dest.GetDirection()
in
let idealSteeringPos =
ComputeIdealSteering(currentDir, newDir)
in ComputeSteeringCommand(fps, idealSteeringPos);
ComputeIdealSteering: Vector * Vector -> Vector
ComputeIdealSteering(v1, v2) ==
let angle1 = v1.AngleBetween(v2),
angle2 = v2.Angle()
in new Vector(MATH2`cos(angle2 - angle1),
MATH2`sin(angle2 - angle1));
ComputeSteeringCommand: Vector * Vector -> SteeringCommands
ComputeSteeringCommand (actual, ideal) ==
if actual.AngleBetween(ideal) > 0
then <Neg>
elseif actual.AngleBetween(ideal) < 0
then <Pos>
else <Zero>;
operations
public SetJourneyPlan: set of Point ==> ()
SetJourneyPlan(pts) ==
journeyPlan := pts;
end Controllerclass Route
instance variables
points: set of Point;
inv forall p1, p2 in set points &
p1.GetCoord() = p2.GetCoord() => p1 = p2 and
forall p in set points &
p.GetIndex() <> card points
=> GetNext(p).GetCoord() in set
{ n.GetCoord() | n in set p.Neighbour()};
functions
static
public AvoidanceRoutes(
obstacles: set of (nat * nat),
currentPosition: Point,
nextWaypoint: Point) routes: set of Route
post forall r in set routes &
r.GetFirst().GetCoord() =
currentPosition.GetCoord() and
r.GetLast().GetCoord() =
nextWaypoint.GetCoord() and
r.GetCoords() inter obstacles = {};
operations
GetFirst: () ==> Point
GetFirst() ==
return GetPointAtIndex(points, 1);
GetLast: () ==> Point
GetLast() ==
return GetPointAtIndex(points, card points);
public Route: set of Point ==> Route
Route(pts) ==
points := pts;
public Length: () ==> nat
Length() ==
return card points;
public GetPoint: nat1 ==> Point
GetPoint(i) ==
return GetAtIndexFromIndex(i)
pre i < card points;
VisitsCoords: set of (nat * nat) ==> bool
VisitsCoords(coords) ==
return coords subset GetCoords();
functions
public static GetPointAtIndex: set of Point * nat -> Point
GetPointAtIndex(pts, index) ==
let p in set pts be st p.GetIndex() = index
in
p
pre exists p in set pts & p.GetIndex() = index;
operations
GetNext: Point ==> Point
GetNext(pt) ==
return GetAtIndexFromIndex(pt.GetIndex() + 1);
GetAtIndexFromIndex: nat ==> Point
GetAtIndexFromIndex(index) ==
return GetPointAtIndex(points, index);
functions
static public TakeStep: set of Point -> set of Point
TakeStep(pts) ==
let laterPoints = { pt | pt in set pts
& pt.GetIndex() <> 1 }
in
{ p.TakeStep() | p in set laterPoints };
operations
GetCoords: () ==> set of (nat * nat)
GetCoords() ==
return { p.GetCoord() | p in set points };
end Routeclass SteeringMonitor
instance variables
steering : SteeringController;
emergencyBrake: EmergencyBrake;
active : bool := true;
log : seq of seq of char;
operations
CheckSteering: () ==> bool
CheckSteering() ==
def sp = steering.GetPosition();
in if sp.ExceedsLimits(<Zero>)
then
( LogCondition("Steering beyond safety limits");
return false;
)
else
( if abs sp.Angle() > MATH`pi/4
then LogCondition("Steering near safety limit");
return true;
);
LogCondition: seq1 of char ==> ()
LogCondition(str) ==
log := log ^ [str];
thread
while active do
if not CheckSteering()
then ( emergencyBrake.Enable();
active := false;
)
end SteeringMonitorclass EmergencyBrake
instance variables
enabled: bool := false;
operations
public Enable: () ==> ()
Enable() ==
enabled := true;
public IsEnabled: () ==> bool
IsEnabled() ==
return enabled;
end EmergencyBrakeclass Point
instance variables
static maxX: nat1 := 4;
static maxY: nat1 := 4;
x: nat;
y: nat;
index: nat;
operations
public GetX: () ==> nat
GetX() ==
return x;
public GetY: () ==> nat
GetY() ==
return y;
SetX: nat ==> ()
SetX(nx) ==
x:= nx;
SetY: nat ==> ()
SetY(ny) ==
y := ny;
static public GetMaxX: () ==> nat
GetMaxX() ==
return maxX;
static public GetMaxY: () ==> nat
GetMaxY() ==
return maxY;
operations
public GetIndex: () ==> nat
GetIndex() ==
return index;
public GetCoord: () ==> nat * nat
GetCoord() ==
return mk_(x,y);
public Point: nat * nat * nat ==> Point
Point(nx, ny, nindex) ==
( x := nx;
y := ny;
index := nindex;
);
public Dist: Point ==> real
Dist(p) ==
return MATH2`sqrt((x - p.GetX()) ** 2 +
(y - p.GetY()) ** 2);
operations
public Neighbour: () ==> set of Point
Neighbour () ==
return { new Point(x, y1, index + 1)
| y1 in set {y-1,y+1}
& y1 >= 0 } union
{ new Point(x1, y, index + 1)
| x1 in set {x-1,x+1}
& x1 >= 0 };
public ShortestDist: set of Point * Point ==> bool
ShortestDist(posPoints, waypoint) ==
return forall p in set posPoints &
Dist(waypoint) <= p.Dist(waypoint)
pre self in set posPoints;
public GetDirection: () ==> Vector
GetDirection() ==
return new Vector(x, y);
operations
public TakeStep: () ==> Point
TakeStep() ==
( index := index - 1;
return self;
)
pre index > 1;
end Point