Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add height table and fix large ID bug in ACF

This commit is contained in:
dave 2018-09-11 00:02:20 -04:00
parent 353f169be1
commit 4c5d2e5ef2
6 changed files with 46 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
buzz_scripts/include/act/barrier.bzz
View File

@ -11,7 +11,8 @@ BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
bc = 0;
hvs = 0;
#
# Sets a barrier
@ -23,7 +24,13 @@ function barrier_create() {
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
# BARRIER_VSTIG = BARRIER_VSTIG +1
if(hvs) {
BARRIER_VSTIG = BARRIER_VSTIG -1
hvs = 0
}else{
BARRIER_VSTIG = BARRIER_VSTIG +1
hvs = 1
}
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)

5
buzz_scripts/include/act/states.bzz
View File

@ -12,9 +12,9 @@ TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXVEL = 0.85 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.8 # m.
GOTODIST_TOL = 0.4 # m.
GOTOANG_TOL = 0.1 # rad.
path_it = 0
pic_time = 0
@ -270,7 +270,6 @@ function nei_cmd_listen() {
AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME"
} else if(value==21 and BVMSTATE!="TURNEDOFF") {
AUTO_LAUNCH_STATE = "TURNEDOFF"
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
arm()

41
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -176,7 +176,7 @@ function find(table,value){
#
function packmessage(send_table){
var send_value
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
send_value=1000000*send_table.State+100000*send_table.Label+10000*send_table.ReqLabel+100*send_table.ReqID+send_table.Response
return send_value
}
#
@ -200,14 +200,14 @@ function pack_guide_msg(send_table){
#unpack message
#
function unpackmessage(recv_value){
var wan=(recv_value-recv_value%100000)/100000
recv_value=recv_value-wan*100000
var qian=(recv_value-recv_value%10000)/10000
recv_value=recv_value-qian*10000
var bai=(recv_value-recv_value%1000)/1000
recv_value=recv_value-bai*1000
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
var wan=(recv_value-recv_value%1000000)/1000000
recv_value=recv_value-wan*1000000
var qian=(recv_value-recv_value%100000)/100000
recv_value=recv_value-qian*100000
var bai=(recv_value-recv_value%10000)/10000
recv_value=recv_value-bai*10000
var shi=(recv_value-recv_value%100)/100
recv_value=recv_value-shi*100
var ge=recv_value
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
return_table.State=wan
@ -310,10 +310,10 @@ neighbors.listen("m",
#
#Function used to get the station info of the sender of the message
#
function Get_DisAndAzi(id){
function Get_DisAndAzi(t_id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
if(rid==t_id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
@ -411,7 +411,8 @@ function DoFree() {
function DoAsking(){
if(BVMSTATE!="GRAPH_ASKING"){
BVMSTATE="GRAPH_ASKING"
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
#math.rng.setseed(id)
m_unRequestId=id#math.rng.uniform(0,10) #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
@ -513,17 +514,17 @@ function set_rc_goto() {
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Range=m_MessageRange[IDofPred]#the position of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
var S2Target=math.vec2.add(S2Pred,P2Target)
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
var S2Target=math.vec2.add(S2Pred,P2Target)
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude)
storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1
}
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal (", m_nLabel, " from ", IDofPred, "): ",goal.latitude, goal.longitude, "(", S2Target.x/100, S2Target.y/100, ")")
storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1
}
}
#
#Do joined

10
buzz_scripts/include/utils/takeoff_heights.bzz Normal file
View File

@ -0,0 +1,10 @@
takeoff_heights ={
.1 = 21.0,
.2 = 18.0,
.3 = 12.0,
.5 = 24.0,
.6 = 3.0,
.9 = 15.0,
.11 = 6.0,
.16 = 9.0
}

5
buzz_scripts/main.bzz
View File

@ -6,6 +6,7 @@ include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
include "timesync.bzz"
include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
@ -14,7 +15,7 @@ AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
LOWEST_ROBOT_ID = 0
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 2
ROOT_ID = 3
graph_id = 3
graph_loop = 0
@ -37,7 +38,7 @@ function init() {
# initGraph()
TARGET_ALTITUDE = 4 + (id-LOWEST_ROBOT_ID)*3.0 # m
TARGET_ALTITUDE = takeoff_heights[id] #4 + (id-LOWEST_ROBOT_ID)*3.0 # m
# start the swarm command listener
nei_cmd_listen()

2
src/roscontroller.cpp
View File

@ -1024,7 +1024,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
// To prevent drifting from stable position, uncomment
//if(fabs(x)>0.005 || fabs(y)>0.005) {
// localsetpoint_nonraw_pub.publish(moveMsg);
localsetpoint_nonraw_pub.publish(moveMsg);
//}
}