PARS-MultiOAOld < Main

Tags: view all tags
<span style="color: #7a4707; font-family: 'Courier New', courier, monaco, monospace; white-space: pre;">&lt;meta name="robots" content="noindex" /&gt;</span>

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


---------------------------------------------------------------------------
-- MP processes to make message-passing more explicit-----------------------

TYPE VEC2F putVal

TYPE VEC2F gotVal

TYPE VEC2F check

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

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

---------------------------------------------------------------------------

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<G1>(odM2) ; Put_message<SIGNAL2>(cG1))	|  

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

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

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

	(Got_message<SIGNAL2>(cG1) ; Avoid_Obstacles<Z0,G2>(odM,obM)(cC202))		|
			
	(Is_at_goal<G2>(odM) ; Put_message<SIGNAL1>(cG2))			|

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

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

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

			(Got_message<SIGNAL1>(cG2) ; Avoid_Obstacles<Z0,G3>(odM2,obM2)(cC302))	|
	
			(Is_at_goal<G3>(odM2) ; Spin<G90>(hM2)(voM2))	|
			
			(Is_at_head<G90>(hM2) ; Put_message<SIGNAL2>(cG3))	|

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

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

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

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

	(Is_at_goal<G4>(odM) ; Spin<G90>(hM)(voM))			|

	(Is_at_head<G90>(hM) ; Put_message<SIGNAL1>(cG4))		|

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

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

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

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

			(Is_at_goal<G5>(odM2) ; Put_message<SIGNAL2>(cG5))			|
	
	(Got_message<SIGNAL2>(cG5) ; Coop<1,1,1,G6>(odM,cV601,cC601,cC602)(voM)) 	|	

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

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

	(Got_message<SIGNAL2>(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
Topic revision: r1 - 2015-01-29 - PengTang