restructured bzz for path planning
This commit is contained in:
dave
parent
390a02df8d
commit
80799aeaa6
|
|
@ -28,9 +28,9 @@ function barrier_create() {
|
|||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
function barrier_set(threshold, transf, resumef, bdt) {
|
||||
function barrier_set(threshold, transf, resumef) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf, resumef, bdt);
|
||||
barrier_wait(threshold, transf, resumef);
|
||||
}
|
||||
BVMSTATE = "BARRIERWAIT"
|
||||
barrier_create()
|
||||
|
|
@ -48,7 +48,7 @@ function barrier_ready() {
|
|||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
function barrier_wait(threshold, transf, resumef, bdt) {
|
||||
function barrier_wait(threshold, transf, resumef) {
|
||||
barrier.put(id, 1)
|
||||
|
||||
barrier.get(id)
|
||||
|
|
@ -56,31 +56,13 @@ function barrier_wait(threshold, transf, resumef, bdt) {
|
|||
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
|
||||
barrier.put("d", 1)
|
||||
timeW = 0
|
||||
transf()
|
||||
BVMSTATE = transf
|
||||
} else if(timeW >= BARRIER_TIMEOUT) {
|
||||
log("------> Barrier Timeout !!!!")
|
||||
barrier=nil
|
||||
timeW = 0
|
||||
resumef()
|
||||
BVMSTATE = resumef
|
||||
}
|
||||
|
||||
if(bdt!=-1)
|
||||
neighbors.broadcast("cmd", bdt)
|
||||
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# get the lowest id of the fleet, but requires too much bandwidth...
|
||||
function getlowest(){
|
||||
Lid = 15;
|
||||
u=15
|
||||
while(u>=0){
|
||||
tab = barrier.get(u)
|
||||
if(tab!=nil){
|
||||
if(tab<Lid)
|
||||
Lid=tab
|
||||
}
|
||||
u=u-1
|
||||
}
|
||||
log("--> LOWEST ID:",Lid)
|
||||
}
|
||||
|
|
@ -0,0 +1,234 @@
|
|||
########################################
|
||||
#
|
||||
# FLIGHT-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
include "utils/vec2.bzz"
|
||||
include "plan/rrtstar.bzz"
|
||||
include "act/barrier.bzz"
|
||||
include "utils/conversions.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 15.0 # m.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
PICTURE_WAIT = 20 # steps
|
||||
GOTO_MAXVEL = 1.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTODIST_TOL = 0.5 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
path_it = 0
|
||||
rc_State = 0
|
||||
pic_time = 0
|
||||
g_it = 0
|
||||
|
||||
function turnedoff() {
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
function idle() {
|
||||
BVMSTATE = "IDLE"
|
||||
}
|
||||
|
||||
function launch() {
|
||||
BVMSTATE = "LAUNCH"
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
||||
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
|
||||
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
|
||||
barrier_ready()
|
||||
} else {
|
||||
log("Altitude: ", pose.position.altitude)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
} else {
|
||||
barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
||||
function stop() {
|
||||
BVMSTATE = "STOP"
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
if(flight.status != 2 and flight.status != 3) {
|
||||
barrier_set(ROBOTS,"TURNEDOFF","STOP")
|
||||
barrier_ready()
|
||||
}
|
||||
} else {
|
||||
barrier_set(ROBOTS,"TURNEDOFF","STOP")
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
||||
function take_picture() {
|
||||
BVMSTATE="PICTURE"
|
||||
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
|
||||
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
||||
uav_takepicture()
|
||||
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
|
||||
BVMSTATE="IDLE"
|
||||
pic_time=0
|
||||
}
|
||||
pic_time=pic_time+1
|
||||
}
|
||||
|
||||
#
|
||||
# Work in progress....
|
||||
function navigate() {
|
||||
BVMSTATE="NAVIGATE"
|
||||
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
|
||||
uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
|
||||
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
||||
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
|
||||
}
|
||||
|
||||
print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
|
||||
if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
|
||||
log("Sorry this is too far.")
|
||||
return
|
||||
}
|
||||
|
||||
# create the map
|
||||
if(map==nil) {
|
||||
dyn_init_map(cur_goal)
|
||||
if(V_TYPE == 0) {
|
||||
homegps.lat = pose.position.latitude
|
||||
homegps.long = pose.position.longitude
|
||||
}
|
||||
}
|
||||
|
||||
if(Path==nil) {
|
||||
# add proximity sensor and neighbor obstacles to the map
|
||||
populateGrid(m_pos)
|
||||
# start the planner
|
||||
path_it = 1
|
||||
pathPlanner(cur_goal, m_pos)
|
||||
} else if(path_it <= size(Path)) {
|
||||
var cur_path_pt = convert_pt(getvec(Path, path_it))
|
||||
print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
|
||||
if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
|
||||
populateGrid(m_pos)
|
||||
if(check_path(Path, path_it, m_pos, 0)) {
|
||||
# stop
|
||||
goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
|
||||
Path = nil
|
||||
if(V_TYPE == 0)
|
||||
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
||||
# re-start the planner
|
||||
path_it = 1
|
||||
pathPlanner(cur_goal, m_pos)
|
||||
} else {
|
||||
cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
|
||||
goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude)
|
||||
}
|
||||
} else
|
||||
path_it = path_it + 1
|
||||
} else {
|
||||
Path = nil
|
||||
BVMSTATE="IDLE"
|
||||
}
|
||||
}
|
||||
|
||||
# function goto_direct(transf) {
|
||||
# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
|
||||
# print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
|
||||
|
||||
# if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
|
||||
# log("Sorry this is too far.")
|
||||
# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
|
||||
# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
|
||||
# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
# } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
||||
# transf()
|
||||
# else
|
||||
# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
# }
|
||||
|
||||
function follow() {
|
||||
if(size(targets)>0) {
|
||||
BVMSTATE = "FOLLOW"
|
||||
attractor=math.vec2.newp(0,0)
|
||||
foreach(targets, function(id, tab) {
|
||||
force=(0.05)*(tab.range)^4
|
||||
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
|
||||
})
|
||||
uav_moveto(attractor.x, attractor.y, 0.0)
|
||||
} else {
|
||||
log("No target in local table!")
|
||||
BVMSTATE = "IDLE"
|
||||
}
|
||||
}
|
||||
|
||||
function rc_cmd_listen() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "LAUNCH"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "STOP"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "PATHPLAN"
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
} else if (flight.rc_cmd==666){
|
||||
flight.rc_cmd=0
|
||||
stattab_send()
|
||||
} else if (flight.rc_cmd==900){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 0
|
||||
neighbors.broadcast("cmd", 900)
|
||||
} else if (flight.rc_cmd==901){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 1
|
||||
neighbors.broadcast("cmd", 901)
|
||||
} else if (flight.rc_cmd==902){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 2
|
||||
neighbors.broadcast("cmd", 902)
|
||||
} else if (flight.rc_cmd==903){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 3
|
||||
neighbors.broadcast("cmd", 903)
|
||||
}
|
||||
}
|
||||
|
||||
function nei_cmd_listen() {
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
|
||||
if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") {
|
||||
BVMSTATE = "LAUNCH"
|
||||
} else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") {
|
||||
BVMSTATE = "STOP"
|
||||
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
|
||||
uav_arm()
|
||||
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||
uav_disarm()
|
||||
} else if(value==900){
|
||||
rc_State = 0
|
||||
} else if(value==901){
|
||||
rc_State = 1
|
||||
} else if(value==902){
|
||||
rc_State = 2
|
||||
} else if(value==903){
|
||||
rc_State = 3
|
||||
} else if(value==16 and BVMSTATE=="IDLE"){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
# # if(gt.id == id) statef=goto
|
||||
# })
|
||||
}
|
||||
})
|
||||
}
|
||||
|
|
@ -3,7 +3,7 @@
|
|||
#
|
||||
# map table-based matrix
|
||||
#####
|
||||
include "mapmatrix.bzz"
|
||||
include "plan/mapmatrix.bzz"
|
||||
|
||||
RRT_TIMEOUT = 500
|
||||
RRT_RUNSTEP = 3
|
||||
|
|
@ -78,7 +78,7 @@ function getcell(m_curpos) {
|
|||
}
|
||||
|
||||
function populateGrid(m_pos) {
|
||||
getproxobs(m_pos)
|
||||
# getproxobs(m_pos)
|
||||
getneiobs (m_pos)
|
||||
export_map(map)
|
||||
}
|
||||
|
|
@ -211,8 +211,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
|
|||
##
|
||||
function rrtstar() {
|
||||
# update state machine variables
|
||||
statef = rrtstar
|
||||
BVMSTATE = "RRTSTAR"
|
||||
BVMSTATE = "PATHPLAN"
|
||||
|
||||
|
||||
step_timeout = 0
|
||||
|
|
@ -336,16 +335,14 @@ function rrtstar() {
|
|||
|
||||
if(goalReached){
|
||||
log("Goal found(",numberOfPoints,")!")
|
||||
Path = getPath(Q,numberOfPoints, 1)
|
||||
Path = convert_path(getPath(Q,numberOfPoints))
|
||||
# done. resume the state machine
|
||||
BVMSTATE = "GOTOGPS"
|
||||
statef = old_statef
|
||||
BVMSTATE = "NAVIGATE"
|
||||
} else if(timeout >= RRT_TIMEOUT) {
|
||||
log("FAILED TO FIND A PATH!!!")
|
||||
Path = nil
|
||||
# done. resume the state machine
|
||||
BVMSTATE = "GOTOGPS"
|
||||
statef = old_statef
|
||||
BVMSTATE = "NAVIGATE"
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -437,7 +434,7 @@ function doesItIntersect(point,vector) {
|
|||
}
|
||||
|
||||
# create a table with only the path's points in order
|
||||
function getPath(Q,nb,gps){
|
||||
function getPath(Q,nb){
|
||||
var pathL={}
|
||||
var npt=0
|
||||
# get the path from the point list
|
||||
|
|
@ -460,15 +457,9 @@ function getPath(Q,nb,gps){
|
|||
var totpt = npt + 1
|
||||
while(npt > 0){
|
||||
pathR[totpt-npt] = {}
|
||||
if(gps) {
|
||||
tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell))
|
||||
pathR[totpt-npt][1]=tmpgoal.latitude
|
||||
pathR[totpt-npt][2]=tmpgoal.longitude
|
||||
} else {
|
||||
tmpgoal = getvec(path,npt)
|
||||
pathR[totpt-npt][1]=tmpgoal.x
|
||||
pathR[totpt-npt][2]=tmpgoal.y
|
||||
}
|
||||
var tmpgoal = getvec(pathL,npt)
|
||||
pathR[totpt-npt][1]=tmpgoal.x
|
||||
pathR[totpt-npt][2]=tmpgoal.y
|
||||
npt = npt - 1
|
||||
}
|
||||
#log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
|
||||
|
|
@ -1,111 +0,0 @@
|
|||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
m_vecNodes_fixed={}
|
||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
||||
.Lable = 0, # Lable of the point
|
||||
.Pred = -1, # Lable of its predecessor
|
||||
.distance = -1, # distance to the predecessor
|
||||
.bearing = -1, # bearing form the predecessor to this dot
|
||||
.height = 3000, # height of this dot
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[1] = {
|
||||
.Lable = 1,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 0.0,
|
||||
.height = 5000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[2] = {
|
||||
.Lable = 2,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 1.57,
|
||||
.height = 7000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[3] = {
|
||||
.Lable = 3,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 3.14,
|
||||
.height = 9000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[4] = {
|
||||
.Lable = 4,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 4.71,
|
||||
.height = 11000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
|
||||
#
|
||||
# Graph parsing
|
||||
#
|
||||
function parse_graph(fname) {
|
||||
# Graph data
|
||||
var gd = {}
|
||||
# Open the file
|
||||
var fd = io.fopen(fname, "r")
|
||||
if(not fd) {
|
||||
log("Can't open '", fname, "'")
|
||||
return nil
|
||||
}
|
||||
# Parse the file line by line
|
||||
var rrec # Record read from line
|
||||
var grec # Record parsed into graph
|
||||
io.fforeach(fd, function(line) {
|
||||
# Parse file line
|
||||
rrec = string.split(line, "\t ")
|
||||
# Make record
|
||||
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
||||
.Lable = string.toint(rrec[0]), # Lable of the point
|
||||
.Pred = string.toint(rrec[1]), # Lable of its predecessor
|
||||
.distance = string.tofloat(rrec[2]), # distance to the predecessor
|
||||
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
|
||||
.height = string.tofloat(rrec[4]), # height of this dot
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}})
|
||||
# All done
|
||||
io.fclose(fd)
|
||||
return gd
|
||||
}
|
||||
|
||||
function parse_graph_fixed(fname) {
|
||||
# Graph data
|
||||
var gd = {}
|
||||
# Open the file
|
||||
var fd = io.fopen(fname, "r")
|
||||
if(not fd) {
|
||||
log("Can't open '", fname, "'")
|
||||
return nil
|
||||
}
|
||||
# Parse the file line by line
|
||||
var rrec # Record read from line
|
||||
var grec # Record parsed into graph
|
||||
io.fforeach(fd, function(line) {
|
||||
# Parse file line
|
||||
rrec = string.split(line, "\t ")
|
||||
# Make record
|
||||
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
|
||||
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
|
||||
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
|
||||
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
|
||||
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
|
||||
.Lable=string.toint(rrec[0]),
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}})
|
||||
# All done
|
||||
io.fclose(fd)
|
||||
return gd
|
||||
}
|
||||
|
Before Width: | Height: | Size: 67 KiB After Width: | Height: | Size: 67 KiB |
|
Before Width: | Height: | Size: 77 KiB After Width: | Height: | Size: 77 KiB |
|
Before Width: | Height: | Size: 75 KiB After Width: | Height: | Size: 75 KiB |
|
Before Width: | Height: | Size: 96 KiB After Width: | Height: | Size: 96 KiB |
|
|
@ -1,293 +0,0 @@
|
|||
########################################
|
||||
#
|
||||
# FLIGHT-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
include "vec2.bzz"
|
||||
include "rrtstar.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 15.0 # m.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
PICTURE_WAIT = 20 # steps
|
||||
GOTO_MAXVEL = 1.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTODIST_TOL = 0.5 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
cur_goal_l = 0
|
||||
rc_State = 0
|
||||
|
||||
function uav_initswarm() {
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
}
|
||||
|
||||
function turnedoff() {
|
||||
statef=turnedoff
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
function idle() {
|
||||
statef=idle
|
||||
BVMSTATE = "IDLE"
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
BVMSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
|
||||
|
||||
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS, action, land, -1)
|
||||
barrier_ready()
|
||||
} else {
|
||||
log("Altitude: ", pose.position.altitude)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
|
||||
function land() {
|
||||
BVMSTATE = "LAND"
|
||||
statef=land
|
||||
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
|
||||
if(flight.status != 2 and flight.status != 3) {
|
||||
barrier_set(ROBOTS,turnedoff,land, 21)
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
||||
function set_goto(transf) {
|
||||
BVMSTATE = "GOTOGPS"
|
||||
statef=function() {
|
||||
gotoWPRRT(transf)
|
||||
}
|
||||
|
||||
if(rc_goto.id==id){
|
||||
if(s!=nil){
|
||||
if(s.in())
|
||||
s.leave()
|
||||
}
|
||||
} else {
|
||||
neighbors.broadcast("cmd", 16)
|
||||
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
|
||||
}
|
||||
}
|
||||
|
||||
ptime=0
|
||||
function picture() {
|
||||
statef=picture
|
||||
BVMSTATE="PICTURE"
|
||||
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
|
||||
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
||||
uav_takepicture()
|
||||
} else if(ptime>=PICTURE_WAIT) { # wait for the picture
|
||||
statef=action
|
||||
ptime=0
|
||||
}
|
||||
ptime=ptime+1
|
||||
}
|
||||
|
||||
#
|
||||
# still requires to be tuned, replaning takes too much time..
|
||||
# DS 23/11/2017
|
||||
function gotoWPRRT(transf) {
|
||||
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
||||
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)
|
||||
|
||||
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) {
|
||||
log("Sorry this is too far.")
|
||||
return
|
||||
}
|
||||
# create the map
|
||||
if(map==nil) {
|
||||
dyn_init_map(rc_goal)
|
||||
homegps.lat = pose.position.latitude
|
||||
homegps.long = pose.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 - pose.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 - pose.position.altitude)
|
||||
}
|
||||
} else
|
||||
cur_goal_l = cur_goal_l + 1
|
||||
} else {
|
||||
Path = nil
|
||||
transf()
|
||||
}
|
||||
}
|
||||
|
||||
function gotoWP(transf) {
|
||||
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
|
||||
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
|
||||
|
||||
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
|
||||
log("Sorry this is too far.")
|
||||
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
|
||||
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
|
||||
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
||||
transf()
|
||||
else
|
||||
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
}
|
||||
|
||||
function follow() {
|
||||
if(size(targets)>0) {
|
||||
BVMSTATE = "FOLLOW"
|
||||
statef=follow
|
||||
attractor=math.vec2.newp(0,0)
|
||||
foreach(targets, function(id, tab) {
|
||||
force=(0.05)*(tab.range)^4
|
||||
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
|
||||
})
|
||||
uav_moveto(attractor.x, attractor.y, 0.0)
|
||||
} else {
|
||||
log("No target in local table!")
|
||||
#statef=idle
|
||||
}
|
||||
}
|
||||
|
||||
function uav_rccmd() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef = takeoff
|
||||
BVMSTATE = "TAKEOFF"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef = land
|
||||
BVMSTATE = "LAND"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "GOTOGPS"
|
||||
statef = goto
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
} else if (flight.rc_cmd==666){
|
||||
flight.rc_cmd=0
|
||||
stattab_send()
|
||||
} else if (flight.rc_cmd==900){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 0
|
||||
neighbors.broadcast("cmd", 900)
|
||||
} else if (flight.rc_cmd==901){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 1
|
||||
neighbors.broadcast("cmd", 901)
|
||||
} else if (flight.rc_cmd==902){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 2
|
||||
neighbors.broadcast("cmd", 902)
|
||||
} else if (flight.rc_cmd==903){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 3
|
||||
neighbors.broadcast("cmd", 903)
|
||||
}
|
||||
}
|
||||
|
||||
function uav_neicmd() {
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
|
||||
if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") {
|
||||
statef=takeoff
|
||||
BVMSTATE = "TAKEOFF"
|
||||
} else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") {
|
||||
statef=land
|
||||
BVMSTATE = "LAND"
|
||||
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
|
||||
uav_arm()
|
||||
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||
uav_disarm()
|
||||
} else if(value==900){
|
||||
rc_State = 0
|
||||
} else if(value==901){
|
||||
rc_State = 1
|
||||
} else if(value==902){
|
||||
rc_State = 2
|
||||
} else if(value==903){
|
||||
rc_State = 3
|
||||
} else if(value==16 and BVMSTATE=="IDLE"){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
# # if(gt.id == id) statef=goto
|
||||
# })
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
function LimitAngle(angle){
|
||||
if(angle>2*math.pi)
|
||||
return angle-2*math.pi
|
||||
else if (angle<0)
|
||||
return angle+2*math.pi
|
||||
else
|
||||
return angle
|
||||
}
|
||||
|
||||
function vec_from_gps(lat, lon, home_ref) {
|
||||
d_lon = lon - pose.position.longitude
|
||||
d_lat = lat - pose.position.latitude
|
||||
if(home_ref == 1) {
|
||||
d_lon = lon - homegps.long
|
||||
d_lat = lat - homegps.lat
|
||||
}
|
||||
ned_x = d_lat/180*math.pi * 6371000.0;
|
||||
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
|
||||
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
|
||||
return math.vec2.new(ned_x,ned_y)
|
||||
}
|
||||
|
||||
function gps_from_vec(vec) {
|
||||
Lgoal = {.latitude=0, .longitude=0}
|
||||
Vrange = math.vec2.length(vec)
|
||||
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
|
||||
# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude)
|
||||
latR = pose.position.latitude*math.pi/180.0;
|
||||
lonR = pose.position.longitude*math.pi/180.0;
|
||||
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
|
||||
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
|
||||
Lgoal.latitude = target_lat*180.0/math.pi;
|
||||
Lgoal.longitude = target_lon*180.0/math.pi;
|
||||
#d_lat = (vec.x / 6371000.0)*180.0/math.pi;
|
||||
#goal.latitude = d_lat + pose.position.latitude;
|
||||
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
|
||||
#goal.longitude = d_lon + pose.position.longitude;
|
||||
|
||||
return Lgoal
|
||||
}
|
||||
|
|
@ -1,152 +0,0 @@
|
|||
# Utilities
|
||||
|
||||
# Rads to degrees
|
||||
function rtod(r) {
|
||||
return (r*(180.0/math.pi))
|
||||
}
|
||||
|
||||
# Copy a table
|
||||
function table_deep_copy(new_t, old_t, depth) {
|
||||
depth = depth - 1
|
||||
if (old_t != nil) {
|
||||
foreach(old_t, function(key, value) {
|
||||
new_t[key] = value
|
||||
if(depth != 0) {
|
||||
new_t[key] = {}
|
||||
table_deep_copy(new_t[key], value, depth)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_copy(old_t, depth) {
|
||||
var t = {};
|
||||
table_deep_copy(t, old_t, depth);
|
||||
return t;
|
||||
}
|
||||
|
||||
# Print the contents of a table
|
||||
function table_print(t, depth) {
|
||||
depth = depth - 1
|
||||
if (t != nil) {
|
||||
foreach(t, function(key, value) {
|
||||
log(key, " -> ", value)
|
||||
if(depth != 0) {
|
||||
table_print(t[key], depth)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Write a table as if it was a vector
|
||||
function write_vector(k, index, val) {
|
||||
var key = string.tostring(index)
|
||||
k[key] = val
|
||||
}
|
||||
|
||||
# Read a table as if it was a vector
|
||||
function read_vector(k, index) {
|
||||
var key = string.tostring(index)
|
||||
if (k[key] == nil) {
|
||||
return -1
|
||||
} else {
|
||||
return k[key]
|
||||
}
|
||||
}
|
||||
|
||||
# Write a table as if it was a matrix
|
||||
function write_matrix(k, row, col, val) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
k[key] = val
|
||||
}
|
||||
|
||||
# Read a table as if it was a matrix
|
||||
function read_matrix(k, row, col) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
if (k[key] == nil) {
|
||||
return -1
|
||||
} else {
|
||||
return k[key]
|
||||
}
|
||||
}
|
||||
|
||||
# Int to String
|
||||
function itos(i) {
|
||||
|
||||
log("Use 'string.tostring(OJB)' instead")
|
||||
|
||||
if (i==0) { return "0" }
|
||||
if (i==1) { return "1" }
|
||||
if (i==2) { return "2" }
|
||||
if (i==3) { return "3" }
|
||||
if (i==4) { return "4" }
|
||||
if (i==5) { return "5" }
|
||||
if (i==6) { return "6" }
|
||||
if (i==7) { return "7" }
|
||||
if (i==8) { return "8" }
|
||||
if (i==9) { return "9" }
|
||||
|
||||
log("Function 'itos' out of bounds, returning the answer (42)")
|
||||
return "42"
|
||||
}
|
||||
|
||||
# String to Int
|
||||
function stoi(s) {
|
||||
if (s=='0') { return 0 }
|
||||
if (s=='1') { return 1 }
|
||||
if (s=='2') { return 2 }
|
||||
if (s=='3') { return 3 }
|
||||
if (s=='4') { return 4 }
|
||||
if (s=='5') { return 5 }
|
||||
if (s=='6') { return 6 }
|
||||
if (s=='7') { return 7 }
|
||||
if (s=='8') { return 8 }
|
||||
if (s=='9') { return 9 }
|
||||
|
||||
log("Function 'stoi' out of bounds, returning the answer (42)")
|
||||
return 42
|
||||
|
||||
}
|
||||
|
||||
# Force angles in the (-pi,pi) interval
|
||||
function radians_interval(a) {
|
||||
var temp = a
|
||||
while ((temp>2.0*math.pi) or (temp<0.0)) {
|
||||
if (temp > 2.0*math.pi) {
|
||||
temp = temp - 2.0*math.pi
|
||||
} else if (temp < 0.0){
|
||||
temp = temp + 2.0*math.pi
|
||||
}
|
||||
}
|
||||
if (temp > math.pi) {
|
||||
temp = temp - 2.0*math.pi
|
||||
}
|
||||
return temp
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
#base = {}
|
||||
|
||||
#base.create = function() {
|
||||
# return {
|
||||
# .method = function(a,b) {
|
||||
# return a + b
|
||||
# }
|
||||
# }
|
||||
# }
|
||||
|
||||
#x = base.create()
|
||||
#x.method(3,4) # returns 7
|
||||
|
||||
#derived = {}
|
||||
|
||||
#derived.create = function() {
|
||||
# b = base.create()
|
||||
# b.method = function(a,b) {
|
||||
# return a * b
|
||||
# }
|
||||
#}
|
||||
|
||||
#x = derived.create()
|
||||
#x.method(3,4) # returns 12
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
function LimitAngle(angle){
|
||||
if(angle>2*math.pi)
|
||||
return angle-2*math.pi
|
||||
else if (angle<0)
|
||||
return angle+2*math.pi
|
||||
else
|
||||
return angle
|
||||
}
|
||||
|
||||
# TODO: add other conversions....
|
||||
function convert_path(P) {
|
||||
var pathR={}
|
||||
if(V_TYPE == 0) {
|
||||
var n = 1
|
||||
while(n <= size(P)){
|
||||
pathR[n] = {}
|
||||
var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell))
|
||||
pathR[n][1]=tmpgoal.latitude
|
||||
pathR[n][2]=tmpgoal.longitude
|
||||
n = n + 1
|
||||
}
|
||||
}
|
||||
return pathR
|
||||
}
|
||||
|
||||
# TODO: add other conversions....
|
||||
function convert_pt(in) {
|
||||
if(V_TYPE == 0)
|
||||
return vec_from_gps(in.x, in.y, 0)
|
||||
}
|
||||
|
||||
function vec_from_gps(lat, lon, home_ref) {
|
||||
d_lon = lon - pose.position.longitude
|
||||
d_lat = lat - pose.position.latitude
|
||||
if(home_ref == 1) {
|
||||
d_lon = lon - homegps.long
|
||||
d_lat = lat - homegps.lat
|
||||
}
|
||||
ned_x = d_lat/180*math.pi * 6371000.0;
|
||||
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
|
||||
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
|
||||
return math.vec2.new(ned_x,ned_y)
|
||||
}
|
||||
|
||||
function gps_from_vec(vec) {
|
||||
Lgoal = {.latitude=0, .longitude=0}
|
||||
Vrange = math.vec2.length(vec)
|
||||
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
|
||||
# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude)
|
||||
latR = pose.position.latitude*math.pi/180.0;
|
||||
lonR = pose.position.longitude*math.pi/180.0;
|
||||
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
|
||||
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
|
||||
Lgoal.latitude = target_lat*180.0/math.pi;
|
||||
Lgoal.longitude = target_lon*180.0/math.pi;
|
||||
#d_lat = (vec.x / 6371000.0)*180.0/math.pi;
|
||||
#goal.latitude = d_lat + pose.position.latitude;
|
||||
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
|
||||
#goal.longitude = d_lon + pose.position.longitude;
|
||||
|
||||
return Lgoal
|
||||
}
|
||||
|
|
@ -19,7 +19,13 @@ var v_ground = {}
|
|||
b_updating = 0
|
||||
vstig_counter = WAIT4STEP
|
||||
|
||||
function uav_initstig() {
|
||||
|
||||
function init_swarm() {
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
}
|
||||
|
||||
function init_stig() {
|
||||
v_status = stigmergy.create(STATUS_VSTIG)
|
||||
v_ground = stigmergy.create(GROUND_VSTIG)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,81 @@
|
|||
include "update.bzz"
|
||||
# don't use a stigmergy id=11 with this header, for barrier
|
||||
# it requires an 'action' function to be defined here.
|
||||
include "act/states.bzz"
|
||||
include "vstigenv.bzz"
|
||||
|
||||
#####
|
||||
# Vehicule type:
|
||||
# 0 -> outdoor flying vehicle
|
||||
# 1 -> indoor flying vehicle
|
||||
# 2 -> outdoor wheeled vehicle
|
||||
# 3 -> indoor wheeled vehicle
|
||||
V_TYPE = 0
|
||||
|
||||
goal_list = {
|
||||
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
|
||||
}
|
||||
|
||||
function action() {
|
||||
statef=action
|
||||
set_goto(idle)
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
init_stig()
|
||||
init_swarm()
|
||||
|
||||
TARGET_ALTITUDE = 25.0 # m
|
||||
|
||||
# start the swarm command listener
|
||||
# nei_cmd_listen()
|
||||
|
||||
# Starting state
|
||||
# BVMSTATE = "TURNEDOFF"
|
||||
BVMSTATE = "LAUNCHED"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
# listen to Remote Controller
|
||||
rc_cmd_listen()
|
||||
|
||||
# update the vstig (status/net/batt/...)
|
||||
# uav_updatestig()
|
||||
|
||||
#
|
||||
# graph state machine
|
||||
#
|
||||
if(BVMSTATE=="TURNEDOFF")
|
||||
statef=turnedoff
|
||||
else if(BVMSTATE=="STOP") # ends on turnedoff
|
||||
statef=stop
|
||||
else if(BVMSTATE=="LAUNCHED") # ends on navigate
|
||||
statef=launch
|
||||
else if(BVMSTATE=="IDLE")
|
||||
statef=idle
|
||||
else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure
|
||||
statef=makegraph # or bidding
|
||||
else if(BVMSTATE=="PATHPLAN") # ends on navigate
|
||||
statef=rrtstar
|
||||
else if(BVMSTATE=="NAVIGATE") # ends on idle
|
||||
statef=navigate
|
||||
else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure
|
||||
statef=follow
|
||||
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
|
||||
statef=take_picture
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -1,45 +0,0 @@
|
|||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
|
||||
function action() {
|
||||
statef=action
|
||||
uav_storegoal(45.5088103899,-73.1540826153,2.5)
|
||||
set_goto(idle)
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
uav_initstig()
|
||||
uav_initswarm()
|
||||
|
||||
TARGET_ALTITUDE = 2.5 # m
|
||||
# statef=turnedoff
|
||||
# BVMSTATE = "TURNEDOFF"
|
||||
statef = takeoff
|
||||
BVMSTATE = "TAKEOFF"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
uav_rccmd()
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
# log("Obstacles: ")
|
||||
# reduce(proximity,
|
||||
# function(key, value, acc) {
|
||||
# log(key, " - ", value.angle, value.value)
|
||||
# return acc
|
||||
# }, math.vec2.new(0, 0))
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -201,7 +201,7 @@ static int buzz_register_hooks()
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
||||
|
|
@ -255,7 +255,7 @@ static int testing_buzz_register_hooks()
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
||||
|
|
|
|||
Loading…
Reference in New Issue