From 02f76b8f67eab43d655ac008df19f681410a6238 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 6 Nov 2018 04:01:16 -0500 Subject: [PATCH] added potential control in webcontrol --- buzz_scripts/include/act/barrier.bzz | 18 ++--- buzz_scripts/include/act/neighborcomm.bzz | 61 +++++++++++--- buzz_scripts/include/act/states.bzz | 79 ++++++++++--------- buzz_scripts/include/utils/table.bzz | 6 +- .../include/utils/takeoff_heights.bzz | 16 ++-- buzz_scripts/main.bzz | 6 +- 6 files changed, 114 insertions(+), 72 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index eb6398d..6f1588b 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -63,14 +63,14 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier.put(id, bc) barrier.get(id) log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) - if(barrier.size()-1 >= threshold or barrier.get("d") == 1) { + #if(barrier.size()-1 >= threshold or barrier.get("d") == 1) { if(barrier_allgood(barrier,bc)) { barrier.put("d", 1) timeW = 0 BVMSTATE = transf } else barrier.put("d", 0) - } + #} if(timeW >= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") @@ -86,13 +86,13 @@ function barrier_wait(threshold, transf, resumef, bc) { # Check all members state function barrier_allgood(barrier, bc) { barriergood = 1 - barrier.foreach( - function(key, value, robot){ - #log("VS entry : ", key, " ", value, " ", robot) - if(value != bc and key != "d"){ + barrier.foreach(function(key, value, robot){ + log("VS entry : ", key, " ", value, " ", robot) + if(key == "d"){ + if(value == 1) + return 1 + } else if(value != bc) barriergood = 0 - } - } - ) + }) return barriergood } \ No newline at end of file diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 7ed6c07..91b0379 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -50,15 +50,22 @@ function rc_cmd_listen() { } else if (flight.rc_cmd==902){ flight.rc_cmd=0 destroyGraph() - barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902) barrier_ready(902) neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 destroyGraph() - barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + resetWP() + barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) barrier_ready(903) neighbors.broadcast("cmd", 903) + } else if (flight.rc_cmd==904){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "IDLE", BVMSTATE, 904) + barrier_ready(904) + neighbors.broadcast("cmd", 904) } } @@ -85,22 +92,28 @@ function nei_cmd_listen() { }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) - neighbors.broadcast("cmd", 900) - } else if(value==901){ # Pursuit + #neighbors.broadcast("cmd", 900) + } else if(value==901 and BVMSTATE!="BARRIERWAIT"){ # Pursuit destroyGraph() barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) #barrier_ready(901) - neighbors.broadcast("cmd", 901) - } else if(value==902){ # Agreggate + #neighbors.broadcast("cmd", 901) + } else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # Waypoint destroyGraph() - barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902) #barrier_ready(902) - neighbors.broadcast("cmd", 902) - } else if(value==903){ # Formation + #neighbors.broadcast("cmd", 902) + } else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){ # Formation destroyGraph() - barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + resetWP() + barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) #barrier_ready(903) - neighbors.broadcast("cmd", 903) + #neighbors.broadcast("cmd", 903) + } else if(value==904 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="IDLE"){ # idle + destroyGraph() + barrier_set(ROBOTS, "IDLE", BVMSTATE, 904) + #barrier_ready(904) + #neighbors.broadcast("cmd", 904) } else if(value==16 and BVMSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) @@ -110,3 +123,29 @@ function nei_cmd_listen() { #} }) } + +firsttimeinwp = 1 +function check_rc_wp() { + if(firsttimeinwp) { + v_wp = stigmergy.create(WP_STIG) + storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) + firsttimeinwp = 0 + } + if(rc_goto.id != -1) { + if(rc_goto.id == id) { + wpreached = 0 + storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) + return + } else { + var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0) + v_wp.put(rc_goto.id,ls) + reset_rc() + } + } +} + +function resetWP() { + firsttimeinwp = 1 + if(v_wp!=nil) + v_wp.foreach(function(key, value, robot){v_wp[key]=nil}) +} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 0fb8cbf..2f18024 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -97,40 +97,23 @@ function stop() { } # State functions for individual waypoint control -firsttimeinwp = 1 + wpreached = 1 function indiWP() { - if(firsttimeinwp) { - v_wp = stigmergy.create(WP_STIG) - storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) - firsttimeinwp = 0 - } BVMSTATE = "INDIWP" - if(rc_goto.id != -1) { - if(rc_goto.id == id) { - wpreached = 0 - storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) - return - } else { - var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0) - v_wp.put(rc_goto.id,ls) - reset_rc() - } - } + check_rc_wp() - #if(vstig_buzzy == 0) { - wpi = v_wp.get(id) - if(wpi!=nil) { - wp = unpackWP2i(wpi) - if(wp.pro == 0) { - wpreached = 0 - storegoal(wp.lat, wp.lon, pose.position.altitude) - var ls = packWP2i(wp.lat, wp.lon, 1) - v_wp.put(id,ls) - return - } - } - #} + wpi = v_wp.get(id) + if(wpi!=nil) { + wp = unpackWP2i(wpi) + if(wp.pro == 0) { + wpreached = 0 + storegoal(wp.lat, wp.lon, pose.position.altitude) + var ls = packWP2i(wp.lat, wp.lon, 1) + v_wp.put(id,ls) + return + } + } if(wpreached!=1) goto_gps(indiWP_done) @@ -138,7 +121,7 @@ function indiWP() { } function indiWP_done() { - BVMSTATE = "INDIWP" + BVMSTATE = "WAYPOINT" wpreached = 1 } @@ -208,10 +191,12 @@ function pursuit() { } # Lennard-Jones interaction magnitude -TARGET = 8.0 -EPSILON = 30.0 +TARGET = 16.0 +EPSILON = 30.0 #30 +GAIN_ATT = 50.0 +GAIN_REP = 30.0 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4)# - (target / dist)^2) #repulse only to avoid each other return lj } @@ -225,14 +210,32 @@ function lj_sum(rid, data, accum) { return math.vec2.add(data, accum) } -# Sate function that calculates and actuates the flocking interaction -function formation() { - BVMSTATE="FORMATION" +# State function that calculates and actuates LJ flocking interaction with vstig targets (attractor/repulsors) +function lennardjones() { + BVMSTATE="POTENTIAL" + check_rc_wp() # Calculate accumulator accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) - accum_lj = LimitSpeed(accum_lj, 1.0/3.0) + # Add attractor/repulsor effects + log(v_wp.size()) + accum_t = math.vec2.new(0.0, 0.0); + v_wp.foreach( + function(key, value, robot){ + wp = unpackWP2i(value) + dvec = vec_from_gps(wp.lat, wp.lon, 0) + Tdist = math.vec2.length(dvec) + if(key > 99 and Tdist < 40) + accum_t = math.vec2.sub(accum_t, math.vec2.newp(GAIN_REP*(TARGET/Tdist)^4, math.vec2.angle(dvec))) + else if(key > 49 and Tdist > 10) + accum_t = math.vec2.add(accum_t, math.vec2.newp(GAIN_ATT*(TARGET/Tdist)^2, math.vec2.angle(dvec))) + }) + if(v_wp.size() > 0) + accum_t = math.vec2.scale(accum_t, 1.0 / v_wp.size()) + #log(math.vec2.length(accum_t),math.vec2.length(accum_lj)) + + accum_lj = LimitSpeed(math.vec2.add(accum_t,accum_lj), 1.0) #1/3 goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } diff --git a/buzz_scripts/include/utils/table.bzz b/buzz_scripts/include/utils/table.bzz index ed19a6d..36c9a8a 100644 --- a/buzz_scripts/include/utils/table.bzz +++ b/buzz_scripts/include/utils/table.bzz @@ -55,7 +55,7 @@ function i2s(value){ return "LAUNCH" } else if(value==9){ - return "STOP" + return "FORMATION" } else { return "UNDETERMINED" @@ -80,7 +80,7 @@ function s2i(value){ else if(value=="BARRIERWAIT"){ return 5 } - else if(value=="INDIWP"){ + else if(value=="WAYPOINT"){ return 6 } else if(value=="TASK_ALLOCATE"){ @@ -89,7 +89,7 @@ function s2i(value){ else if(value=="LAUNCH"){ return 8 } - else if(value=="STOP"){ + else if(value=="POTENTIAL"){ return 9 } else diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index 693c730..5e760d8 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -1,12 +1,12 @@ takeoff_heights ={ - .0 = 30.0, - .1 = 21.0, - .2 = 18.0, - .3 = 12.0, - .4 = 28.0, - .5 = 12.0, - .6 = 3.0, - .9 = 15.0, + .0 = 0.0, + .1 = 32.0, + .2 = 28.0, + .3 = 24.0, + .4 = 20.0, + .5 = 16.0, + .6 = 12.0, + .9 = 8.0, .11 = 6.0, .14 = 18.0, .16 = 9.0, diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 84db2d9..5f2d9c1 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "INDIWP" +AUTO_LAUNCH_STATE = "IDLE" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 @@ -78,8 +78,8 @@ function step() { statef=idle else if(BVMSTATE=="AGGREGATE") statef=aggregate - else if(BVMSTATE=="FORMATION") - statef=formation + else if(BVMSTATE=="POTENTIAL") + statef=lennardjones else if(BVMSTATE=="PURSUIT") statef=pursuit else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?