minor fixes and fixed pursuit (vector field guidance)
This commit is contained in:
parent
a1d13d312a
commit
2f25df02f0
|
@ -108,26 +108,31 @@ function aggregate() {
|
||||||
return centroid
|
return centroid
|
||||||
}, {.x=0, .y=0})
|
}, {.x=0, .y=0})
|
||||||
if(neighbors.count() > 0)
|
if(neighbors.count() > 0)
|
||||||
math.vec2.scale(centroid, 1.0 / neighbors.count())
|
centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
|
||||||
cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS))
|
cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
|
||||||
cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
|
cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
|
||||||
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
|
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
|
||||||
}
|
}
|
||||||
|
|
||||||
# follow one another
|
# follow one another
|
||||||
rotang = 0.0
|
|
||||||
function pursuit() {
|
function pursuit() {
|
||||||
BVMSTATE="PURSUIT"
|
BVMSTATE="PURSUIT"
|
||||||
centroid = neighbors.reduce(function(rid, data, centroid) {
|
rd = 15.0
|
||||||
centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
|
pc = 3.2
|
||||||
return centroid
|
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})
|
}, {.x=0, .y=0})
|
||||||
if(neighbors.count() > 0)
|
if(neighbors.count() > 0)
|
||||||
math.vec2.scale(centroid, 1.0 / neighbors.count())
|
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
|
||||||
rotang = rotang + math.pi/50.0
|
r = math.vec2.length(r_vec)
|
||||||
cmdbin = math.vec2.sub(centroid, math.vec2.newp(80.0, id * 2 * math.pi / ROBOTS + rotang))
|
gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
|
||||||
cmdbin = LimitSpeed(cmdbin, 1.0/5.0)
|
dr = -gamma * (r-rd)
|
||||||
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
|
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
|
# Lennard-Jones interaction magnitude
|
||||||
|
@ -135,7 +140,6 @@ TARGET = 8.0
|
||||||
EPSILON = 3.0
|
EPSILON = 3.0
|
||||||
function lj_magnitude(dist, target, epsilon) {
|
function lj_magnitude(dist, target, epsilon) {
|
||||||
lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
|
lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
|
||||||
# log(dist, (target / dist), (epsilon / dist), lj)
|
|
||||||
return lj
|
return lj
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -156,7 +160,7 @@ function formation() {
|
||||||
# Calculate accumulator
|
# Calculate accumulator
|
||||||
accum_lj = 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)
|
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)
|
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)
|
goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
|
||||||
}
|
}
|
||||||
|
|
|
@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
|
||||||
include "vstigenv.bzz"
|
include "vstigenv.bzz"
|
||||||
|
|
||||||
#State launched after takeoff
|
#State launched after takeoff
|
||||||
AUTO_LAUNCH_STATE = "AGGREGATE"
|
AUTO_LAUNCH_STATE = "PURSUIT"
|
||||||
|
|
||||||
#####
|
#####
|
||||||
# Vehicule type:
|
# Vehicule type:
|
||||||
|
|
Loading…
Reference in New Issue