From 21ee1660284718a0720632079febca0556d9f7a5 Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 4 Mar 2018 21:44:39 -0500 Subject: [PATCH] fixed formation and enhanced aggregation --- buzz_scripts/include/act/states.bzz | 59 ++++++++----------- .../include/taskallocate/graphformGPS.bzz | 11 +--- buzz_scripts/include/utils/conversions.bzz | 6 ++ buzz_scripts/main.bzz | 2 +- 4 files changed, 35 insertions(+), 43 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index fa6cffc..3e27726 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -77,13 +77,12 @@ function goto_gps(transf) { print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) - } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() - else + else { + LimitSpeed(m_navigation, 1.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + } } function follow() { @@ -110,40 +109,34 @@ function aggregate() { }, {.x=0, .y=0}) if(neighbors.count() > 0) math.vec2.scale(centroid, 1.0 / neighbors.count()) - if(math.vec2.length(centroid)>GOTO_MAXVEL) - centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid)) - goto_abs(centroid.x, centroid.y, 0.0, 0.0) + cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS)) + cmdbin = LimitSpeed(cmdbin, 1.0/2.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # follow one another rotang = 0.0 function pursuit() { BVMSTATE="PURSUIT" - insight = 0 - leader = math.vec2.newp(0.0, 0.0) - var cmdbin = math.vec2.newp(0.0, 0.0) - neighbors.foreach(function(rid, data) { - if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) { - insight = 1 - leader = math.vec2.newp(data.distance, data.azimuth) - } - }) - if(insight == 1) { - log("Leader in sight !") - #cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader)) - cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader)) - } else { - rotang = rotang + math.pi/60 - cmdbin = math.vec2.newp(2.0, rotang) - } - goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang) + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + math.vec2.scale(centroid, 1.0 / neighbors.count()) + rotang = rotang + math.pi/50.0 + cmdbin = math.vec2.sub(centroid, math.vec2.newp(80.0, id * 2 * math.pi / ROBOTS + rotang)) + cmdbin = LimitSpeed(cmdbin, 1.0/5.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 0.000001 +EPSILON = 3.0 function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + # log(dist, (target / dist), (epsilon / dist), lj) + return lj } # Neighbor data to LJ interaction vector @@ -157,15 +150,15 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction +old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + accum_lj = 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()) - if(math.vec2.length(accum)>GOTO_MAXVEL*10) - accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum)) - goto_abs(accum.x, accum.y, 0.0, 0.0) + math.vec2.scale(accum_lj, 1.0 / neighbors.count()) + old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) + goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) } function rc_cmd_listen() { diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 826ee01..61e77fd 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -78,14 +78,7 @@ m_fTargetDistanceTolerance=0 m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 4000 #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 -} +EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots # #return the number of value in table @@ -264,7 +257,7 @@ function LJ_vec(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),ljbearing) + cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) } #log(id,"dis=",dis,"target=",target,"label=",nei_id) #log("x=",cDir.x,"y=",cDir.y) diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 144cabf..50a58bc 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -7,6 +7,12 @@ function LimitAngle(angle){ return angle } +function LimitSpeed(vel_vec, factor){ + if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) + vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) + return vel_vec +} + # TODO: add other conversions.... function convert_path(P) { var pathR={} diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 7191155..1927d66 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "AGGREGATE" ##### # Vehicule type: