diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp index e36bd73c7aebab7df67e74506241cfa842d5af2a..e5858f16c3c0e2661616ba6ddff54ebf2538bf3f 100644 --- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp @@ -324,7 +324,7 @@ int main(int argc, char **argv) { /* get latest yaw*/ robot_state_t current_state_; current_state_ = uavs[robotid-1]->get_state(); - float yaw = current_state_.p(2)*M_PI/180.0; // TODO confirm that indeed it is in degrees + float yaw = current_state_.q(3)*M_PI/180.0; // TODO confirm that indeed it is in degrees // float vxl = vel_command_(0)*std::cos(yaw) + vel_command_(1)*std::sin(yaw) ; // float vyl = - vel_command_(0)*std::sin(yaw) + vel_command_(1)*std::cos(yaw) ; float vxl = vel_command(0)*std::cos(yaw) + vel_command(1)*std::sin(yaw) ; @@ -335,7 +335,7 @@ int main(int argc, char **argv) { row_vector_2d_t in; in << vxl, vyl; row_vector_2d_t range; - range << -0.15, 0.15; + range << -0.2, 0.2; row_vector_2d_t out = saturate(in, range); current_command.vx = out(0); current_command.vy = out(1); @@ -418,17 +418,19 @@ int main(int argc, char **argv) { logdata->input.y = input(1,0); logdata->input_formation.x = input(0,1); logdata->input_formation.y = input(1,1); - logdata->input_obstacle.x = input(0,2); - logdata->input_obstacle.y = input(1,2); + logdata->input_obstacle.x = current_command.vx; //input(0,2); + logdata->input_obstacle.y = current_command.vy;//input(1,2); logdata->input_navigation.x = input(0,3); logdata->input_navigation.y = input(1,3); - logdata->input_orientation.x = input(0,4); - logdata->input_orientation.y = input(1,4); + logdata->input_orientation.x = uav_state_p(0, robotid-1); //input(0,4); + logdata->input_orientation.y = uav_state_p(1, robotid-1);// input(1,4); logdata->input_integration.x = input(0,5); logdata->input_integration.y = input(1,5); logdata->input_to_system.x = current_command.vx; logdata->input_to_system.y = current_command.vy; + // uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) + }