-- Multirobot Waypoint mission with obstacle avoidance

-- August 2014 dml

-- This version modified from

-- multirobot w OBSTACLE AVOIDANCE (both static obstacles (via union) and other robot pos.

-- dagan 3/14

-- and includes the mutual robot obstacle avoidance, and so uses different robot models

--

TSS RESULT-VAR posR1

TSS RESULT-VAR posR2

TSS RESULT-VAR oposR1

TSS RESULT-VAR oposR2

-- include atomic definitions

PARS INCLUDE atomic

-- include environment model

PARS INCLUDE two_obstacles_environment

-- * Some Constants *

CONST REAL Tstep=1.0 -- discrete time step of environment model

CONST REAL MaxSpeed=300.0 -- speed in mm per timestep

CONST REAL BaseSpeed=200.0 -- speed in mm per timestep

CONST REAL AngSpeed=0.29 -- angular velocity in radians per timestep

CONST VEC2F SIGNAL1=(5,5) --arbitrarily-valued flag variable

CONST VEC2F SIGNAL2=(5,5) --arbitrarily-valued flag variable

-- initial configuration of the robots

CONST NORM P01=(P01mean,P01var) --* Robot 1 initial position distribution

CONST VEC2F P01mean=(5500,2000) --testing

CONST VEC4F P01var=(500,0,0,500)

CONST NORM P02=(P02mean,P02var) --* Robot 2 initial position distribution

CONST VEC2F P02mean=(6500,2000) --testing

CONST VEC4F P02var=(500,0,0,500)

CONST VEC2F V0=(0,0) --* initial velocity

--New goal locations created by subtracting goal_Tolerance value from actual waypoint --along the axis of travel.

--CONST VEC2F G1=(9500,6000)

CONST VEC2F G1=(9000,6000)

--CONST VEC2F G2=(11500,4000)

CONST VEC2F G2=(11000,4000)

--CONST VEC2F G3=(17000,6000)

CONST VEC2F G3=(16500,6000)

--CONST VEC2F G4=(19500,4000)

CONST VEC2F G4=(19000,4000)

CONST VEC2F G5=(16500,11000)

CONST VEC2F G5r=(16500,11500) --the real final waypoint (no tolerances subtracted) for use with IAP

CONST VEC2F G6=(19000,12500)

CONST VEC2F G6r=(19000,13000) --the real final waypoint

-- dml 4/14/14 - needed to define different init values for the two robots

-- so they could be started seperately, but also so indep static analysis catches connections

CONST NORM Z0=(Z0mean,Z0var)

CONST VEC2F Z0mean=(0.0,0.0)

CONST VEC4F Z0var=(0,0,0,0)

CONST NORM Z0R1=(Z0mean,Z0var)

CONST NORM Z0R2=(Z0mean,Z0var)

CONST NORM H0=(H0R1mean,H0var)

CONST VEC2F H0R1mean=(1.0,0.0) --* initial heading unit vector

CONST VEC4F H0var=(0,0,0,0)

CONST NORM H0R1=(H0R1mean,H0var)

CONST NORM H0R2=(H0R2mean,H0var)

CONST VEC2F H0R2mean=(1.0,0.0) --* initial heading unit vector

-- include the CNL definitions

PARS INCLUDE cnl

-- include robot definitions

PARS INCLUDE robot1OA

PARS INCLUDE robot2OA

TYPE VEC2F putVal

TYPE VEC2F gotVal

TYPE VEC2F check

Put_message(mOUT) = OUT<mOUT, putVal > ; COND<GTR,putVal,SIGNAL1> ; Put_message .

Got_message(mIN) = IN ; COND<GTR,gotVal,check> ; Got_message .

Mission (piM,piM2,obM,obM2,siM,siM2,hM,hM2,odM,odM2)(voM,voM2) =

Coop<1,1,1,G1>(odM2,cV101,cC101,cC102)(voM2) |

Move_to<G1,2000,1.5>(odM2)(cV101) |

Noise<Z0,G1>(odM2)(cC101) |

Avoid_Obstacles<Z0,G1>(odM2,obM2)(cC102) |

(Is_at_goal(odM2) ; Put_message(cG1)) |

(Got_message(cG1) ; Coop<1,1,1,G2>(odM,cV201,cC201,cC202)(voM)) |

(Got_message(cG1) ; Move_to<G2,2000,1.5>(odM)(cV201)) |

(Got_message(cG1) ; Noise<Z0,G2>(odM)(cC201)) |

(Got_message(cG1) ; Avoid_Obstacles<Z0,G2>(odM,obM)(cC202)) |

(Is_at_goal(odM) ; Put_message(cG2)) |

(Got_message(cG2) ; Coop<1,1,1,G3>(odM2,cV301,cC301,cC302)(voM2)) |

(Got_message(cG2) ; Move_to<G3,2000,1.5>(odM2)(cV301)) |

(Got_message(cG2) ; Noise<Z0,G3>(odM2)(cC301)) |

(Got_message(cG2) ; Avoid_Obstacles<Z0,G3>(odM2,obM2)(cC302)) |

(Is_at_goal(odM2) ; Spin(hM2)(voM2)) |

(Is_at_head(hM2) ; Put_message(cG3)) |

(Got_message(cG3) ; Coop<1,1,1,G4>(odM,cV401,cC401,cC402)(voM)) |

(Got_message(cG3) ; Move_to<G4,2000,1.5>(odM)(cV401)) |

(Got_message(cG3) ; Noise<Z0,G4>(odM)(cC401)) |

(Got_message(cG3) ; Avoid_Obstacles<Z0,G4>(odM,obM)(cC402)) |

(Is_at_goal(odM) ; Spin(hM)(voM)) |

(Is_at_head(hM) ; Put_message(cG4)) |

(Got_message(cG4) ; Coop<1,1,1,G5>(odM2,cV501,cC501,cC502)(voM2)) |

(Got_message(cG4) ; Move_to<G5,2000,1.5>(odM2)(cV501)) |

(Got_message(cG4) ; Noise<Z0,G5>(odM2)(cC501)) |

(Got_message(cG4) ; Avoid_Obstacles<Z0,G5>(odM2,obM2)(cC502)) |

(Is_at_goal(odM2) ; Put_message(cG5)) |

(Got_message(cG5) ; Coop<1,1,1,G6>(odM,cV601,cC601,cC602)(voM)) |

(Got_message(cG5) ; Move_to<G6,2000,1.5>(odM)(cV601)) |

(Got_message(cG5) ; Noise<Z0,G6>(odM)(cC601)) |

(Got_message(cG5) ; Avoid_Obstacles<Z0,G6>(odM,obM)(cC602)).

--System is the concurrent composition of controller (Mission) and environment (Robots)

SYS = Mission (cPOS,cPOS2,cOB,cOB2,cSEN,cSEN2,cHEAD,cHEAD2,cOPOS,cOPOS2)(cVEL,cVEL2) |

Robot1<P01,P02,Z0R1,H0R1,Z0R1,H0R1mean,P01>(cVEL,cSH2)(cPOS, cSH1, cOB, cSEN,cHEAD,cOPOS) |

Robot2<P02,P01,Z0R2,H0R2,Z0R2,H0R2mean,P02>(cVEL2,cSH1)(cPOS2,cSH2, cOB2,cSEN2,cHEAD2,cOPOS2) .

-- NO GOAL PROCESS NETWORK DEFINED - GOAL IS BY TERMINATION

-- (c) Fordham University Robotics and Computer Vision