300 lines
7.8 KiB
Plaintext
300 lines
7.8 KiB
Plaintext
########################################
|
|
#
|
|
# FLIGHT-RELATED FUNCTIONS
|
|
#
|
|
########################################
|
|
include "utils/vec2.bzz"
|
|
include "act/barrier.bzz"
|
|
include "utils/conversions.bzz"
|
|
include "act/naviguation.bzz"
|
|
include "act/CA.bzz"
|
|
include "act/neighborcomm.bzz"
|
|
|
|
TARGET_ALTITUDE = 15.0 # m.
|
|
BVMSTATE = "TURNEDOFF"
|
|
PICTURE_WAIT = 20 # steps
|
|
WP_STIG = 8
|
|
|
|
path_it = 0
|
|
pic_time = 0
|
|
g_it = 0
|
|
|
|
# Core state function when on the ground
|
|
function turnedoff() {
|
|
BVMSTATE = "TURNEDOFF"
|
|
}
|
|
|
|
# Core state function when hovering
|
|
function idle() {
|
|
BVMSTATE = "IDLE"
|
|
}
|
|
|
|
# Core state function to launch the robot: takeoff and wait for others, or stop (land)
|
|
function launch() {
|
|
BVMSTATE = "LAUNCH"
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
|
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
|
|
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
|
|
barrier_ready(22)
|
|
} else {
|
|
log("Altitude: ", pose.position.altitude)
|
|
neighbors.broadcast("cmd", 22)
|
|
uav_takeoff(TARGET_ALTITUDE)
|
|
}
|
|
} else {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
|
|
barrier_ready(22)
|
|
}
|
|
}
|
|
|
|
# Launch function version without the timeout and stop state.
|
|
function launch_switch() {
|
|
BVMSTATE = "LAUNCH_SWITCH"
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
|
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
|
|
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
|
|
barrier_ready(22)
|
|
} else {
|
|
log("Altitude: ", pose.position.altitude)
|
|
neighbors.broadcast("cmd", 22)
|
|
uav_takeoff(TARGET_ALTITUDE)
|
|
}
|
|
} else {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
|
|
barrier_ready(22)
|
|
}
|
|
}
|
|
|
|
gohomeT=100
|
|
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
|
|
function goinghome() {
|
|
BVMSTATE = "GOHOME"
|
|
if(gohomeT > 0) { # TODO: Make a real check if home is reached
|
|
gohome()
|
|
gohomeT = gohomeT - 1
|
|
} else
|
|
BVMSTATE = AUTO_LAUNCH_STATE
|
|
}
|
|
|
|
# Core state function to stop and land.
|
|
function stop() {
|
|
BVMSTATE = "STOP"
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
|
neighbors.broadcast("cmd", 21)
|
|
uav_land()
|
|
if(pose.position.altitude <= 0.2) {
|
|
BVMSTATE = "TURNEDOFF"
|
|
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
|
|
#barrier_ready(21)
|
|
}
|
|
} else {
|
|
BVMSTATE = "TURNEDOFF"
|
|
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
|
|
#barrier_ready(21)
|
|
}
|
|
}
|
|
|
|
# State functions for individual waypoint control
|
|
firsttimeinwp = 1
|
|
wpreached = 1
|
|
function indiWP() {
|
|
if(firsttimeinwp) {
|
|
v_wp = stigmergy.create(WP_STIG)
|
|
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
|
|
firsttimeinwp = 0
|
|
}
|
|
BVMSTATE = "INDIWP"
|
|
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()
|
|
}
|
|
}
|
|
|
|
#if(vstig_buzzy == 0) {
|
|
wpi = v_wp.get(id)
|
|
if(wpi!=nil) {
|
|
wp = unpackWP2i(wpi)
|
|
if(wp.pro == 0) {
|
|
wpreached = 0
|
|
storegoal(wp.lat, wp.lon, pose.position.altitude)
|
|
var ls = packWP2i(wp.lat, wp.lon, 1)
|
|
v_wp.put(id,ls)
|
|
return
|
|
}
|
|
}
|
|
#}
|
|
|
|
if(wpreached!=1)
|
|
goto_gps(indiWP_done)
|
|
|
|
}
|
|
|
|
function indiWP_done() {
|
|
BVMSTATE = "INDIWP"
|
|
wpreached = 1
|
|
}
|
|
|
|
# State function to take a picture from the camera server (developed by HS)
|
|
function take_picture() {
|
|
BVMSTATE="PICTURE"
|
|
setgimbal(0.0, 0.0, -90.0, 20.0)
|
|
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
|
takepicture()
|
|
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
|
|
BVMSTATE="IDLE"
|
|
pic_time=0
|
|
}
|
|
pic_time=pic_time+1
|
|
}
|
|
|
|
# State function to follow a moving attractor (GPS sent from a user phone)
|
|
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))
|
|
})
|
|
goto_abs(attractor.x, attractor.y, 0.0, 0.0)
|
|
} else {
|
|
log("No target in local table!")
|
|
BVMSTATE = "IDLE"
|
|
}
|
|
}
|
|
|
|
# State function to converge to centroid
|
|
function aggregate() {
|
|
BVMSTATE="AGGREGATE"
|
|
centroid = neighbors.reduce(function(rid, data, centroid) {
|
|
centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
|
|
return centroid
|
|
}, {.x=0, .y=0})
|
|
if(neighbors.count() > 0)
|
|
centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
|
|
cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
|
|
cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
|
|
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
|
|
}
|
|
|
|
# State fucntion to circle all together around centroid
|
|
function pursuit() {
|
|
BVMSTATE="PURSUIT"
|
|
rd = 20.0
|
|
pc = 3.2
|
|
vd = 15.0
|
|
r_vec = neighbors.reduce(function(rid, data, r_vec) {
|
|
r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth))
|
|
return r_vec
|
|
}, {.x=0, .y=0})
|
|
if(neighbors.count() > 0)
|
|
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
|
|
r = math.vec2.length(r_vec)
|
|
var sqr = (r-rd)*(r-rd)+pc*pc*r*r
|
|
gamma = vd / math.sqrt(sqr)
|
|
dr = -gamma * (r-rd)
|
|
dT = gamma * pc
|
|
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
|
|
vfg = LimitSpeed(vfg, 2.0)
|
|
goto_abs(vfg.x, vfg.y, 0.0, 0.0)
|
|
}
|
|
|
|
# Lennard-Jones interaction magnitude
|
|
TARGET = 8.0
|
|
EPSILON = 30.0
|
|
function lj_magnitude(dist, target, epsilon) {
|
|
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
|
return lj
|
|
}
|
|
|
|
# Neighbor data to LJ interaction vector
|
|
function lj_vector(rid, data) {
|
|
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
|
}
|
|
|
|
# Accumulator of neighbor LJ interactions
|
|
function lj_sum(rid, data, accum) {
|
|
return math.vec2.add(data, accum)
|
|
}
|
|
|
|
# Sate function that calculates and actuates the flocking interaction
|
|
function formation() {
|
|
BVMSTATE="FORMATION"
|
|
# Calculate accumulator
|
|
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
|
if(neighbors.count() > 0)
|
|
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
|
|
accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
|
|
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
|
|
}
|
|
|
|
# Custom state function for the developer to play with
|
|
function cusfun(){
|
|
BVMSTATE="CUSFUN"
|
|
#log("cusfun: yay!!!")
|
|
#LOCAL_TARGET = math.vec2.new(5 ,0 )
|
|
#local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
|
|
#if(math.vec2.length(local_set_point) > GOTODIST_TOL){
|
|
# local_set_point = LimitSpeed(local_set_point, 1.0)
|
|
# goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
|
|
#}
|
|
#else{
|
|
# log("TARGET REACHED")
|
|
#}
|
|
|
|
initialPoint = 0
|
|
|
|
# Get waypoint list
|
|
if (id == 1) {
|
|
waypoints = {
|
|
.0 = math.vec2.new(-5.0, -5.0),
|
|
.1 = math.vec2.new(5.0, 5.0)
|
|
}
|
|
} else if (id == 2 ) {
|
|
waypoints = {
|
|
.0 = math.vec2.new(5.0, 5.0),
|
|
.1 = math.vec2.new(-5.0, -5.0)
|
|
}
|
|
}
|
|
|
|
# Current waypoint
|
|
target_point = waypoints[point]
|
|
|
|
# Get drone position and transform to global frame - depends on lauch file
|
|
offset_x = 0.0
|
|
offset_y = 0.0
|
|
if (id == 1) {
|
|
offset_x = 5.0
|
|
offset_y = 5.0
|
|
} else if (id == 2 ) {
|
|
offset_x = -5.0
|
|
offset_y = -5.0
|
|
}
|
|
pos_x = pose.position.x + offset_x;
|
|
pos_y = pose.position.y + offset_y;
|
|
|
|
# Get a vector pointing to target and target distance
|
|
vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y))
|
|
distance_to_target = math.vec2.length(vector_to_target)
|
|
|
|
# Run LCA and increment waypoint
|
|
if(distance_to_target > 0.2){
|
|
vector_to_target = LCA(vector_to_target)
|
|
vector_to_target = LimitSpeed(vector_to_target, 0.5)
|
|
goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0)
|
|
} else {
|
|
if(point < size(waypoints) - 1){
|
|
point = point + 1
|
|
} else {
|
|
point = 0
|
|
}
|
|
}
|
|
} |