added potential control in webcontrol
This commit is contained in:
dave
parent
18360d3bfd
commit
02f76b8f67
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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})
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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 ?
|
||||
|
|
|
|||
Loading…
Reference in New Issue