Solution to the Mars Lander Problem

mars-lander.cpp

/*
 * https://www.codingame.com/training/medium/mars-lander-episode-2
 *
 * @author Dave Walter <[email protected]>
 */

#include <iostream>
#include <string>
#include <vector>
#include <algorithm>
#include <utility>  //std::pair
#include <vector>
#include <math.h> //fabs, atan2, M_PI
#include <cmath>

#define MIN_ANGLE -90.0
#define MAX_ANGLE 90.0
#define MIN_POWER 0.0
#define MAX_POWER 4.0
#define TARGET_VERTICAL_SPEED_FINAL -20.0
#define TARGET_VERTICAL_SPEED_LANDING -35.0
#define TARGET_VERTICAL_SPEED_ASCENT 10.0
#define TARGET_VERTICAL_SPEED_LATERAL 0.0
#define ASCENT_REL_ALT_LIMIT 500
#define LANDING_ALT 200.0
#define MAX_VERTICAL 3000.0
#define MAX_HORIZONTAL 7000.0
#define RAD2DEG 180.0/M_PI
#define G_CONSTANT 3.711

typedef std::pair<double, double> CoordXY;

typedef struct _landing_setpoint {
    CoordXY landing_coordinate;
    double landing_radius;
} landing_setpoint_t;

typedef enum {
    INITIAL = 0,
    ASCENT,
    LATERAL,
    LANDING,
    FINAL,
} vehicle_mode_t;

typedef struct _vehicle_state {
    double alt = 0.0;
    double rel_alt = 0.0;
    double pos_x = 0.0;
    double vel_x = 0.0;
    double vel_y = 0.0;
    double fuel = 0.0;
    double power = 0.0;
    double angle = 0.0;
    vehicle_mode_t mode = INITIAL;
} vehicle_state_t;

typedef struct _vehicle_setpoint {
    double pos_x = 0.0;
    double pos_y = 0.0;
    double error_pos_x = 0.0;
    double vel_x = 0.0;
    double vel_y = 0.0;
    double force_x = 0.0;
    double force_y = 0.0;
    double radius = 0.0;
} vehicle_setpoint_t;

typedef struct _pid_gains {
    double p_gain = 0.0;
    double i_gain = 0.0;
    double d_gain = 0.0;
} pid_gains_t;


/* @brief Constrains val between min and max
*/
double constrain(double val, double min, double max)
{
    if(val < min) {
        return min;
    } else if( val > max) {
        return max;
    }
}

/* @brief Calculates the linear interpolation between two 
*         values a + t(b-a) 
*/
double lerp(double a, double b, double t)
{
    return a + t * (b - a);
}

/* @brief Calculates the center of the landing zone
*
* @param surface: vector representing the x,y pairs of the landing surface
* @return CoordXY representing x,y coordinates of landing target
*/
landing_setpoint_t calculate_landing_center(const std::vector<CoordXY>& surface_coordinates_xy) 
{
    CoordXY landing_coord_begin = surface_coordinates_xy.front();
    CoordXY landing_coord_end = surface_coordinates_xy.back();
    for(auto coord_xy : surface_coordinates_xy) {
        std::cerr << coord_xy.first << ", " << coord_xy.second << std::endl; 
        // skip the first coordinate
        if(coord_xy.first == landing_coord_begin.first) {
            continue;
        }
        else {
            // determine if we are level (y-coordinate) with previous coordinate
            if(fabs(coord_xy.second - landing_coord_begin.second) < 1e-6) {
                // we have the landing zone
                landing_coord_end = coord_xy;
                break;
            } else {
                landing_coord_begin = coord_xy;
            }
        }
    }
    double x_center = (landing_coord_end.first - landing_coord_begin.first)/2.0 + landing_coord_begin.first;
    double y_center = landing_coord_end.second;

    CoordXY landing_center_xy(x_center, y_center);
    double landing_radius = (landing_coord_end.first - landing_coord_begin.first)/2.0;

    landing_setpoint_t landing_setpoint = {landing_center_xy, landing_radius};

    return landing_setpoint;
    std::cerr << "landing target at: " << x_center << ", " << y_center << " with r: " << landing_radius << std::endl;
}


/* @brief Calculates the velocity setpoint to control horizontal position. generates x velocity setpoint
*         Runs a basic P controller on the position error
*/
void control_x_position(const vehicle_state_t& state, const pid_gains_t& pid, vehicle_setpoint_t& setpoint)
{

    double error_pos_x =  setpoint.pos_x - state.pos_x;

    setpoint.vel_x = error_pos_x * pid.p_gain;

    double t = 1.0;

    // if we are within 25% of landing radius, set horizontal speed to 0
    if(fabs(error_pos_x) < setpoint.radius *0.25) {
        setpoint.vel_x = 0.0;
    // if we are within the landing radius, start reducing horizontal speed
    } else if(fabs(error_pos_x) < setpoint.radius) {
        t = fabs(error_pos_x)/(setpoint.radius*0.75);
        setpoint.vel_x = lerp(0.0, setpoint.vel_x, t);
    }

    setpoint.error_pos_x = error_pos_x;

    std::cerr << "x_pos: " << state.pos_x
              << ", x_err: " << error_pos_x
              << ", x_vel_sp: " << setpoint.vel_x
              << ", t: " << t
              << ", rel_alt: " << state.rel_alt
              << std::endl;
 
}

/* @brief Calculate the horizontal force needed to control horizontal velocity, generates x force
*/
void control_x_velocity(const vehicle_state_t& state, const pid_gains_t& pid, vehicle_setpoint_t& setpoint)
{

    double error_vel_x = setpoint.vel_x -  state.vel_x;
    
    setpoint.force_x = error_vel_x * pid.p_gain;

    if(state.mode == FINAL) {
        setpoint.force_x = 0.0;
    }
    
    std::cerr << "x_vel: " << state.vel_x
              << ", x_vel_err: " << error_vel_x
              << ", x_force: " << setpoint.force_x
              << std::endl;

}

/* @brief Calculate the vertical force need to control vertical velocity generate y force
*/
void control_y_velocity(const vehicle_state_t& state, const pid_gains_t& pid, vehicle_setpoint_t& setpoint)
{
    // schedule vertical speed based on distance from landing
    
    // if below the landing point need more vertical
    switch(state.mode) {
        case ASCENT: {
            setpoint.vel_y = TARGET_VERTICAL_SPEED_ASCENT;
            break;
        }
        case LATERAL: {
            setpoint.vel_y = TARGET_VERTICAL_SPEED_LATERAL;
            break;
        }
        case LANDING: {
            setpoint.vel_y = TARGET_VERTICAL_SPEED_LANDING;
            break;
        }
        case FINAL: {
            setpoint.vel_y = TARGET_VERTICAL_SPEED_FINAL;
            break;
        }
        default:
            setpoint.vel_y = TARGET_VERTICAL_SPEED_LANDING;
    }
    

    double error_vel_y = setpoint.vel_y - state.vel_y;
    setpoint.force_y = G_CONSTANT + pid.p_gain * error_vel_y;


    std::cerr << "y_vel: " << state.vel_y
              << ", y_vel_err: " << error_vel_y
              << ", y_force: " << setpoint.force_y
              << std::endl;
}

/* @brief Calculate the angle to achieve force
* 
*  @return angle in degrees
*/
double calculate_angle(const vehicle_setpoint_t& setpoint)
{
    // atan2 returns angle between vector and y axis
    double angle_raw =  atan2(setpoint.force_y, setpoint.force_x)*RAD2DEG;
    return -1.0 * (90 - angle_raw);
}

/* @brief Calculate the total thrust
*/
double calculate_thrust(const vehicle_setpoint_t& setpoint)
{
    return (sqrt(pow(setpoint.force_y,2) + pow(setpoint.force_x,2)));
}

/* @brief reduces the demand prioritizing vertical thrust over horizontal
*/
void mix_reducer(vehicle_setpoint_t& setpoint)
{
    double thrust = calculate_thrust(setpoint);
    
    // if thrust is more than 4, reduce horizontal force
    if(thrust > MAX_POWER) {
        double delta_thrust = thrust - MAX_POWER;
        if(setpoint.force_x > 0.0) {
            setpoint.force_x -= delta_thrust;
        } else if(setpoint.force_x < 0.0) {
            setpoint.force_x += delta_thrust;
        }

    }
}

void update_mode(vehicle_state_t& state, vehicle_setpoint_t& setpoint) {
 
    // above LZ
    if(fabs(setpoint.error_pos_x) < setpoint.radius) {
        if(state.rel_alt < LANDING_ALT) {
            std::cerr << "FINAL" << std::endl;
            state.mode = FINAL;
        } else {
            std::cerr << "LANDING" << std::endl;
            state.mode = LANDING;
        }
    // not above LZ and too low
    } else if(state.rel_alt < ASCENT_REL_ALT_LIMIT) {
        std::cerr << "ASCENT" << std::endl;
        state.mode = ASCENT;
    // if far from LZ maintain altitude
    } else if(fabs(setpoint.error_pos_x) > setpoint.radius ){
        std::cerr << "LATERAL" << std::endl;
        state.mode = LATERAL;

    } else {
            std::cerr << "LANDING" << std::endl;
            state.mode = LANDING;
    }

}

/* @brief at final landing stage (10m) forces angle to 0.
*/
void check_final(const vehicle_state_t& state, vehicle_setpoint_t& setpoint)
{
    if(state.mode == FINAL) {
        setpoint.force_x = 0.0;
    }
}

int main()
{
    int surface_n; // the number of points used to draw the surface of Mars.
    std::vector<CoordXY> surface_coordinates_xy;

    // the vehicle state as reported
    vehicle_state_t vehicle_state = {};

    // the calculated setpoints to control
    vehicle_setpoint_t vehicle_setpoint = {};

    // the x position PID control gains
    // these values are manually tuned
    pid_gains_t pid_pos_x = {0.02, 0.0, 0.0};
    pid_gains_t pid_vel_x = {0.5, 0.0, 0.0};
    pid_gains_t pid_vel_y = {0.1, 0.0, 0.0};

    std::cin >> surface_n; std::cin.ignore();
    for (int i = 0; i < surface_n; i++) {
        double land_x; // X coordinate of a surface point. (0 to 6999)
        double land_y; // Y coordinate of a surface point. By linking all the points together in a sequential fashion, you form the surface of Mars.
        std::cin >> land_x >> land_y; std::cin.ignore();
        surface_coordinates_xy.push_back(CoordXY(land_x, land_y));
    }

    landing_setpoint_t landing_setpoint = calculate_landing_center(surface_coordinates_xy);
    std::cerr << "landing target at: " << landing_setpoint.landing_coordinate.first << ", " 
              << landing_setpoint.landing_coordinate.second << ", with r: " 
              << landing_setpoint.landing_radius 
              << std::endl;
    
    vehicle_setpoint.pos_x = landing_setpoint.landing_coordinate.first;
    vehicle_setpoint.radius = landing_setpoint.landing_radius;

    // game loop
    while (1) {
        // load in vehicle state
        std::cin >> vehicle_state.pos_x >> vehicle_state.alt 
                 >> vehicle_state.vel_x >> vehicle_state.vel_y 
                 >> vehicle_state.fuel >> vehicle_state.angle >> vehicle_state.power; std::cin.ignore();

        vehicle_state.rel_alt = vehicle_state.alt - landing_setpoint.landing_coordinate.second;

        // update the vehicle mode for vertical velocity control scheduling
        update_mode(vehicle_state, vehicle_setpoint);

        // control for x position and velocity
        control_x_position(vehicle_state, pid_pos_x, vehicle_setpoint);
        control_x_velocity(vehicle_state, pid_vel_x, vehicle_setpoint);

        // control for y velocity
        control_y_velocity(vehicle_state, pid_vel_y, vehicle_setpoint);

        // ensure vertical at landing
        check_final(vehicle_state, vehicle_setpoint);

        // reduce horizontal thrust if exceeding max thrust
        mix_reducer(vehicle_setpoint);

        // calculate the final thrust and angle
        double angle = calculate_angle(vehicle_setpoint);
        double thrust = calculate_thrust(vehicle_setpoint);

        // constrain and convert commands to int
        int angle_int = std::roun