test
This commit is contained in:
parent
34b70eb851
commit
03c35afc17
|
@ -663,7 +663,8 @@ 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));
|
||||
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
|
||||
|
|
|
@ -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()
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue