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

-- 1/17 peng, using cnl V2.0

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

--TSS RESULT-VAR uR1

--TSS RESULT-VAR uR2

--TSS RESULT-VAR scaledX

--TSS RESULT-VAR scaledX2

--TSS RESULT-VAR rdh1N

--TSS RESULT-VAR rdh2N

--TSS RESULT-VAR skitter1N

--TSS RESULT-VAR skitter2N

-- include atomic definitions

PARS INCLUDE atomic

-- include environment model

PARS INCLUDE empty_environment

--PARS INCLUDE 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 REAL AngSpeed=0.17        -- angular velocity in radians per timestep

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

CONST REAL Tol=500.0

CONST REAL VelGain=1.5

CONST REAL DropOff=2000.0


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

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

CONST VEC2F SPINSIGNAL1=(15,15) 	--arbitrarily-valued flag variable

CONST VEC2F SPINSIGNAL2=(15,15) 	--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=(9265,6424)   	

CONST VEC2F G2=(11476,4172)

CONST VEC2F G3=(17000,6000)

CONST VEC2F G4=(19500,4000)	

CONST VEC2F G5=(16500,11500)	

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

CONST VEC2F G6=(19000,13000)	

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

-- 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,Z0,Z0,Tol,G1>(odM2,cV101,cC101,cC102)(voM2) |

	    Move_to<G1,DropOff,VelGain,Tol,G1>(odM2)(cV101)           |

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

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


			(Is_at_goal<G1,Tol,G1>(odM2) ; Put_message<SIGNAL2>(cG1))	|  

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

	(Got_message<SIGNAL2>(cG1) ; Move_to<G2,DropOff,VelGain,Tol,G2>(odM)(cV201)) 		        |

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

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

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

			(Got_message<SIGNAL1>(cG2) ; Move_to<G3,DropOff,VelGain,Tol,G3>(odM2)(cV301))		|

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

			(Got_message<SIGNAL1>(cG2) ; Avoid_Obstacles<Z0,G3,Tol,G3>(odM2,obM2)(cC302))	|
	
			(Is_at_goal<G3,Tol,G3>(odM2) ; Spin<G90R>(hM2)(voM2,alt2))	|
			
			(Spin_done(alt2)   ; Put_message<SIGNAL2>(cG3))	|

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

	(Got_message<SIGNAL2>(cG3) ; Move_to<G4,DropOff,VelGain,Tol,G4>(odM)(cV401))  		|

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

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

	(Is_at_goal<G4,Tol,G4>(odM) ; Spin<G90R>(hM)(voM,alt))			|

	(Spin_done(alt)      ; Put_message<SIGNAL1>(cG4))		|

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

			(Got_message<SIGNAL1>(cG4) ; Move_to<G5,DropOff,VelGain,Tol,G5>(odM2)(cV501))  		|

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

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

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

	(Got_message<SIGNAL2>(cG5) ; Move_to<G6,DropOff,VelGain,Tol,G6>(odM)(cV601))  		|

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

	(Got_message<SIGNAL2>(cG5) ; Avoid_Obstacles<Z0,G6,Tol,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