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

test

This commit is contained in:
David 2017-02-19 23:06:01 +00:00
parent 34b70eb851
commit 03c35afc17
2 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/roscontroller.cpp
View File

@ -663,9 +663,10 @@ namespace rosbzz_node{
neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
}
double *cvt_neighbours_pos_payload = cvt_neighbours_pos_test;
// cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
//double cvt_neighbours_pos_payload[3];
//cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
// cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
/*pass neighbour position to local maintaner*/

24
src/test1.bzz
View File

@ -12,7 +12,7 @@ function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.0
TARGET_ALTITUDE = 2.0
# Lennard-Jones parameters
TARGET = 50.0 #0.000001001
@ -41,8 +41,8 @@ function hexagon() {
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
# print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
# uav_moveto(accum.x, accum.y)
print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x, accum.y)
}
########################################
@ -55,7 +55,7 @@ function hexagon() {
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 1 # number of robots in the swarm
ROBOTS = 2 # number of robots in the swarm
#
# Sets a barrier
@ -106,10 +106,10 @@ if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/
}
function takeoff() {
#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
# barrier_set(ROBOTS,hexagon)
# barrier_ready()
#}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
if( flight.status !=3){
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
@ -118,10 +118,10 @@ function takeoff() {
}
function land() {
#log("Land: ", flight.status)
#if(flight.status != 0 and flight.status != 4){
# neighbors.broadcast("cmd", 21)
# uav_land()
#}
if(flight.status != 0 and flight.status != 4){
neighbors.broadcast("cmd", 21)
uav_land()
}
uav_land()
}