Tags:
tag this topic
create new tag
view all tags
<span style="color: #7a4707; font-family: 'Courier New', courier, monaco, monospace; white-space: pre;"><meta name="robots" content="noindex" /></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
E
dit
|
A
ttach
|
Watch
|
P
rint version
|
H
istory
: r1
|
B
acklinks
|
V
iew topic
|
Ra
w
edit
|
M
ore topic actions
Topic revision: r1 - 2015-01-29
-
PengTang
Home
Site map
Main web
Sandbox web
TWiki web
Main Web
Users
Groups
Index
Search
Changes
Notifications
RSS Feed
Statistics
Preferences
P
P
P
P
View
Raw View
Print version
Find backlinks
History
More topic actions
Edit
Raw edit
Attach file or image
Edit topic preference settings
Set new parent
More topic actions
Account
Log In
E
dit
A
ttach
Copyright © 2008-2023 by the contributing authors. All material on this collaboration platform is the property of the contributing authors.
Ideas, requests, problems regarding TWiki?
Send feedback