added potential control in webcontrol

This commit is contained in:
dave 2018-11-06 04:01:16 -05:00
parent 18360d3bfd
commit 02f76b8f67
6 changed files with 114 additions and 72 deletions

View File

@ -63,14 +63,14 @@ function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, bc) barrier.put(id, bc)
barrier.get(id) barrier.get(id)
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) 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)) { if(barrier_allgood(barrier,bc)) {
barrier.put("d", 1) barrier.put("d", 1)
timeW = 0 timeW = 0
BVMSTATE = transf BVMSTATE = transf
} else } else
barrier.put("d", 0) barrier.put("d", 0)
} #}
if(timeW >= BARRIER_TIMEOUT) { if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!") log("------> Barrier Timeout !!!!")
@ -86,13 +86,13 @@ function barrier_wait(threshold, transf, resumef, bc) {
# Check all members state # Check all members state
function barrier_allgood(barrier, bc) { function barrier_allgood(barrier, bc) {
barriergood = 1 barriergood = 1
barrier.foreach( barrier.foreach(function(key, value, robot){
function(key, value, robot){ log("VS entry : ", key, " ", value, " ", robot)
#log("VS entry : ", key, " ", value, " ", robot) if(key == "d"){
if(value != bc and key != "d"){ if(value == 1)
return 1
} else if(value != bc)
barriergood = 0 barriergood = 0
} })
}
)
return barriergood return barriergood
} }

View File

@ -50,15 +50,22 @@ function rc_cmd_listen() {
} else if (flight.rc_cmd==902){ } else if (flight.rc_cmd==902){
flight.rc_cmd=0 flight.rc_cmd=0
destroyGraph() destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
barrier_ready(902) barrier_ready(902)
neighbors.broadcast("cmd", 902) neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){ } else if (flight.rc_cmd==903){
flight.rc_cmd=0 flight.rc_cmd=0
destroyGraph() destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
barrier_ready(903) barrier_ready(903)
neighbors.broadcast("cmd", 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 }else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900) #barrier_ready(900)
neighbors.broadcast("cmd", 900) #neighbors.broadcast("cmd", 900)
} else if(value==901){ # Pursuit } else if(value==901 and BVMSTATE!="BARRIERWAIT"){ # Pursuit
destroyGraph() destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
#barrier_ready(901) #barrier_ready(901)
neighbors.broadcast("cmd", 901) #neighbors.broadcast("cmd", 901)
} else if(value==902){ # Agreggate } else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # Waypoint
destroyGraph() destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
#barrier_ready(902) #barrier_ready(902)
neighbors.broadcast("cmd", 902) #neighbors.broadcast("cmd", 902)
} else if(value==903){ # Formation } else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){ # Formation
destroyGraph() destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
#barrier_ready(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"){ } else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) { # neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", 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})
}

View File

@ -97,40 +97,23 @@ function stop() {
} }
# State functions for individual waypoint control # State functions for individual waypoint control
firsttimeinwp = 1
wpreached = 1 wpreached = 1
function indiWP() { function indiWP() {
if(firsttimeinwp) {
v_wp = stigmergy.create(WP_STIG)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0
}
BVMSTATE = "INDIWP" BVMSTATE = "INDIWP"
if(rc_goto.id != -1) { check_rc_wp()
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()
}
}
#if(vstig_buzzy == 0) { wpi = v_wp.get(id)
wpi = v_wp.get(id) if(wpi!=nil) {
if(wpi!=nil) { wp = unpackWP2i(wpi)
wp = unpackWP2i(wpi) if(wp.pro == 0) {
if(wp.pro == 0) { wpreached = 0
wpreached = 0 storegoal(wp.lat, wp.lon, pose.position.altitude)
storegoal(wp.lat, wp.lon, pose.position.altitude) var ls = packWP2i(wp.lat, wp.lon, 1)
var ls = packWP2i(wp.lat, wp.lon, 1) v_wp.put(id,ls)
v_wp.put(id,ls) return
return }
} }
}
#}
if(wpreached!=1) if(wpreached!=1)
goto_gps(indiWP_done) goto_gps(indiWP_done)
@ -138,7 +121,7 @@ function indiWP() {
} }
function indiWP_done() { function indiWP_done() {
BVMSTATE = "INDIWP" BVMSTATE = "WAYPOINT"
wpreached = 1 wpreached = 1
} }
@ -208,10 +191,12 @@ function pursuit() {
} }
# Lennard-Jones interaction magnitude # Lennard-Jones interaction magnitude
TARGET = 8.0 TARGET = 16.0
EPSILON = 30.0 EPSILON = 30.0 #30
GAIN_ATT = 50.0
GAIN_REP = 30.0
function lj_magnitude(dist, target, epsilon) { 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 return lj
} }
@ -225,14 +210,32 @@ function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum) return math.vec2.add(data, accum)
} }
# Sate function that calculates and actuates the flocking interaction # State function that calculates and actuates LJ flocking interaction with vstig targets (attractor/repulsors)
function formation() { function lennardjones() {
BVMSTATE="FORMATION" BVMSTATE="POTENTIAL"
check_rc_wp()
# 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)
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) 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) goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
} }

View File

@ -55,7 +55,7 @@ function i2s(value){
return "LAUNCH" return "LAUNCH"
} }
else if(value==9){ else if(value==9){
return "STOP" return "FORMATION"
} }
else { else {
return "UNDETERMINED" return "UNDETERMINED"
@ -80,7 +80,7 @@ function s2i(value){
else if(value=="BARRIERWAIT"){ else if(value=="BARRIERWAIT"){
return 5 return 5
} }
else if(value=="INDIWP"){ else if(value=="WAYPOINT"){
return 6 return 6
} }
else if(value=="TASK_ALLOCATE"){ else if(value=="TASK_ALLOCATE"){
@ -89,7 +89,7 @@ function s2i(value){
else if(value=="LAUNCH"){ else if(value=="LAUNCH"){
return 8 return 8
} }
else if(value=="STOP"){ else if(value=="POTENTIAL"){
return 9 return 9
} }
else else

View File

@ -1,12 +1,12 @@
takeoff_heights ={ takeoff_heights ={
.0 = 30.0, .0 = 0.0,
.1 = 21.0, .1 = 32.0,
.2 = 18.0, .2 = 28.0,
.3 = 12.0, .3 = 24.0,
.4 = 28.0, .4 = 20.0,
.5 = 12.0, .5 = 16.0,
.6 = 3.0, .6 = 12.0,
.9 = 15.0, .9 = 8.0,
.11 = 6.0, .11 = 6.0,
.14 = 18.0, .14 = 18.0,
.16 = 9.0, .16 = 9.0,

View File

@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz"
#State launched after takeoff #State launched after takeoff
AUTO_LAUNCH_STATE = "INDIWP" AUTO_LAUNCH_STATE = "IDLE"
TARGET = 9.0 TARGET = 9.0
EPSILON = 30.0 EPSILON = 30.0
ROOT_ID = 3 ROOT_ID = 3
@ -78,8 +78,8 @@ function step() {
statef=idle statef=idle
else if(BVMSTATE=="AGGREGATE") else if(BVMSTATE=="AGGREGATE")
statef=aggregate statef=aggregate
else if(BVMSTATE=="FORMATION") else if(BVMSTATE=="POTENTIAL")
statef=formation statef=lennardjones
else if(BVMSTATE=="PURSUIT") else if(BVMSTATE=="PURSUIT")
statef=pursuit statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ? else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?