diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev1.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev2.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev3.bzz b/script/update_sim/testflockfev3.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev3.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev4.bzz b/script/update_sim/testflockfev4.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev4.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev5.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev6.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev7.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# 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() { + statef=hexagon + CURSTATE = "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 + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 152f3e9..c86e7d5 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -19,8 +19,8 @@ namespace buzz_utility{ static buzzdebug_t DBG_INFO = 0; static uint8_t MSG_SIZE = 250; // Only 100 bytes of Buzz messages every step static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets - static int Robot_id = 0; - + static int Robot_id = 0; + static std::vector IN_MSG; std::map< int, Pos_struct> users_map; /****************************************/ @@ -173,41 +173,55 @@ namespace buzz_utility{ void in_msg_append(uint64_t* payload){ - /* Go through messages and add them to the FIFO */ + /* Go through messages and append them to the vector */ uint16_t* data= u64_cvt_u16((uint64_t)payload[0]); /*Size is at first 2 bytes*/ uint16_t size=data[0]*sizeof(uint64_t); delete[] data; uint8_t* pl =(uint8_t*)malloc(size); - memset(pl, 0,size); - /* Copy packet into temporary buffer */ - memcpy(pl, payload ,size); - /*size and robot id read*/ - size_t tot = sizeof(uint32_t); - /* Go through the messages until there's nothing else to read */ - uint16_t unMsgSize=0; - - /*Obtain Buzz messages only when they are present*/ - do { - /* Get payload size */ - unMsgSize = *(uint16_t*)(pl + tot); - tot += sizeof(uint16_t); - /* Append message to the Buzz input message queue */ - if(unMsgSize > 0 && unMsgSize <= size - tot ) { - buzzinmsg_queue_append(VM, - buzzmsg_payload_frombuffer(pl +tot, unMsgSize)); - tot += unMsgSize; - } - }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - free(pl); + /* Copy packet into temporary buffer */ + memcpy(pl, payload ,size); + IN_MSG.push_back(pl); } + + 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]); + /*Size is at first 2 bytes*/ + uint16_t size=data[0]*sizeof(uint64_t); + delete[] data; + /*size and robot id read*/ + size_t tot = sizeof(uint32_t); + /* Go through the messages until there's nothing else to read */ + uint16_t unMsgSize=0; + /*Obtain Buzz messages push it into queue*/ + do { + /* Get payload size */ + unMsgSize = *(uint16_t*)(first_INmsg + tot); + tot += sizeof(uint16_t); + /* Append message to the Buzz input message queue */ + if(unMsgSize > 0 && unMsgSize <= size - tot ) { + buzzinmsg_queue_append(VM, + 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); + } + /* Process messages VM call*/ + buzzvm_process_inmsgs(VM); + } /***************************************************/ /*Obtains messages from buzz out message Queue*/ /***************************************************/ uint64_t* obt_out_msg(){ - + /* Process out messages */ + buzzvm_process_outmsgs(VM); uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE); /*Taking into consideration the sizes included at the end*/ @@ -657,8 +671,8 @@ static int create_stig_tables() { } void buzz_script_step() { - /* Process messages */ - buzzvm_process_inmsgs(VM); + /*Process available messages*/ + in_message_process(); /*Update sensors*/ update_sensors(); /* Call Buzz step() function */ @@ -668,8 +682,7 @@ static int create_stig_tables() { buzz_error_info()); buzzvm_dump(VM); } - /* Process out messages */ - buzzvm_process_outmsgs(VM); + /*Print swarm*/ //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;