diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index 94d3ebc..c4246ab 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -23,19 +23,6 @@ function lj_vector(rid, data) { function lj_sum(rid, data, accum) { return math.vec2.add(data, accum) } - -function user_attract(t) { - fus = math.vec2.new(0.0, 0.0) - if(size(t)>0) { - foreach(t, function(u, tab) { - #log("id: ",u," Range ", tab.r, "Bearing ", tab.b) - fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b)) - }) - math.vec2.scale(fus, 1.0 / size(t)) - } - #print("User attract:", fus.x," ", fus.y, " [", size(t), "]") - return fus -} # Calculates and actuates the flocking interaction function action() { @@ -45,9 +32,6 @@ function action() { if(neighbors.count() > 0) accum = math.vec2.scale(accum, 1.0 / neighbors.count()) - #accum = math.vec2.add(accum, user_attract(users.dataL)) - #accum = math.vec2.scale(accum, 1.0 / 2.0) - if(math.vec2.length(accum) > 1.0) { accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum)) } @@ -80,6 +64,7 @@ function action() { # Executed once at init time. function init() { uav_initswarm() + uav_initstig() } # Executed at each time step. @@ -88,9 +73,11 @@ function step() { uav_neicmd() statef() + uav_updatestig() log("Current state: ", UAVSTATE) log("Swarm size: ",ROBOTS) - + if(id==0) + stattab_print() } # Executed once when the robot (or the simulator) is reset. diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 1da9ed9..e7439e9 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -7,35 +7,38 @@ include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "graphs/shapes_square.bzz" +include "graphs/shapes_L.bzz" ROBOT_RADIUS=50 ROBOT_DIAMETER=2.0*ROBOT_RADIUS ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER +# max velocity in cm/step +ROBOT_MAXVEL = 500.0 + # # Global variables # # -#Save message from all neighours +# Save message from all neighours #the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot m_MessageState={}#store received neighbour message -m_MessageLable={}#store received neighbour message -m_MessageReqLable={}#store received neighbour message +m_MessageLabel={}#store received neighbour message +m_MessageReqLabel={}#store received neighbour message m_MessageReqID={}#store received neighbour message m_MessageResponse={}#store received neighbour message m_MessageRange={}#store received neighbour message m_MessageBearing={}#store received neighbour message -m_neighbourCunt=0#used to cunt neighbours +m_neighbourCount=0#used to cunt neighbours #Save message from one neighbour -#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Response,Range,Bearing -m_receivedMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} +#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing +m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} # #Save the message to send -#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")} +#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} #Current robot state m_eState="STATE_FREE" @@ -84,7 +87,7 @@ m_fTargetDistanceTolerance=0 step_cunt=0 #virtual stigmergy -v_tag = stigmergy.create(1) +m_lockstig = 1 # Lennard-Jones parameters, may need change EPSILON = 13.5 #the LJ parameter for other robots @@ -220,7 +223,7 @@ function pow(base,exponent){ # function packmessage(send_table){ var send_value - send_value=10000*send_table.State+1000*send_table.Lable+100*send_table.ReqLable+10*send_table.ReqID+send_table.Response + send_value=10000*send_table.State+1000*send_table.Label+100*send_table.ReqLabel+10*send_table.ReqID+send_table.Response return send_value } # @@ -253,10 +256,10 @@ function unpackmessage(recv_value){ var shi=(recv_value-recv_value%10)/10 recv_value=recv_value-shi*10 var ge=recv_value - var return_table={.State=0.0,.Lable=0.0,.Reqlable=0.0,.ReqID=0.0,.Response=0.0} + var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0} return_table.State=wan - return_table.Lable=qian - return_table.ReqLable=bai + return_table.Label=qian + return_table.ReqLabel=bai return_table.ReqID=shi return_table.Response=ge return return_table @@ -309,13 +312,13 @@ function LJ_vec(i){ function motion_vector(){ var i=0 var m_vector={.x=0.0,.y=0.0} - while(i0) m_unWaitCount=m_unWaitCount-1 #find a set of joined robots - var setJoinedLables={} + var setJoinedLabels={} var setJoinedIndexes={} var i=0 var j=0 - while(i0){ - TransitionToAsking(unFoundLable) + if(unFoundLabel>0){ + TransitionToAsking(unFoundLabel) return } @@ -527,12 +530,15 @@ function DoFree() { tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes)) tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes)) m_navigation=math.vec2.add(tempvec_P,tempvec_N) + # Limit the mvt + if(math.vec2.length(m_navigation)>ROBOT_MAXVEL) + m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL/math.vec2.length(m_navigation)) uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) }else{ #no joined robots in sight i=0 var tempvec={.x=0.0,.y=0.0} - while(iROBOT_MAXVEL) + S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target)) + m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) @@ -648,7 +663,7 @@ function DoJoining(){ #test if is already in desired position if(math.abs(S2Target.x)0) log("Got a user!") - # log(users) - #users_print(users.dataG) - # if(size(users.dataG)>0) - # vt.put("p", users.dataG) +# log(users) +# users_print(users.dataG) +# if(size(users.dataG)>0) +# vt.put("p", users.dataG) # Get the number of keys in the structure #log("The vstig has ", vt.size(), " elements") @@ -24,10 +46,27 @@ function users_save(t) { } # printing the contents of a table: a custom function -function table_print(t) { +function usertab_print(t) { if(size(t)>0) { foreach(t, function(u, tab) { log("id: ",u," Range ", tab.r, "Bearing ", tab.b) }) } +} + +function stattab_print() { + if(v_status.size()>0) { + u=0 + while(u<8){ + tab = v_status.get(u) + if(tab!=nil) { + log("id: ", u) + log("- GPS ", tab.g) + log("- Battery ", tab.b) + log("- Xbee ", tab.x) + log("- Status ", tab.f) + } + u=u+1 + } + } } \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 66ef6d6..7a5eaf2 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step + static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size) static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; @@ -175,18 +175,17 @@ namespace buzz_utility{ } - void in_message_process(){ +void in_message_process(){ while(!IN_MSG.empty()){ - uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); /* Go through messages and append them to the FIFO */ - uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]); + uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); + size_t tot =0; /*Size is at first 2 bytes*/ - uint16_t size=data[0]*sizeof(uint64_t); - uint16_t neigh_id = data[1]; - ROS_WARN("NEIG ID %i",neigh_id); - delete[] data; - /*size and robot id read*/ - size_t tot = sizeof(uint32_t); + uint16_t size=(*(uint16_t*)first_INmsg)*sizeof(uint64_t); + tot += sizeof(uint16_t); + /*Decode neighbor Id*/ + uint16_t neigh_id =*(uint16_t*)(first_INmsg+tot); + tot+=sizeof(uint16_t); /* Go through the messages until there's nothing else to read */ uint16_t unMsgSize=0; /*Obtain Buzz messages push it into queue*/ @@ -197,13 +196,13 @@ namespace buzz_utility{ /* Append message to the Buzz input message queue */ if(unMsgSize > 0 && unMsgSize <= size - tot ) { buzzinmsg_queue_append(VM, - neigh_id, + neigh_id, buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize)); tot += unMsgSize; } }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - IN_MSG.erase(IN_MSG.begin()); - free(first_INmsg); + free(first_INmsg); + IN_MSG.erase(IN_MSG.begin()); } /* Process messages VM call*/ buzzvm_process_inmsgs(VM);