removed old buzz_scripts

This commit is contained in:
dave 2017-12-08 19:58:16 -05:00
parent 3576b624aa
commit 4c2dfe655d
5 changed files with 1 additions and 1822 deletions

View File

@ -1,896 +0,0 @@
#
# Include files
#
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=11 for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
# max velocity in cm/step
ROBOT_MAXVEL = 150.0
#
# Global variables
#
#
# 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_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_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#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),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" # replace with default UAVSTATE
#navigation vector
m_navigation={.x=0,.y=0}
#Debug message to be displayed in qt-opengl
#m_ossDebugMsg
#Debug vector to draw
#CVector2 m_cDebugVector
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
m_messageID={}
#neighbor distance to lock the current pattern
lock_neighbor_id={}
lock_neighbor_dis={}
#Label request id
m_unRequestId=0
#Global bias, used to map local coordinate to global coordinate
m_bias=0
#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
#Counter to wait for something to happen
m_unWaitCount=0
#Number of steps to wait before looking for a free label
m_unLabelSearchWaitTime=0
#Number of steps to wait for an answer to be received
m_unResponseTimeThreshold=0
#Number of steps to wait until giving up joining
m_unJoiningLostPeriod=0
#Tolerance distance to a target location
m_fTargetDistanceTolerance=0
#step cunt
step_cunt=0
# virtual stigmergy for the LOCK barrier.
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON = 1800 #13.5 the LJ parameter for other robots
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#map from int to state
function i2s(value){
if(value==1){
return "STATE_FREE"
}
else if(value==2){
return "STATE_ASKING"
}
else if(value==3){
return "STATE_JOINING"
}
else if(value==4){
return "STATE_JOINED"
}
else if(value==5){
return "STATE_LOCK"
}
}
#map from state to int
function s2i(value){
if(value=="STATE_FREE"){
return 1
}
else if(value=="STATE_ASKING"){
return 2
}
else if(value=="STATE_JOINING"){
return 3
}
else if(value=="STATE_JOINED"){
return 4
}
else if(value=="STATE_LOCK"){
return 5
}
}
#map form int to response
function i2r(value){
if(value==1){
return "REQ_NONE"
}
else if(value==2){
return "REQ_GRANTED"
}
}
#map from response to int
function r2i(value){
if(value=="REQ_NONE"){
return 1
}
else if(value=="REQ_GRANTED"){
return 2
}
}
#
#return the index of value
#
function find(table,value){
var index=nil
var i=0
while(i<size(table)){
if(table[i]==value)
index=i
i=i+1
}
return index
}
#
#pack message into 1 number
#
function packmessage(send_table){
var send_value
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
return send_value
}
#
#pack guide message into 1 number
#
function pack_guide_msg(send_table){
var send_value
var r_id=send_table.Label#id of target robot
var pon#positive or negative ,0 postive, 1 negative
if(send_table.Bearing>=0){
pon=0
}
else{
pon=1
}
var b=math.abs(send_table.Bearing)
send_value=r_id*1000+pon*100+b
return send_value
}
#
#unpack message
#
function unpackmessage(recv_value){
var wan=(recv_value-recv_value%100000)/100000
recv_value=recv_value-wan*100000
var qian=(recv_value-recv_value%10000)/10000
recv_value=recv_value-qian*10000
var bai=(recv_value-recv_value%1000)/1000
recv_value=recv_value-bai*1000
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
var ge=recv_value
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
return_table.State=wan
return_table.Label=qian
return_table.ReqLabel=bai
return_table.ReqID=shi
return_table.Response=ge
return return_table
}
#
#unpack guide message
#
function unpack_guide_msg(recv_value){
log(id,"I pass value=",recv_value)
var qian=(recv_value-recv_value%1000)/1000
recv_value=recv_value-qian*1000
var bai=(recv_value-recv_value%100)/100
recv_value=recv_value-bai*100
var b=recv_value
var return_table={.Label=0.0,.Bearing=0.0}
return_table.Label=qian
if(bai==1){
b=b*-1.0
}
return_table.Bearing=b
return return_table
}
#get the target distance to neighbr nei_id
function target4label(nei_id){
var return_val="miss"
var i=0
while(i<size(lock_neighbor_id)){
if(lock_neighbor_id[i]==nei_id){
return_val=lock_neighbor_dis[i]
}
i=i+1
}
return return_val
}
#calculate LJ vector for neibhor stored in i
function LJ_vec(i){
var dis=m_MessageRange[i]
var bearing=m_MessageBearing[i]
var nei_id=m_messageID[i]
var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0}
if(target!="miss"){
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)
}
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y)
return cDir
}
#calculate the motion vector
function motion_vector(){
var i=0
var m_vector={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
#calculate and add the motion vector
m_vector=math.vec2.add(m_vector,LJ_vec(i))
#log(id,"x=",m_vector.x,"y=",m_vector.y)
i=i+1
}
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
return m_vector
}
function start_listen(){
neighbors.listen("m",
function(vid,value,rid){
#store the received message
var temp_id=rid
var recv_val=unpackmessage(value)
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
m_MessageLabel[m_neighbourCount]=recv_val.Label
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid
m_neighbourCount=m_neighbourCount+1
})
}
#
#Function used to get the station info of the sender of the message
function Get_DisAndAzi(id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
}
i=i+1
}
}
#
#Transistion to state free
#
function TransitionToFree(){
UAVSTATE="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
UAVSTATE="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
neighbors.listen("r",
function(vid,value,rid){
var recv_table={.Label=0,.Bearing=0.0}
recv_table=unpack_guide_msg(value)
#store the received message
if(recv_table.Label==m_nLabel){
m_cMeToPred.GlobalBearing=recv_table.Bearing
}
})
}
#
#Transistion to state joined
#
function TransitionToJoined(){
UAVSTATE="STATE_JOINED"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r")
#write statues
v_tag.put(m_nLabel, m_lockstig)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Transistion to state Lock, lock the current formation
#
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
UAVSTATE="STATE_LOCK"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]
i=i+1
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=s2i(UAVSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLabels={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper Label
var unFoundLabel=0
# var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLabel==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
unFoundLabel=m_vecNodes[i].Label
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
}
i=i+1
}
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
return
}
#navigation
#if there is a joined robot within sight, move around joined robots
#else, gather with other free robots
if(size(setJoinedIndexes)>0){
var tempvec_P={.x=0.0,.y=0.0}
var tempvec_N={.x=0.0,.y=0.0}
i=0
while(i<size(setJoinedIndexes)){
var index=setJoinedIndexes[i]
tempvec_P=math.vec2.add(tempvec_P,math.vec2.newp(m_MessageRange[index],m_MessageBearing[index]+0.5*math.pi))
tempvec_N=math.vec2.add(tempvec_N,math.vec2.newp(m_MessageRange[index]-5.0*ROBOT_SAFETYDIST,m_MessageBearing[index]))
i=i+1
}
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*2)
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}else{ #no joined robots in sight
i=0
var tempvec={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
i=i+1
}
m_navigation=math.vec2.scale(tempvec,1.0/i)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#jump the first step
if(step_cunt<=1){
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#set message
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCount and psResponse==-1){
#the respond robot in joined state
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
log("get response")
psResponse=i
}}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
else{
log("respond id=",m_MessageReqID[psResponse])
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Do joining
#
function DoJoining(){
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
var S2PGlobalBearing=0
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
if(m_cMeToPred.GlobalBearing>math.pi)
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
else
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
#the vector from self to target in global coordinate
var S2Target=math.vec2.add(S2Pred,P2Target)
#change the vector to local coordinate of self
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
# Limit the mvt
if(math.vec2.length(S2Target)>ROBOT_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, 0.0)
#test if is already in desired position
if(math.vec2.length(S2Target)<m_fTargetDistanceTolerance and S2Target_bearing<m_fTargetAngleTolerance){
log("JOINED! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
TransitionToJoined()
return
}
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
m_unWaitCount=m_unWaitCount-1
}
if(m_unWaitCount==0){
TransitionToFree()
return
}
#pack the communication package
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
}
#
#Do joined
#
function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLabel
var JoiningLabel
var seenPred=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLabel=m_MessageReqLabel[i]
log("ReqLabel var:",ReqLabel)
log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
log("Into if m_nLabel")
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
}
}
#if it is the pred
if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
}
i=i+1
}
#get request
if(size(mapRequests)!=0){
i=1
var ReqIndex=0
while(i<size(mapRequests)){
#compare the distance
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
ReqIndex=i
i=i+1
}
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLabel=ReqLabel
m_selfMessage.ReqID=ReqID
m_selfMessage.Response=r2i("REQ_GRANTED")
m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",ReqLabel)
log("ID=",ReqID)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
#lost pred, wait for some time and transit to free
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
#check if should to transists to lock
#write statues
v_tag.get(m_nLabel)
log(v_tag.size(), " of ", ROBOTS, " ready to lock")
if(v_tag.size()==ROBOTS){
TransitionToLock()
}
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0 #change value so that robot 0 will move
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
}
#move
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
function action(){
statef=action
UAVSTATE="STATE_FREE"
# reset the graph
Reset()
}
#
# Executed at init
#
function init() {
#
# Global parameters for graph formation
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
v_tag = stigmergy.create(m_lockstig)
uav_initstig()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
#
# Executed every step
#
function step() {
# listen to potential RC
uav_rccmd()
# get the swarm commands
uav_neicmd()
# update the vstig (status/net/batt)
uav_updatestig()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#
# graph state machine
#
if(UAVSTATE=="STATE_FREE")
statef=DoFree
else if(UAVSTATE=="STATE_ESCAPE")
statef=DoEscape
else if(UAVSTATE=="STATE_ASKING")
statef=DoAsking
else if(UAVSTATE=="STATE_JOINING")
statef=DoJoining
else if(UAVSTATE=="STATE_JOINED")
statef=DoJoined
else if(UAVSTATE=="STATE_LOCK")
statef=DoLock
# high level UAV state machine
statef()
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
log("Swarm size: ", ROBOTS)
#navigation
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
#
#clean message storage
m_MessageState={}#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_neighbourCount=0
#step cunt+1
step_cunt=step_cunt+1
}
#
# Executed when reset
#
function Reset(){
Read_Graph()
m_nLabel=-1
#start listening
start_listen()
#
#set initial state, only one robot choose [A], while the rest choose [B]
#
#[A]The robot used to triger the formation process is defined as joined,
if(id==ROOT_ID){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroy() {
#clear neighbour message
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")
}

View File

@ -1,269 +0,0 @@
include "vec2.bzz"
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.
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 10.0
#################################################
### UTILITY FUNCTIONS ###########################
#################################################
# Write a table as if it was a matrix
function write_knowledge(k, row, col, val) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
k[key] = val
log("Writing knowledge:", val, " to ", row, " ", col)
}
# Read a table as if it was a matrix
function read_knowledge(k, row, col) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
if (k[key] == nil) {
log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
return -1
} else {
return k[key]
}
}
# Int to String
function itos(i) {
log("Use 'string.tostring(OJB)' instead")
if (i==0) { return "0" }
if (i==1) { return "1" }
if (i==2) { return "2" }
if (i==3) { return "3" }
if (i==4) { return "4" }
if (i==5) { return "5" }
if (i==6) { return "6" }
if (i==7) { return "7" }
if (i==8) { return "8" }
if (i==9) { return "9" }
log("Function 'itos' out of bounds, returning the answer (42)")
return "42"
}
# String to Int
function stoi(s) {
if (s=='0') { return 0 }
if (s=='1') { return 1 }
if (s=='2') { return 2 }
if (s=='3') { return 3 }
if (s=='4') { return 4 }
if (s=='5') { return 5 }
if (s=='6') { return 6 }
if (s=='7') { return 7 }
if (s=='8') { return 8 }
if (s=='9') { return 9 }
log("Function 'stoi' out of bounds, returning the answer (42)")
return 42
}
# Rads to degrees
function rtod(r) {
return (r*(180.0/math.pi))
}
# Degrees to rads
function dtor(d) {
return (math.pi*(d/180.0))
}
# Force angles in the (-180,180) interval
function degrees_interval(a) {
var temp = a
while ((temp>360.0) or (temp<0.0)) {
if (temp > 360.0) {
temp = temp - 360.0
} else if (temp < 0.0){
temp = temp + 360.0
}
}
if (temp > 180.0) {
temp = temp - 360.0
}
return temp
}
# Force angles in the (-pi,pi) interval
function radians_interval(a) {
var temp = a
while ((temp>2.0*math.pi) or (temp<0.0)) {
if (temp > 2.0*math.pi) {
temp = temp - 2.0*math.pi
} else if (temp < 0.0){
temp = temp + 2.0*math.pi
}
}
if (temp > math.pi) {
temp = temp - 2.0*math.pi
}
return temp
}
#################################################
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
#################################################
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
moveto(accum.x, accum.y)
}
function inform_your_neighborhood() {
# Reset to 0 the visibility of all neighbors
foreach(knowledge, function(key, value) {
column = string.sub(key, string.length(key)-1,string.length(key))
if (column=='3') {
knowledge[key] = 0
}
})
neighbors.foreach( function(rid, data) {
# For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
message_id = string.tostring(rid)
neighbors.broadcast(message_id, rtod(data.azimuth))
# Record the neighbor azimuth in my own knowledge table
write_knowledge(knowledge, rid, 0, rtod(data.azimuth))
# Record the neighbor distance in my own knowledge table
write_knowledge(knowledge, rid, 2, data.distance)
# Set neighbor as visible
write_knowledge(knowledge, rid, 3, 1)
})
# Send a message with the desired direction, as seen by the broadcasting robot
neighbors.broadcast("direction", local_dir)
}
function listen_to_your_neighborhood() {
# For all "senders" in my neighborhood, record my azimuth, as seen by them
message_id = string.tostring(id)
neighbors.listen(message_id, function(vid, value, rid) {
write_knowledge(knowledge, rid, 1, value)
})
}
#################################################
### ACTUAL CONTROLLERS ##########################
#################################################
function zero() {
# Do not move
moveto(0.0,0.0)
# Tell the neighbors of the center where to go
inform_your_neighborhood()
}
function onetwo() {
if (id == 1) {
angle = 45
} else {
angle = -135
}
# Broadcast information
inform_your_neighborhood()
# If the center 0 is in sight
if (read_knowledge(knowledge, 0, 3) == 1) {
arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
if (arm_offset<3 and arm_offset>(-3)) {
hexagon() # Underlying Lennard-Jones potential behavior
} else {
local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
local_arm = degrees_interval(angle - local_rotation)
if (read_knowledge(knowledge, 0, 2) > 250.0) {
x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
} else if (read_knowledge(knowledge, 0, 2) < 30.0) {
x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
} else {
spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
if (arm_offset > 0) { # Clockwise
x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
} else { # Counterclockwise
x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
}
}
speed = 100
moveto(speed * x_mov,speed * y_mov)
}
} else {
hexagon()
}
}
#################################################
### BUZZ FUNCTIONS ##############################
#################################################
function action(){
if (id == 0)
statef=zero
else
statef=onetwo
UAVSTATE="TENTACLES"
}
# Executed at init time
function init() {
uav_initswarm()
# Local knowledge table
knowledge = {}
# Update local knowledge with information from the neighbors
listen_to_your_neighborhood()
# Variables initialization
iteration = 0
}
# Executed every time step
function step() {
uav_rccmd()
uav_neicmd()
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
# Count the number of steps
iteration = iteration + 1
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Execute at exit
function destroy() {
}

View File

@ -1,119 +0,0 @@
include "vec2.bzz"
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 "vstigenv.bzz"
TARGET_ALTITUDE = 10.0
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 14.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
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() {
statef=action
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
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))
}
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# UAVSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# } else {
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# }
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
uav_initswarm()
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
}
# Executed at each time step.
function step() {
uav_rccmd()
uav_neicmd()
statef()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
# Read a value from the structure
# 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")
users_save(vt.get("p"))
table_print(users.dataL)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

View File

@ -1,537 +0,0 @@
# Include files
include "string.bzz"
include "vec2.bzz"
include "utilities.bzz"
include "barrier.bzz"
############################################
# Global variables
RANGE = 200 # rab range in cm, get from argos file
N_SONS = 10 # Maximum number of sons
TRIGGER_VSTIG = 101 # ID of trigger virtual stigmergy
BARRIER_VSTIG = 102 # ID of barrier virtual stigmergy
################################################################
################################################################
# Tree utility functions
function parent_select() {
# Selects potential parents
var candidates = {}
candidates = neighbors.filter(function(rid, data) {
return ((knowledge.level[rid] > 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))})
# and (data.distance > 10.0)
# Selects closest parent candidate as parent
var temp = {}
temp = candidates.reduce(function(rid, data, accum) {
accum.min = math.min(accum.min, data.distance)
return accum
}, {.min = RANGE})
var min = temp.min
var flag = 0
foreach(knowledge.distance, function(key, value) {
if ((flag == 0) and (candidates.data[key] != nil)) {
if (value == min) {
tree.parent.id = key
tree.parent.distance = value
tree.parent.azimuth = knowledge.azimuth[key]
flag = 1
}
}
#break (when implemented)
})
}
####################################
function count() {
if (nb_sons == 0) {
eq_level = level
}
else if (nb_sons >= 1) {
var temp = {}
temp = sons.reduce(function(rid, data, accum) {
accum.sum = accum.sum + tree.sons[rid].eq_level
return accum
}, {.sum = 0})
eq_level = temp.sum - (nb_sons - 1) * level
}
}
####################################
function change_frame(p01, p1, theta) {
var p0 = {
.x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x,
.y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y
}
return p0
}
####################################
transform_accum = function(rid, data) {
# Son contribution in frame F1
var p1 = {
.x = tree.sons[rid].accum_x,
.y = tree.sons[rid].accum_y
}
# Rotation angle between F0 and F1
var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi
# Translation vector from F0 to F1
var p01 = {
.x = 0,
.y = 0
}
var p0 = {}
if (tree.sons[rid].angle_parent != nil) {
var rot_angle = radians_interval(theta)
p0 = change_frame(p01, p1, rot_angle)
}
else {
p0.x = p1.x
p0.y = p1.y
}
return p0
}
####################################
function centroid() {
# If the robot has a parent
if ((tree.parent != nil) or (root == 1)) {
var sum_F1 = { .x = 0, .y = 0}
# If the robot has at least one son
if (nb_sons > 0) {
var temp = {}
# Expresses son contrib (in F1) in its own reference frame (F0)
temp = sons.map(transform_accum)
# Sums son contributions expressed in F0 frame
sum_F1 = temp.reduce(function(rid, data, accum) {
accum.x = accum.x + data.x
accum.y = accum.y + data.y
#log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y)
return accum
}, {.x = 0, .y = 0})
}
# If the robot is not the root
if ((root == 0) and (tree.parent.id != nil)) {
#var nb_descendants = eq_level - level
var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance
var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth
# Adds current robot contribution to centroid sum
accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta)
accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta)
}
# If the robot is the root
else if ((root == 1) and (robot_count != 0)) {
# Centroid coordinates in root ref frame
accum_x = sum_F1.x / robot_count
accum_y = sum_F1.y / robot_count
}
}
}
################################################################
################################################################
# Tree reconfiguration functions
function tree_config() {
statef()
}
function end_fun() {
if (root == 1) {
log("centroid X: ", accum_x, " Y ", accum_y)
}
}
####################################
function root_select() {
log(id," root_select")
if (tree.parent.id != nil) {
if(neighbors.data[tree.parent.id] != nil) {
angle_parent = neighbors.data[tree.parent.id].azimuth
}
}
if (root == 1) {
# Listens for new root acknowledgment
foreach(knowledge.ackn, function(key, value) {
if (value == better_root) {
#log(id, " got it")
root = 0
level = 0
setleds(0,0,0)
}
})
if (ack == 1) {
# Triggers transition to new state
trigger.put("a", 1)
}
else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) {
setleds(255,0,0)
better_root = id
# Centroid position in root reference frame (F0)
var c0 = math.vec2.new(accum_x, accum_y)
# Distance from current root to centroid
var dist_rc = math.vec2.length(c0)
#log("root ", id, " dist_centr ", dist_rc)
# Distances from neighbors to centroid
var dist_centroid = {}
dist_centroid = neighbors.map(function(rid, data) {
# Neighbour position in frame F0
var p0 = math.vec2.newp(data.distance, data.azimuth)
# Difference vector between p0 and c0
var v = math.vec2.sub(p0, c0)
# Distance between robot and centroid
return math.vec2.length(v)
})
# Minimal distance to centroid
var temp = {}
temp = dist_centroid.reduce(function(rid, data, accum) {
accum.min = math.min(accum.min, data)
return accum
}, {.min = dist_rc})
var min = temp.min
#log("min ", min)
# If the current root is the closest to the centroid
if(dist_rc == min) {
ack = 1
}
# Otherwise
else {
var flag = 0
# Selects closest robot to centroid as better root
foreach(dist_centroid.data, function(key, value) {
if(flag == 0) {
if(value == min) {
better_root = key
flag = 1
}
# break (when implemented)
}
})
#log(id, " better root : ", better_root)
#log("X : ", accum_x, "Y : ", accum_y)
var angle = neighbors.data[better_root].azimuth
# Broadcasts
var message = {
.better_root = better_root,
.centroid_x = accum_x,
.centroid_y = accum_y,
.angle_old_root = angle
}
neighbors.broadcast("msg1", message)
}
}
}
else if (better_root == nil) {
# Listen for old root message
foreach(knowledge.better_root, function(rid, value) {
if(value == id) {
var theta = neighbors.data[rid].azimuth
var p1 = {
.x = knowledge.centroid_x[rid],
.y = knowledge.centroid_y[rid]
}
var p01 = {
.x = neighbors.data[rid].distance * math.cos(theta),
.y = neighbors.data[rid].distance * math.sin(theta)
}
var p0 = {}
if (knowledge.angle_old_root[rid] != nil) {
var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi)
p0 = change_frame(p01, p1, rot_angle)
}
else {
p0.x = p1.x
p0.y = p1.y
}
accum_x = p0.x
accum_y = p0.y
centroid_x = accum_x
centroid_y = accum_y
root = 1
neighbors.broadcast("got_it", id)
}
})
}
# Transitions to new state when all robots are ready
if (trigger.get("a") == 1) {
barrier_set(ROBOTS, end_fun)
barrier_ready()
}
}
####################################
function tree_select() {
log(id, " tree_select")
neighbors.map(function(rid, data) {
knowledge.distance[rid] = data.distance
knowledge.azimuth[rid] = data.azimuth
return 1
})
if (level == 0) {
# Finds a parent
parent_select()
# Updates robot level
if (tree.parent.id != nil) {
#log(" ")
#log("selected")
#log("son ", id)
#log("parent ", tree.parent.id)
#log(" ")
level = knowledge.level[tree.parent.id] + 1
angle_parent = neighbors.data[tree.parent.id].azimuth
}
}
else {
# Updates list of sons
foreach(knowledge.parent, function(key, value) {
if(value == id) {
#log(value)
if(tree.sons[key] == nil) {
# Updates robot nb_sons
nb_sons = nb_sons + 1
# Updates robot free status
if (nb_sons >= N_SONS) {
free = 0
}
}
tree.sons[key] = {
.distance = knowledge.distance[key],
.azimuth = knowledge.azimuth[key],
.eq_level = knowledge.eq_level[key],
.accum_x = knowledge.accum_x[key],
.accum_y = knowledge.accum_y[key],
.angle_parent = knowledge.angle_parent[key]
}
}
})
}
# Creates a subset of neighbors to get the sons
# and parent
sons = {}
sons = neighbors.filter(function(rid, data) {
return (tree.sons[rid] != nil)
})
parent = {}
parent = neighbors.filter(function(rid, data) {
return (tree.parent.id == rid)
})
# Counts robots (updates eq_level)
count()
# Updates count of robots in the tree
if (root == 1) {
robot_count = eq_level
}
nb_descendants = eq_level - level
# Computes centroid (updates accum_x, accum_y)
centroid()
# Broadcast information to other robots
var message = {
.level = level,
.parent = tree.parent.id,
.eq_level = eq_level,
.free = free,
.accum_x = accum_x,
.accum_y = accum_y,
.angle_parent = angle_parent
}
neighbors.broadcast("msg2", message)
# Triggers transition to new state if root count = ROBOTS
if (root == 1) {
if(robot_count == ROBOTS) {
log("centroid X: ", accum_x, " Y ", accum_y)
trigger.put("b", 1)
}
}
# Transitions to new state when all robots are ready
if (trigger.get("b") == 1) {
barrier_set(ROBOTS, root_select)
barrier_ready()
}
}
################################################################
################################################################
# This function is executed every time you press the 'execute' button
function init() {
trigger = stigmergy.create(TRIGGER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
# Trees
old_tree = {}
tree = old_tree
old_tree.parent = {}
old_tree.sons = {}
# Boolean variables
root = 0 # Root status
free = 1 # Node status (true if accepts sons)
ack = 0
# Tree variables
level = 0
eq_level = 0
nb_sons = 0
nb_descendants = 0
# Update root status
if (id == 0) {
root = 1 # True if robot is the ro ot
level = 1
robot_count = 0
}
statef = tree_select
knowledge = {
.level = {},
.parent = {},
.eq_level = {},
.free = {},
.accum_x = {},
.accum_y = {},
.angle_parent = {},
.distance = {},
.azimuth = {},
.better_root = {},
.centroid_x = {},
.centroid_y = {},
.angle_old_root = {},
.ackn = {}
}
# Broadcast information to other robots
var message = {
.level = level,
.parent = old_tree.parent.id,
.eq_level = eq_level,
.free = free,
.accum_x = accum_x,
.accum_y = accum_y,
.angle_parent = 0.0
}
neighbors.broadcast("msg2", message)
# Listen to information from other robots for root_select
neighbors.listen("msg1", function(vid, value, rid) {
knowledge.better_root[rid] = value.better_root
knowledge.centroid_x[rid] = value.centroid_x
knowledge.centroid_y[rid] = value.centroid_y
knowledge.angle_old_root[rid] = value.angle_old_root
knowledge.angle_parent[rid] = value.angle_parent
})
# Listen to information from other robots for tree_select
neighbors.listen("msg2", function(vid, value, rid) {
knowledge.level[rid] = value.level
knowledge.parent[rid] = value.parent
knowledge.eq_level[rid] = value.eq_level
knowledge.free[rid] = value.free
knowledge.accum_x[rid] = value.accum_x
knowledge.accum_y[rid] = value.accum_y
knowledge.angle_parent[rid] = value.angle_parent
})
neighbors.listen("got_it", function(vid, value, rid) {
knowledge.ackn[rid] = value
})
}
############################################
# This function is executed at each time step
function step() {
tree_config()
goal = math.vec2.new(0.0, 0.0)
}
############################################
# This function is executed every time you press the 'reset'
# button in the GUI.
function reset() {
# put your code here
}
############################################
# This function is executed only once, when the robot is removed
# from the simulation
function destroy() {
# put your code here
}
################

View File

@ -93,7 +93,7 @@ Service
------- -------
* Remote Controller: * Remote Controller:
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line.
References References
------ ------