diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index b99432a..79ad775 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -5,8 +5,8 @@ robot_marker = "X" # copy a full matrix row function mat_copyrow(out,ro,in,ri){ out[ro] = {} - var icr = 0 - while(icr < size(in[ri])) { + var icr = 1 + while(icr <= size(in[ri])) { out[ro][icr]=in[ri][icr] icr = icr + 1 } @@ -18,11 +18,11 @@ function getvec(t,row){ function init_test_map(len){ map = {} - var indexR = 0 - while(indexR < len) { + var indexR = 1 + while(indexR <= len) { map[indexR] = {} - var indexC = 0 - while(indexC < len) { + var indexC = 1 + while(indexC <= len) { map[indexR][indexC] = 1.0 indexC = indexC + 1 } @@ -33,22 +33,22 @@ function init_test_map(len){ map[6][5] = 0.0 map[4][5] = 0.0 - log("Occupancy grid initialized (",len,"x",len,") with obstacles.") + log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.") } function init_map(len){ map = {} - var indexR = 0 - while(indexR < len) { + var indexR = 1 + while(indexR <= len) { map[indexR] = {} - var indexC = 0 - while(indexC < len) { + var indexC = 1 + while(indexC <= len) { map[indexR][indexC] = 1.0 indexC = indexC + 1 } indexR = indexR + 1 } - log("Occupancy grid initialized (",len,"x",len,").") + log("Occupancy grid initialized (",size(map),"x",size(map[1]),").") } function add_obstacle(pos, off, inc_trust) { @@ -57,7 +57,7 @@ function add_obstacle(pos, off, inc_trust) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { #log("Add obstacle in cell: ", xi, " ", yi) - old=map[xi][yi] + var old=map[xi][yi] if(old-inc_trust > 0.0) map[xi][yi] = old-inc_trust else @@ -71,7 +71,7 @@ function remove_obstacle(pos, off, dec_trust) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ #log("Remove obstacle in cell: ", xi, " ", yi) - old=map[xi][yi] + var old=map[xi][yi] if(old + dec_trust < 1.0) #x,y map[xi][yi] = old+dec_trust else @@ -79,6 +79,30 @@ function remove_obstacle(pos, off, dec_trust) { } } + +function get_occupied_cells(cur_cell){ + var i = 1 + var occupied_cells = {} + occupied_cells[0] = cur_cell + occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y) + occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y) + occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1) + occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1) + return occupied_cells +} + +function is_in(dictionary, x, y){ + var i = 0 + while(i < size(dictionary)){ + if(dictionary[i].x == x and dictionary[i].y == y){ + return 1 + } + i = i + 1 + } + return 0 +} + + function table_print(t) { foreach(t, function(key, value) { log(key, " -> ", value) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index ff0630c..b84fec9 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -14,12 +14,12 @@ function UAVinit_map(m_navigation) { # create a map big enough for the goal init_map(2*math.round(math.vec2.length(m_navigation))+10) # center the robot on the grid - cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0)) + cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) } function UAVpathPlanner(m_navigation, m_pos) { # place the robot on the grid - update_curcell(m_pos,0) + cur_cell = getcell(m_pos,0) # create the goal in the map grid mapgoal = math.vec2.add(m_navigation,cur_cell) mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) @@ -34,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) { m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) # place the robot on the grid - update_curcell(m_pos,1) + cur_cell = getcell(m_pos,1) log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) log("Going to cell: ", m_goal.x, " ", m_goal.y) @@ -44,26 +44,35 @@ function Kh4pathPlanner(m_goal, m_pos) { return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) } -function update_curcell(m_curpos, kh4) { +function getcell(m_curpos, kh4) { + var cell = {} if(kh4) { # for khepera playground - cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) + cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y)) } else { # for uav map relative to home - cur_cell = math.vec2.add(cur_cell, m_curpos) - cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y)) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + cell = math.vec2.add(cell, m_curpos) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) } - if(cur_cell.x > size(map)) - cur_cell.x = size(map) - if(cur_cell.y > size(map[1])) - cur_cell.y = size(map[1]) - if(cur_cell.x < 1) - cur_cell.x = 1 - if(cur_cell.y < 1) - cur_cell.y = 1 + if(cell.x > size(map)) + cell.x = size(map) + if(cell.y > size(map[1])) + cell.y = size(map[1]) + if(cell.x < 1) + cell.x = 1 + if(cell.y < 1) + cell.y = 1 + + return cell +} + +function updateMap(m_pos) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) } function UAVgetneiobs (m_curpos) { - update_curcell(m_curpos,0) + cur_cell = getcell(m_curpos,0) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) @@ -72,7 +81,7 @@ function UAVgetneiobs (m_curpos) { function getneiobs (m_curpos) { var foundobstacle = 0 - update_curcell(m_curpos,1) + cur_cell = getcell(m_curpos,1) var old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -118,7 +127,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - update_curcell(m_curpos,1) + cur_cell = getcell(m_curpos,1) reduce(proximity, function(key, value, acc) { @@ -159,19 +168,21 @@ function getproxobs (m_curpos) { } function check_path(m_path, goal_l, m_curpos, kh4) { - pathisblocked = 0 - nb=goal_l - update_curcell(m_curpos,kh4) - Cvec = cur_cell - while(nb < size(m_path) and nb <= goal_l+1) { - Nvec = getvec(m_path, nb) - if(kh4==0) - Nvec=vec_from_gps(Nvec.x,Nvec.y) - if(doesItIntersect(Cvec, Nvec)){ - log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") + var pathisblocked = 0 + var nb = goal_l + cur_cell = getcell(m_curpos,kh4) + var Cvec = cur_cell + while(nb <= size(m_path) and nb <= goal_l+1) { + var Nvec = getvec(m_path, nb) + if(kh4 == 0) { + Nvec = vec_from_gps(Nvec.x,Nvec.y,1) + Nvec = getcell(Nvec,kh4) + } + if(doesItIntersect(Cvec, Nvec)) { + log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") pathisblocked = 1 } - Cvec=Nvec + Cvec = Nvec nb = nb + 1 } @@ -184,12 +195,16 @@ function RRTSTAR(GOAL,TOL) { RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive - goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} + 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) - numberOfPoints = 1 - arrayOfPoints = {} + 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 @@ -202,7 +217,7 @@ function RRTSTAR(GOAL,TOL) { pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) #log("Point generated: ", pt.x, " ", pt.y) - pointList = findPointsInRadius(pt,Q,RADIUS); + var pointList = findPointsInRadius(pt,Q,RADIUS); # Find connection that provides the least cost to come nbCount = 0; @@ -210,7 +225,7 @@ function RRTSTAR(GOAL,TOL) { minCounter = 0; if(size(pointList) != 0) { - #log("Found ", size(pointList), " close point:", pointList.mat) + log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) ipt=1 while(ipt <= size(pointList)) { pointNumber = {} @@ -224,7 +239,7 @@ function RRTSTAR(GOAL,TOL) { nbCount = nbCount + 1; if(intersects != 1) { #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") - distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] + var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] if(distance < minCounted) { minCounted = distance; @@ -235,9 +250,11 @@ function RRTSTAR(GOAL,TOL) { } if(minCounter > 0) { numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][2]=pt.y + Q[numberOfPoints] = {} Q[numberOfPoints][1] = pt.x Q[numberOfPoints][2] = pt.y Q[numberOfPoints][3] = pointList[minCounter][4] @@ -249,9 +266,10 @@ function RRTSTAR(GOAL,TOL) { # Now check to see if any of the other points can be redirected nbCount = 0; ipt = 1 - while(ipt < size(pointList)) { + while(ipt <= size(pointList)) { pointNumber = {} mat_copyrow(pointNumber,1,pointList,ipt) + #log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4]) # Follow the line to see if it intersects anything intersects = doesItIntersect(pt,getvec(pointNumber,1)); @@ -262,6 +280,7 @@ function RRTSTAR(GOAL,TOL) { if(intersects != 1) { # If the alternative path is shorter than change it tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt)) + #log("Q: ", size(Q), "x", size(Q[1])) if(tmpdistance < Q[pointNumber[1][4]][5]) { Q[pointNumber[1][4]][3] = numberOfPoints Q[pointNumber[1][4]][5] = tmpdistance @@ -285,9 +304,11 @@ function RRTSTAR(GOAL,TOL) { # If there is no intersection we need to add to the tree if(intersects != 1) { numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][2]=pt.y + Q[numberOfPoints] = {} Q[numberOfPoints][1] = pt.x Q[numberOfPoints][2] = pt.y Q[numberOfPoints][3] = pointNum @@ -320,51 +341,51 @@ function RRTSTAR(GOAL,TOL) { function findClosestPoint(point,aPt) { # Go through each points and find the distances between them and the # target point - distance = 999 - pointNumber = -1 - ifcp=1 + var distance = 999 + var pointNb = -1 + var ifcp=1 while(ifcp <= size(aPt)) { - range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) if(range < distance) { distance = range; - pointNumber = ifcp; + pointNb = ifcp; } ifcp = ifcp + 1 } - return pointNumber + return pointNb } # Find the closest point in the tree function findPointsInRadius(point,q,r) { - pointList = {} + var ptList = {} var counted = 0; var iir = 1 while(iir <= size(q)) { - tmpvv = getvec(q,iir) + var tmpvv = getvec(q,iir) #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) - distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) if(distance < r) { counted = counted+1; - mat_copyrow(pointList,counted,q,iir) + mat_copyrow(ptList,counted,q,iir) } iir = iir + 1 } - return pointList + return ptList } # check if the line between 2 point intersect an obstacle function doesItIntersect(point,vector) { #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) - dif = math.vec2.sub(point,vector) - distance = math.vec2.length(dif) - if(distance==0.0){ + var dif = math.vec2.sub(point,vector) + var distance = math.vec2.length(dif) + if(distance == 0.0){ # Find what block we're in right now var xi = math.round(point.x) #+1 var yi = math.round(point.y) #+1 - if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) { if(map[xi][yi] > 0.5) return 1 else @@ -372,21 +393,22 @@ function doesItIntersect(point,vector) { } else return 0 } - vec = math.vec2.scale(dif,1.0/distance) - pathstep = size(map[1]) - 2 + var vec = math.vec2.scale(dif,1.0/distance) + var pathstep = size(map) - 2 idii = 1.0 while(idii <= pathstep) { - range = distance*idii/pathstep - pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); + var range = distance*idii/pathstep + var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); # Find what block we're in right now var xi = math.round(pos_chk.x) #+1 var yi = math.round(pos_chk.y) #+1 - #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) + #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")") - if(xi!=cur_cell.x and yi!=cur_cell.y){ - if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) { + if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){ + #if(xi!=cur_cell.x and yi!=cur_cell.y) { + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { if(map[xi][yi] < 0.5) { # because obstacle use trust values #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!") return 1; @@ -403,20 +425,28 @@ function doesItIntersect(point,vector) { } function getPathGPS(Q,nb){ + #log("-----------CONVERTING PATH!!!") path={} var npt=0 + var isrecursive = 0 # get the path from the point list - while(nb!=1){ + while(nb > 1 and isrecursive!=1){ npt=npt+1 path[npt] = {} path[npt][1]=Q[nb][1] path[npt][2]=Q[nb][2] - nb = Q[nb][3]; + if(nb != Q[Q[nb][3]][3]) + nb = Q[nb][3]; + else { + isrecursive = 1 + path={} + log("ERROR - Recursive path !!!") + } } # re-order the list and make the path points absolute pathR={} - var totpt = npt + var totpt = npt + 1 while(npt > 0){ tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) pathR[totpt-npt] = {} @@ -433,7 +463,7 @@ function getPath(Q,nb){ var npt=0 # log("get the path from the point list") - while(nb!=1){ + while(nb > 1){ npt=npt+1 path[npt] = {} path[npt][1]=Q[nb][1] @@ -444,7 +474,7 @@ function getPath(Q,nb){ # log("re-order the list") # table_print(path.mat) pathR={} - var totpt = npt + var totpt = npt + 1 while(npt > 0){ tmpgoal = getvec(path,npt) pathR[totpt-npt] = {} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 843f34f..501416d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,6 +3,7 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## +include "vec2.bzz" include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. @@ -14,7 +15,6 @@ GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. cur_goal_l = 0 rc_State = 0 -homegps = {.lat=0, .long=0} function uav_initswarm() { s = swarm.create(1) @@ -34,6 +34,7 @@ function idle() { function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff + homegps = {.lat=position.latitude, .long=position.longitude} if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) @@ -61,7 +62,7 @@ function land() { function set_goto(transf) { UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf) + gotoWPRRT(transf) } if(rc_goto.id==id){ @@ -93,10 +94,8 @@ function picture() { # 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) - homegps.lat = position.latitude - homegps.long = position.longitude - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + 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) @@ -104,35 +103,36 @@ function gotoWPRRT(transf) { else { if(Path==nil){ # create the map - if(map==nil) + if(map==nil) { UAVinit_map(rc_goal) + homegps.lat = position.latitude + homegps.long = position.longitude + } # add proximity sensor and neighbor obstacles to the map while(Path==nil) { - #getproxobs(m_pos) - UAVgetneiobs (m_pos) + updateMap(m_pos) Path = UAVpathPlanner(rc_goal, m_pos) } cur_goal_l = 1 } else if(cur_goal_l <= size(Path)) { - cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude - cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) - print(" heading to ", cur_pt.x,cur_pt.y) - if(math.vec2.length(cur_pt)>GOTODIST_TOL) { - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) - UAVgetneiobs(m_pos) - if(check_path(Path,cur_goal_l,m_pos,0)) { + 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) + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) while(Path == nil) { - #getproxobs(m_pos) - UAVgetneiobs (m_pos) + updateMap(m_pos) Path = UAVpathPlanner(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) } - cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt)) - uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude) } else cur_goal_l = cur_goal_l + 1 @@ -262,14 +262,18 @@ function LimitAngle(angle){ return angle } -function vec_from_gps(lat, lon) { - d_lon = lon - position.longitude +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - position.longitude d_lat = lat - 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) + return math.vec2.new(ned_x,ned_y) } function gps_from_vec(vec) { diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testRRT.bzz similarity index 76% rename from buzz_scripts/testalone.bzz rename to buzz_scripts/testRRT.bzz index 7460de4..7590470 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testRRT.bzz @@ -1,4 +1,3 @@ -include "vec2.bzz" 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. @@ -6,15 +5,18 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5085,-73.1531935979886,5.0) - set_goto(picture) + uav_storegoal(45.5088103899,-73.1540826153,25.0) + set_goto(idle) } # Executed once at init time. function init() { - statef=turnedoff - UAVSTATE = "TURNEDOFF" uav_initstig() + uav_initswarm() + #statef=turnedoff + #UAVSTATE = "TURNEDOFF" + statef = takeoff + UAVSTATE = "TAKEOFF" } # Executed at each time step. diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 97b92bf..e438fb1 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -2,7 +2,6 @@ include "vec2.bzz" include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" function action() { uav_storegoal(-1.0,-1.0,-1.0) @@ -13,7 +12,6 @@ function action() { function init() { statef=turnedoff UAVSTATE = "TURNEDOFF" - uav_initstig() TARGET_ALTITUDE = 15.0 }