Commit 64f89283 authored by mastefan's avatar mastefan
Browse files

Small changes and var to yaml. Pickup of chocolates working. :)

parent 9272a68c
......@@ -238,6 +238,9 @@ std::vector<float> tol_takeoff (3, 0.0);
std::vector<float> tol_approach (3, 0.0);
std::vector<float> tol_land (3, 0.0);
float land_height = 0.03;
float start_height = 0.3;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms
......
......@@ -34,7 +34,7 @@ xm2_height_over_ms: 0.3
xm1_height: 0.6
# velocity of trajctory
trajectory_velocity_of_CF : 2.50
trajectory_velocity_of_CF : 2.0
trajectory_velocity_of_CF_init : 1
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms
......@@ -45,7 +45,9 @@ xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothersh
# tolerances
tol_takeoff: [0.07, 0.07, 0.07]
tol_approach: [0.3, 0.3, 0.1]
tol_land: [0.07, 0.07, 0.04]
tol_land: [0.01, 0.01, 0.05]
land_height: 0.3
start_height: 0.9
......
......@@ -244,7 +244,45 @@ void buttonPressed_integrator_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
integratorFlag = DRONEX_INTEGRATOR_RESET;
//New:
}
void setarrayzero3(float arr[3], int n){
for(int i = 0; i<n; i++){
arr[i] = 0;
}
}
void setarrayzero4(float arr[4], int n){
for(int i = 0; i<n; i++){
arr[i] = 0;
}
}
void buttonPressed_follow_trajectory(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");
// Initialize trajectory timers
flightSequence = SEQUENCE_NONE;
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
total_time_since_start = 0;
//trajectory_x_radius = 0;
//trajectory_y_radius = 0;
first_trajectory_calculation = true;
trajectory_start_time = ros::Time::now().toSec();
ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
}
void buttonPressed_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET");
m_time_ticks = 0;
setarrayzero3(integrator_sum_XYZ, 3);
//integrator_sum_XYZ = {0,0,0};
......@@ -254,20 +292,11 @@ void buttonPressed_integrator_reset(){
savedStartCoordinates = false;
savedStartCoordinatesFromMS = false;
std::fill(tol_takeoff.begin(),tol_takeoff.end(), 0);
std::fill(tol_approach.begin(),tol_approach.end(), 0);
std::fill(tol_land.begin(),tol_land.end(), 0);
/*std::vector<float> tol_takeoff (3, 0.0);
std::vector<float> tol_approach (3, 0.0);
std::vector<float> tol_land (3, 0.0);
*/
trajectory_start_time = 0;
total_time_since_start = 0;
first_trajectory_calculation = true;
......@@ -275,15 +304,11 @@ void buttonPressed_integrator_reset(){
trajectory_deltaT_position = 0;
trajectory_deltaT_velocity = 0;
/*xcf0 = */std::fill(xcf0.begin(),xcf0.end(), 0);
std::fill(xcf0.begin(),xcf0.end(), 0);
std::fill(xm1.begin(),xm1.end(), 0);
std::fill(xm2.begin(),xm2.end(), 0);
std::fill(xms.begin(),xms.end(), 0);
/*std::vector<float> xm1(&3, 0.0);
std::vector<float> xm2(&3, 0.0);
std::vector<float> xms(&3, 0.0);
*/
trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
......@@ -341,43 +366,12 @@ void buttonPressed_integrator_reset(){
thrust_factor = 1;
reset_rviz = false;
flightSequence = 0;
}
void setarrayzero3(float arr[3], int n){
for(int i = 0; i<n; i++){
arr[i] = 0;
}
}
void setarrayzero4(float arr[4], int n){
for(int i = 0; i<n; i++){
arr[i] = 0;
}
}
void buttonPressed_follow_trajectory(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");
// Initialize trajectory timers
flightSequence = SEQUENCE_NONE;
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
total_time_since_start = 0;
//trajectory_x_radius = 0;
//trajectory_y_radius = 0;
first_trajectory_calculation = true;
trajectory_start_time = ros::Time::now().toSec();
ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
}
void buttonPressed_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET");
std::string m_namespace = ros::this_node::getNamespace();
namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
}
......@@ -517,10 +511,17 @@ void WeightParamCallback (const Setpoint& weightParam ) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
// Keep track of time
m_time_ticks++;
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
if(request.otherCrazyflies[0].occluded){
ROS_ERROR_STREAM("Where is the mothership???");
}
ROS_INFO_STREAM("setpoint: " << dronexSetpoint.x << ", " << dronexSetpoint.y << ", " << dronexSetpoint.z);
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
......@@ -595,7 +596,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
dronexSetpoint.z = request.otherCrazyflies[0].z + land_height;//+ 0.05;
//}else {
//
......@@ -624,7 +625,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
thrust_factor = 1;
integratorFlag = DRONEX_INTEGRATOR_ON;
m_shouldSmoothSetpointChanges = true;
//Check where we start from, if from mothership first go straight up to a certain height
// before aiming the startCoordinates.
if(/*previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP && */taking_off_from_ms_height_gain){
......@@ -645,7 +646,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = startCoordinateFromMS[0];
dronexSetpoint.y = startCoordinateFromMS[1];
dronexSetpoint.z = startCoordinateFromMS[2] + 0.4;
dronexSetpoint.z = startCoordinateFromMS[2] + start_height;
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
......@@ -680,7 +681,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4;
dronexSetpoint.z = startCoordinateZ + start_height;
// For debugging the integrator
......@@ -706,7 +707,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_HOVER:
{
integratorFlag = DRONEX_INTEGRATOR_ON;
//integratorFlag = DRONEX_INTEGRATOR_ON;
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
......@@ -820,7 +821,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){ // Trajectory Follower
// to implement the trajectory tracking
m_shouldSmoothSetpointChanges = false;
m_shouldSmoothSetpointChanges = true;
calculateTrajectory(request);
......@@ -852,7 +853,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = request.otherCrazyflies[0].x; // setpoint on mothership
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
dronexSetpoint.z = request.otherCrazyflies[0].z + land_height;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;
dronexVelocity.x = mothership_vel[0]; // velocity of mothership
......@@ -889,14 +890,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Turn motors off if wanted or do LQR-control
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.05 )){
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.06 )){
ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
flying_state = DRONEX_STATE_GROUND;
previous_flying_state = DRONEX_STATE_LAND_ON_GROUND;
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
(abs(request.ownCrazyflie.z - 0.05 - request.otherCrazyflies[0].z) < tol_land[2]) ){
(abs(request.ownCrazyflie.z - land_height - request.otherCrazyflies[0].z) < tol_land[2]) ){
ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
......@@ -904,7 +905,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
motorsOFF(response);
ROS_INFO("On Mothership");
//ROS_INFO("On Mothership");
}else if(flying_state == DRONEX_STATE_GROUND){
motorsOFF(response);
}
......@@ -1132,19 +1133,19 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
if(direction_ms_to_cf_ms_bodyframe[0] > abs(direction_ms_to_cf_ms_bodyframe[1])){
xm2_sign[0] = 1.0;
xm2_sign[1] = 0.0; // Crazyflie can land from behind
xm2_sign[1] = 0.0; // Crazyflie can land from front
}
else if(direction_ms_to_cf_ms_bodyframe[1] > abs(direction_ms_to_cf_ms_bodyframe[0])){
xm2_sign[0] = 0.0;
xm2_sign[1] = -1.0; // Crazyflie can land from front
xm2_sign[1] = -1.0; // Crazyflie can land from right
}
else if(-direction_ms_to_cf_ms_bodyframe[0] > abs(direction_ms_to_cf_ms_bodyframe[1])){
xm2_sign[0] = -1.0;
xm2_sign[1] = 0.0; // Crazyflie can land from front
xm2_sign[1] = 0.0; // Crazyflie can land from back
}
else if(-direction_ms_to_cf_ms_bodyframe[1] > abs(direction_ms_to_cf_ms_bodyframe[0])){
xm2_sign[0] = 0.0;
xm2_sign[1] = 1.0; // Crazyflie can land from front
xm2_sign[1] = 1.0; // Crazyflie can land from left
}
......@@ -1229,6 +1230,38 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_return[4] = mothership_vel[1];
trajectory_return[5] = mothership_vel[2];
//ROS_INFO_STREAM("trajectory_return[0] = xms[0]");
// if(!xm2_arrived){
// xm2save = ros::Time::now().toSec();
// xm2_arrived = true;
// }
// trajectory_return[0] = xms[0];
// trajectory_return[1] = xms[1];
// trajectory_return[2] = xms[2];
// if( (trajectory_velocity[3] - mothership_vel[0]) < tol_smoothxm2 &&
// (trajectory_velocity[2] - mothership_vel[1]) < tol_smoothxm2 &&
// (trajectory_velocity[3] - mothership_vel[0] < tol_smoothxm2) ){
// trajectory_return[3] = mothership_vel[0];
// trajectory_return[4] = mothership_vel[1];
// trajectory_return[5] = mothership_vel[2];
// }else{
// trajectory_return[3] = (mothership_vel[0] - trajectory_velocity[0])/smoothxm2Time*(ros::Time::now().toSec() - xm2save) + trajectory_velocity[0];
// trajectory_return[4] = (mothership_vel[1] - trajectory_velocity[1])/smoothxm2Time*(ros::Time::now().toSec() - xm2save) + trajectory_velocity[1];
// trajectory_return[5] = (mothership_vel[2] - trajectory_velocity[2]) / smoothxm2Time*(ros::Time::now().toSec() - xm2save) + trajectory_velocity[2];
// }
}
......@@ -1312,17 +1345,19 @@ void motorsOFF(Controller::Response &response){
// Estimate mothership velocity
void calculateMSVelocity(Controller::Request &request){
prev_MS_pos[0] = current_MS_pos[0];
prev_MS_pos[1] = current_MS_pos[1];
prev_MS_pos[2] = current_MS_pos[2];
if(!request.otherCrazyflies[0].occluded){
prev_MS_pos[0] = current_MS_pos[0];
prev_MS_pos[1] = current_MS_pos[1];
prev_MS_pos[2] = current_MS_pos[2];
current_MS_pos[0] = request.otherCrazyflies[0].x;
current_MS_pos[1] = request.otherCrazyflies[0].y;
current_MS_pos[2] = request.otherCrazyflies[0].z;
current_MS_pos[0] = request.otherCrazyflies[0].x;
current_MS_pos[1] = request.otherCrazyflies[0].y;
current_MS_pos[2] = request.otherCrazyflies[0].z;
mothership_vel[0] = (current_MS_pos[0] - prev_MS_pos[0])*m_vicon_frequency;
mothership_vel[1] = (current_MS_pos[1] - prev_MS_pos[1])*m_vicon_frequency;
mothership_vel[2] = (current_MS_pos[2] - prev_MS_pos[2])*m_vicon_frequency;
mothership_vel[0] = (current_MS_pos[0] - prev_MS_pos[0])*m_vicon_frequency;
mothership_vel[1] = (current_MS_pos[1] - prev_MS_pos[1])*m_vicon_frequency;
mothership_vel[2] = (current_MS_pos[2] - prev_MS_pos[2])*m_vicon_frequency;
}
//ROS_INFO_STREAM("Velocity: vx " << mothership_vel[0] << ", vy " << mothership_vel[1] << ", vz " << mothership_vel[2]);
......@@ -2409,6 +2444,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_dronexController, "tol_takeoff", tol_takeoff, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "tol_approach", tol_approach, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "tol_land", tol_land, 3);
start_height = getParameterFloat(nodeHandle_for_dronexController, "start_height");
land_height = getParameterFloat(nodeHandle_for_dronexController, "land_height");
// > For the integrator
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorRate", gainIntegratorRate, 3);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment