made rrtstar a state
This commit is contained in:
parent
50368d84d3
commit
63f9111d2f
|
@ -32,7 +32,7 @@ function barrier_set(threshold, transf, resumef, bdt) {
|
||||||
statef = function() {
|
statef = function() {
|
||||||
barrier_wait(threshold, transf, resumef, bdt);
|
barrier_wait(threshold, transf, resumef, bdt);
|
||||||
}
|
}
|
||||||
UAVSTATE = "BARRIERWAIT"
|
BVMSTATE = "BARRIERWAIT"
|
||||||
barrier_create()
|
barrier_create()
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -6,10 +6,13 @@
|
||||||
include "mapmatrix.bzz"
|
include "mapmatrix.bzz"
|
||||||
|
|
||||||
RRT_TIMEOUT = 500
|
RRT_TIMEOUT = 500
|
||||||
|
RRT_RUNSTEP = 3
|
||||||
|
PROX_SENSOR_THRESHOLD = 0.11
|
||||||
GSCALE = {.x=1, .y=1}
|
GSCALE = {.x=1, .y=1}
|
||||||
map = nil
|
map = nil
|
||||||
cur_cell = {}
|
cur_cell = {}
|
||||||
nei_cell = {}
|
nei_cell = {}
|
||||||
|
mapgoal = {}
|
||||||
|
|
||||||
# create a map with its length to fit the goal (rel. pos.) and the current position as the center
|
# create a map with its length to fit the goal (rel. pos.) and the current position as the center
|
||||||
function dyn_init_map(m_navigation) {
|
function dyn_init_map(m_navigation) {
|
||||||
|
@ -29,8 +32,25 @@ function pathPlanner(m_goal, m_pos, kh4) {
|
||||||
#print_map(map)
|
#print_map(map)
|
||||||
export_map(map)
|
export_map(map)
|
||||||
|
|
||||||
|
# Initialize RRTStar var
|
||||||
|
log("--------Initialize RRTStar--------------")
|
||||||
|
HEIGHT = size(map)
|
||||||
|
WIDTH = size(map[1])
|
||||||
|
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
|
||||||
|
goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0}
|
||||||
|
#table_print(goalBoundary)
|
||||||
|
numberOfPoints = 1
|
||||||
|
arrayOfPoints = {}
|
||||||
|
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
|
||||||
|
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
|
||||||
|
Q = {}
|
||||||
|
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0}
|
||||||
|
goalReached = 0
|
||||||
|
timeout = 0
|
||||||
|
|
||||||
# search for a path
|
# search for a path
|
||||||
return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0))
|
old_statef = statef
|
||||||
|
rrtstar()
|
||||||
}
|
}
|
||||||
|
|
||||||
function getcell(m_curpos) {
|
function getcell(m_curpos) {
|
||||||
|
@ -53,12 +73,13 @@ function getcell(m_curpos) {
|
||||||
return cell
|
return cell
|
||||||
}
|
}
|
||||||
|
|
||||||
function updateMap(m_pos) {
|
function populateGrid(m_pos) {
|
||||||
#getproxobs(m_pos)
|
#getproxobs(m_pos)
|
||||||
UAVgetneiobs (m_pos)
|
getneiobs (m_pos)
|
||||||
}
|
}
|
||||||
|
|
||||||
function UAVgetneiobs (m_curpos) {
|
# TODO: populate the map with a blob instead?
|
||||||
|
function getneiobs (m_curpos) {
|
||||||
cur_cell = getcell(m_curpos)
|
cur_cell = getcell(m_curpos)
|
||||||
# add all neighbors as obstacles in the grid
|
# add all neighbors as obstacles in the grid
|
||||||
neighbors.foreach(function(rid, data) {
|
neighbors.foreach(function(rid, data) {
|
||||||
|
@ -66,54 +87,47 @@ function UAVgetneiobs (m_curpos) {
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
function getneiobs (m_curpos) {
|
# old function tested with the khepera for reference
|
||||||
var foundobstacle = 0
|
# DS 20/11
|
||||||
cur_cell = getcell(m_curpos)
|
# function getneiobs (m_curpos) {
|
||||||
var old_nei = table_copy(nei_cell)
|
# var foundobstacle = 0
|
||||||
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
|
# cur_cell = getcell(m_curpos)
|
||||||
|
# var old_nei = table_copy(nei_cell)
|
||||||
|
|
||||||
neighbors.foreach(function(rid, data) {
|
# neighbors.foreach(function(rid, data) {
|
||||||
#log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg")
|
# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
|
||||||
Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
|
# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
||||||
#log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm")
|
# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
|
||||||
Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
|
||||||
Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
|
# if(old_nei[rid]!=nil) {
|
||||||
nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
|
# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
|
||||||
#log("Neighbor in : ", Ncell.x, " ", Ncell.y)
|
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
|
||||||
if(old_nei[rid]!=nil) {
|
# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
|
||||||
if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
|
# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
|
||||||
#log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y)
|
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
|
||||||
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
|
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
|
||||||
remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
|
# add_obstacle(Ncell, 0, 1.0)
|
||||||
remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
|
# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
||||||
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
|
# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
||||||
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
||||||
add_obstacle(Ncell, 0, 1.0)
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
||||||
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
# foundobstacle = 1
|
||||||
add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
# }
|
||||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
# } else {
|
||||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
# add_obstacle(Ncell, 0, 1.0)
|
||||||
foundobstacle = 1
|
# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
||||||
}
|
# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
||||||
} else {
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
||||||
add_obstacle(Ncell, 0, 1.0)
|
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
||||||
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
# foundobstacle = 1
|
||||||
add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
# }
|
||||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
# })
|
||||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
# return foundobstacle
|
||||||
foundobstacle = 1
|
# }
|
||||||
}
|
|
||||||
})
|
|
||||||
|
|
||||||
#if(foundobstacle) {
|
|
||||||
#print_map(map)
|
|
||||||
#}
|
|
||||||
|
|
||||||
return foundobstacle
|
|
||||||
}
|
|
||||||
|
|
||||||
|
# populate a line in front of the sensor using plateform independant proximity sensing
|
||||||
function getproxobs (m_curpos) {
|
function getproxobs (m_curpos) {
|
||||||
foundobstacle = 0
|
var foundobstacle = 0
|
||||||
cur_cell = getcell(m_curpos)
|
cur_cell = getcell(m_curpos)
|
||||||
|
|
||||||
reduce(proximity,
|
reduce(proximity,
|
||||||
|
@ -125,7 +139,7 @@ function getproxobs (m_curpos) {
|
||||||
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
|
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
|
||||||
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
||||||
obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
|
obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
|
||||||
if(value.value > IR_SENSOR_THRESHOLD) {
|
if(value.value > PROX_SENSOR_THRESHOLD) {
|
||||||
if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
|
if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
|
||||||
add_obstacle(obs, 0, 0.25)
|
add_obstacle(obs, 0, 0.25)
|
||||||
add_obstacle(obs2, 0, 0.25)
|
add_obstacle(obs2, 0, 0.25)
|
||||||
|
@ -142,15 +156,6 @@ function getproxobs (m_curpos) {
|
||||||
return acc
|
return acc
|
||||||
}, math.vec2.new(0, 0))
|
}, math.vec2.new(0, 0))
|
||||||
|
|
||||||
#if(foundobstacle) {
|
|
||||||
# reduce(proximity,
|
|
||||||
# function(key, value, acc){
|
|
||||||
# log(value.value, ", ", value.angle)
|
|
||||||
# return acc
|
|
||||||
# }, math.vec2.new(0, 0))
|
|
||||||
# print_map(map)
|
|
||||||
#}
|
|
||||||
|
|
||||||
return foundobstacle
|
return foundobstacle
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -176,29 +181,16 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
|
||||||
return pathisblocked
|
return pathisblocked
|
||||||
}
|
}
|
||||||
|
|
||||||
function RRTSTAR(GOAL,TOL) {
|
function rrtstar() {
|
||||||
HEIGHT = size(map)
|
# update state machine variables
|
||||||
WIDTH = size(map[1])
|
statef = rrtstar
|
||||||
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
|
BVMSTATE = "RRTSTAR"
|
||||||
|
|
||||||
|
|
||||||
var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
|
|
||||||
#table_print(goalBoundary)
|
|
||||||
var numberOfPoints = 1
|
|
||||||
var arrayOfPoints = {}
|
|
||||||
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
|
|
||||||
Path = {}
|
|
||||||
mat_copyrow(Path,1,arrayOfPoints,1)
|
|
||||||
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
|
|
||||||
Q = {}
|
|
||||||
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0}
|
|
||||||
|
|
||||||
goalReached = 0;
|
|
||||||
timeout = 0
|
|
||||||
##
|
##
|
||||||
# main search loop
|
# main search loop
|
||||||
##
|
##
|
||||||
while(goalReached == 0 and timeout < RRT_TIMEOUT) {
|
step_timeout = 0
|
||||||
|
while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) {
|
||||||
|
|
||||||
# Point generation only as grid cell centers
|
# Point generation only as grid cell centers
|
||||||
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
|
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
|
||||||
|
@ -313,15 +305,19 @@ function RRTSTAR(GOAL,TOL) {
|
||||||
log(numberOfPoints, " points processed. Still looking for goal.");
|
log(numberOfPoints, " points processed. Still looking for goal.");
|
||||||
}
|
}
|
||||||
timeout = timeout + 1
|
timeout = timeout + 1
|
||||||
|
step_timeout = step_timeout + 1
|
||||||
}
|
}
|
||||||
|
|
||||||
if(goalReached){
|
if(goalReached){
|
||||||
log("Goal found(",numberOfPoints,")!")
|
log("Goal found(",numberOfPoints,")!")
|
||||||
Path = getPath(Q,numberOfPoints, 1)
|
Path = getPath(Q,numberOfPoints, 1)
|
||||||
|
# done. resume the state machine
|
||||||
|
BVMSTATE = "GOTOGPS"
|
||||||
|
statef = old_statef
|
||||||
} else {
|
} else {
|
||||||
log("FAILED TO FIND A PATH!!!")
|
#log("FAILED TO FIND A PATH!!!")
|
||||||
Path = nil
|
Path = nil
|
||||||
}
|
}
|
||||||
return Path
|
|
||||||
}
|
}
|
||||||
|
|
||||||
function findClosestPoint(point,aPt) {
|
function findClosestPoint(point,aPt) {
|
||||||
|
|
|
@ -7,7 +7,7 @@ include "vec2.bzz"
|
||||||
include "rrtstar.bzz"
|
include "rrtstar.bzz"
|
||||||
|
|
||||||
TARGET_ALTITUDE = 15.0 # m.
|
TARGET_ALTITUDE = 15.0 # m.
|
||||||
UAVSTATE = "TURNEDOFF"
|
BVMSTATE = "TURNEDOFF"
|
||||||
PICTURE_WAIT = 20 # steps
|
PICTURE_WAIT = 20 # steps
|
||||||
GOTO_MAXVEL = 2 # m/steps
|
GOTO_MAXVEL = 2 # m/steps
|
||||||
GOTO_MAXDIST = 150 # m.
|
GOTO_MAXDIST = 150 # m.
|
||||||
|
@ -23,16 +23,16 @@ function uav_initswarm() {
|
||||||
|
|
||||||
function turnedoff() {
|
function turnedoff() {
|
||||||
statef=turnedoff
|
statef=turnedoff
|
||||||
UAVSTATE = "TURNEDOFF"
|
BVMSTATE = "TURNEDOFF"
|
||||||
}
|
}
|
||||||
|
|
||||||
function idle() {
|
function idle() {
|
||||||
statef=idle
|
statef=idle
|
||||||
UAVSTATE = "IDLE"
|
BVMSTATE = "IDLE"
|
||||||
}
|
}
|
||||||
|
|
||||||
function takeoff() {
|
function takeoff() {
|
||||||
UAVSTATE = "TAKEOFF"
|
BVMSTATE = "TAKEOFF"
|
||||||
statef=takeoff
|
statef=takeoff
|
||||||
homegps = {.lat=position.latitude, .long=position.longitude}
|
homegps = {.lat=position.latitude, .long=position.longitude}
|
||||||
|
|
||||||
|
@ -47,7 +47,7 @@ function takeoff() {
|
||||||
}
|
}
|
||||||
|
|
||||||
function land() {
|
function land() {
|
||||||
UAVSTATE = "LAND"
|
BVMSTATE = "LAND"
|
||||||
statef=land
|
statef=land
|
||||||
|
|
||||||
neighbors.broadcast("cmd", 21)
|
neighbors.broadcast("cmd", 21)
|
||||||
|
@ -60,7 +60,7 @@ function land() {
|
||||||
}
|
}
|
||||||
|
|
||||||
function set_goto(transf) {
|
function set_goto(transf) {
|
||||||
UAVSTATE = "GOTOGPS"
|
BVMSTATE = "GOTOGPS"
|
||||||
statef=function() {
|
statef=function() {
|
||||||
gotoWPRRT(transf)
|
gotoWPRRT(transf)
|
||||||
}
|
}
|
||||||
|
@ -79,7 +79,7 @@ function set_goto(transf) {
|
||||||
ptime=0
|
ptime=0
|
||||||
function picture() {
|
function picture() {
|
||||||
statef=picture
|
statef=picture
|
||||||
UAVSTATE="PICTURE"
|
BVMSTATE="PICTURE"
|
||||||
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
|
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
|
||||||
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
||||||
uav_takepicture()
|
uav_takepicture()
|
||||||
|
@ -98,50 +98,45 @@ function gotoWPRRT(transf) {
|
||||||
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
|
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
|
||||||
print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
|
print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
|
||||||
|
|
||||||
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5)
|
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) {
|
||||||
log("Sorry this is too far.")
|
log("Sorry this is too far.")
|
||||||
else {
|
return
|
||||||
if(Path==nil){
|
}
|
||||||
# create the map
|
# create the map
|
||||||
if(map==nil) {
|
if(map==nil) {
|
||||||
dyn_init_map(rc_goal)
|
dyn_init_map(rc_goal)
|
||||||
homegps.lat = position.latitude
|
homegps.lat = position.latitude
|
||||||
homegps.long = position.longitude
|
homegps.long = position.longitude
|
||||||
|
}
|
||||||
|
|
||||||
|
if(Path==nil) {
|
||||||
|
# add proximity sensor and neighbor obstacles to the map
|
||||||
|
populateGrid(m_pos)
|
||||||
|
# start the planner
|
||||||
|
pathPlanner(rc_goal, m_pos)
|
||||||
|
cur_goal_l = 1
|
||||||
|
} else if(cur_goal_l <= size(Path)) {
|
||||||
|
var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude
|
||||||
|
var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0)
|
||||||
|
print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y)
|
||||||
|
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
|
||||||
|
populateGrid(m_pos)
|
||||||
|
if(check_path(Path, cur_goal_l, m_pos, 0)) {
|
||||||
|
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
|
||||||
|
Path = nil
|
||||||
|
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
||||||
|
# re-start the planner
|
||||||
|
pathPlanner(rc_goal, m_pos)
|
||||||
|
cur_goal_l = 1
|
||||||
|
} else {
|
||||||
|
cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
|
||||||
|
uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude)
|
||||||
}
|
}
|
||||||
# add proximity sensor and neighbor obstacles to the map
|
} else
|
||||||
while(Path==nil) {
|
cur_goal_l = cur_goal_l + 1
|
||||||
updateMap(m_pos)
|
} else {
|
||||||
Path = pathPlanner(rc_goal, m_pos)
|
Path = nil
|
||||||
}
|
transf()
|
||||||
print_pos(Path)
|
|
||||||
cur_goal_l = 1
|
|
||||||
} else if(cur_goal_l <= size(Path)) {
|
|
||||||
var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude
|
|
||||||
var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0)
|
|
||||||
print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y)
|
|
||||||
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
|
|
||||||
updateMap(m_pos)
|
|
||||||
if(check_path(Path, cur_goal_l, m_pos, 0)) {
|
|
||||||
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
|
|
||||||
Path = nil
|
|
||||||
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
|
||||||
while(Path == nil) {
|
|
||||||
updateMap(m_pos)
|
|
||||||
Path = pathPlanner(rc_goal, m_pos)
|
|
||||||
}
|
|
||||||
print_pos(Path)
|
|
||||||
cur_goal_l = 1
|
|
||||||
} else {
|
|
||||||
cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
|
|
||||||
uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
cur_goal_l = cur_goal_l + 1
|
|
||||||
} else {
|
|
||||||
Path = nil
|
|
||||||
transf()
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -162,7 +157,7 @@ function gotoWP(transf) {
|
||||||
|
|
||||||
function follow() {
|
function follow() {
|
||||||
if(size(targets)>0) {
|
if(size(targets)>0) {
|
||||||
UAVSTATE = "FOLLOW"
|
BVMSTATE = "FOLLOW"
|
||||||
statef=follow
|
statef=follow
|
||||||
attractor=math.vec2.newp(0,0)
|
attractor=math.vec2.newp(0,0)
|
||||||
foreach(targets, function(id, tab) {
|
foreach(targets, function(id, tab) {
|
||||||
|
@ -181,18 +176,18 @@ function uav_rccmd() {
|
||||||
log("cmd 22")
|
log("cmd 22")
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
statef = takeoff
|
statef = takeoff
|
||||||
UAVSTATE = "TAKEOFF"
|
BVMSTATE = "TAKEOFF"
|
||||||
neighbors.broadcast("cmd", 22)
|
neighbors.broadcast("cmd", 22)
|
||||||
} else if(flight.rc_cmd==21) {
|
} else if(flight.rc_cmd==21) {
|
||||||
log("cmd 21")
|
log("cmd 21")
|
||||||
log("To land")
|
log("To land")
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
statef = land
|
statef = land
|
||||||
UAVSTATE = "LAND"
|
BVMSTATE = "LAND"
|
||||||
neighbors.broadcast("cmd", 21)
|
neighbors.broadcast("cmd", 21)
|
||||||
} else if(flight.rc_cmd==16) {
|
} else if(flight.rc_cmd==16) {
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
UAVSTATE = "GOTOGPS"
|
BVMSTATE = "GOTOGPS"
|
||||||
statef = goto
|
statef = goto
|
||||||
} else if(flight.rc_cmd==400) {
|
} else if(flight.rc_cmd==400) {
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
|
@ -227,16 +222,16 @@ function uav_rccmd() {
|
||||||
function uav_neicmd() {
|
function uav_neicmd() {
|
||||||
neighbors.listen("cmd",
|
neighbors.listen("cmd",
|
||||||
function(vid, value, rid) {
|
function(vid, value, rid) {
|
||||||
print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")")
|
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
|
||||||
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
|
if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") {
|
||||||
statef=takeoff
|
statef=takeoff
|
||||||
UAVSTATE = "TAKEOFF"
|
BVMSTATE = "TAKEOFF"
|
||||||
} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
|
} else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") {
|
||||||
statef=land
|
statef=land
|
||||||
UAVSTATE = "LAND"
|
BVMSTATE = "LAND"
|
||||||
} else if(value==400 and UAVSTATE=="TURNEDOFF") {
|
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
|
||||||
uav_arm()
|
uav_arm()
|
||||||
} else if(value==401 and UAVSTATE=="TURNEDOFF"){
|
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||||
uav_disarm()
|
uav_disarm()
|
||||||
} else if(value==900){
|
} else if(value==900){
|
||||||
rc_State = 0
|
rc_State = 0
|
||||||
|
@ -246,7 +241,7 @@ function uav_neicmd() {
|
||||||
rc_State = 2
|
rc_State = 2
|
||||||
} else if(value==903){
|
} else if(value==903){
|
||||||
rc_State = 3
|
rc_State = 3
|
||||||
} else if(value==16 and UAVSTATE=="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)
|
||||||
# # if(gt.id == id) statef=goto
|
# # if(gt.id == id) statef=goto
|
||||||
|
|
|
@ -16,7 +16,7 @@ function init() {
|
||||||
#statef=turnedoff
|
#statef=turnedoff
|
||||||
#UAVSTATE = "TURNEDOFF"
|
#UAVSTATE = "TURNEDOFF"
|
||||||
statef = takeoff
|
statef = takeoff
|
||||||
UAVSTATE = "TAKEOFF"
|
BVMSTATE = "TAKEOFF"
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
|
@ -25,7 +25,7 @@ function step() {
|
||||||
|
|
||||||
statef()
|
statef()
|
||||||
|
|
||||||
log("Current state: ", UAVSTATE)
|
log("Current state: ", BVMSTATE)
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed once when the robot (or the simulator) is reset.
|
# Executed once when the robot (or the simulator) is reset.
|
||||||
|
|
|
@ -480,7 +480,7 @@ string getuavstate()
|
||||||
---------------------------------------*/
|
---------------------------------------*/
|
||||||
{
|
{
|
||||||
static buzzvm_t VM = buzz_utility::get_vm();
|
static buzzvm_t VM = buzz_utility::get_vm();
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
|
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
|
||||||
buzzvm_pop(VM);
|
buzzvm_pop(VM);
|
||||||
|
|
Loading…
Reference in New Issue