Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor fixes and fixed pursuit (vector field guidance)

This commit is contained in:
dave 2018-03-05 00:10:18 -05:00
parent 21ee166028
commit b145574d6d
2 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
buzz_scripts/include/act/states.bzz
View File

@ -108,26 +108,31 @@ function aggregate() {
return centroid
}, {.x=0, .y=0})
if(neighbors.count() > 0)
math.vec2.scale(centroid, 1.0 / neighbors.count())
cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS))
centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.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"
centroid = neighbors.reduce(function(rid, data, centroid) {
centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
return centroid
rd = 15.0
pc = 3.2
vd = 15.0
r_vec = neighbors.reduce(function(rid, data, r_vec) {
r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth))
return r_vec
}, {.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)
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
r = math.vec2.length(r_vec)
gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
dr = -gamma * (r-rd)
dT = gamma * pc
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
vfg = LimitSpeed(vfg, 2.0)
goto_abs(vfg.x, vfg.y, 0.0, 0.0)
}
# Lennard-Jones interaction magnitude
@ -135,7 +140,6 @@ TARGET = 8.0
EPSILON = 3.0
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
# log(dist, (target / dist), (epsilon / dist), lj)
return lj
}
@ -156,7 +160,7 @@ function formation() {
# Calculate accumulator
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum_lj, 1.0 / neighbors.count())
accum_lj = 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)
}

2
buzz_scripts/main.bzz
View File

@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "AGGREGATE"
AUTO_LAUNCH_STATE = "PURSUIT"
#####
# Vehicule type: