automobile_simple.cc

/*
 * automobile_simple.cc
 *
 * Programmer(s): Michael Paluszek
 *
 * Copyright (c) 2013 Princeton Satellite Systems. All rights reserved.
 *
 */

#include "automobile_simple.h"
#include <math.h>


automobile_simple::automobile_simple(dsim_model_setup *setup) : dsim_model(setup)
{
}

automobile_simple::~automobile_simple()
{
}
        
void automobile_simple::initialize_data()
{               
    // Parameters
    double steering_angle           = 0;
    double torque                   = 0;
    double rolling_friction_coeff   = 0.01; // Ordinary car tires on concrete
    double mass                     = 907.1890;
    double inertia                  = 514.0709;
    double rho                      = 0.2;
    double drag_coeff               = 0.25;
    double frontal_area             = 4.0;
    ml_matrix r                     = "[1.2 1.2 -1 -1;-0.7 0.7 -0.7 0.7]";
    ml_matrix state(6,1);
    ml_matrix position(2,1);
    ml_matrix yaw_matrix(2,2); yaw_matrix.identity();
    
 
    steering_angle_dsim         = create_input("steering_angle",                sd_type_double, &steering_angle,            1,1,    "rad",              "Steering angle");
    torque_dsim                 = create_input("torque",                        sd_type_double, &torque,                    1,1,    "Nm",               "Motor torque");
    
    rolling_friction_coeff_dsim = create_parameter("rolling_friction_coeff",    sd_type_double, &rolling_friction_coeff,    1,1,    "",                 "Rolling friction coefficient"); 
    drag_coeff_dsim             = create_parameter("drag_coeff_coeff",          sd_type_double, &drag_coeff,                1,1,    "",                 "Drag coefficient"); 
    frontal_area_dsim           = create_parameter("frontal_area",              sd_type_double, &frontal_area,              1,1,    "m^2",              "Frontal area"); 
    inertia_dsim                = create_parameter("inertia",                   sd_type_double, &inertia,                   1,1,    "kg-m^2",           "Yaw inertia"); 
    mass_dsim                   = create_parameter("mass",                      sd_type_double, &mass,                      1,1,    "kg",               "Rolling friction coefficient"); 
    r_dsim                      = create_parameter("r",                         sd_type_double, &r,                         2,4,    "m",                "Tire position"); 
    rho_dsim                    = create_parameter("rho",                       sd_type_double, &rho,                       1,1,    "m",                "Tire radius"); 
    
    state_dsim                  = create_integrated_state("state",              sd_type_matrix, &state,                     6,1,    "m,m/s,rad,rad/s",  "State"); 
    position_dsim               = create_output("position",                     sd_type_matrix, &position,                  2,1,    "m",                "Position");
    yaw_matrix_dsim             = create_output("yaw_matrix",                   sd_type_matrix, &yaw_matrix,                2,2,    "",                 "Yaw rotation matrix");
  
    steering_angle_dsim.mark_telemetry_and_command();
    torque_dsim.mark_telemetry_and_command();
    rolling_friction_coeff_dsim.mark_telemetry();
    inertia_dsim.mark_telemetry();
    mass_dsim.mark_telemetry();
    rho_dsim .mark_telemetry();
    r_dsim.mark_telemetry();
    state_dsim.mark_telemetry();
    position_dsim.mark_telemetry();
    yaw_matrix_dsim.mark_telemetry();
    drag_coeff_dsim.mark_telemetry();
    frontal_area_dsim.mark_telemetry();
    
    configure_timestep(false,true,false);
}

void automobile_simple::initialization_complete()
{
}

void automobile_simple::initialize_timestep()
{
}

void automobile_simple::rhs(double t,double jd)
{
    double delta    = steering_angle_dsim.value_as_double();
    double torque   = torque_dsim.value_as_double();
    double cF       = rolling_friction_coeff_dsim.value_as_double();
    double inr      = inertia_dsim.value_as_double();
    double m        = mass_dsim.value_as_double();
    double rho      = rho_dsim.value_as_double();
    double c_d      = drag_coeff_dsim.value_as_double();
    double a        = frontal_area_dsim.value_as_double();
    ml_matrix r     = r_dsim.value_as_matrix();
    ml_matrix x     = state_dsim.value_as_matrix();
    
    const double g = 9.806; // Acceleration of gravity (m/s^2)
    
    // Local variables
    double vX       = x.get(3,1);
    double vY       = x.get(4,1);
    double omega    = x.get(6,1);
    
    double f_friction = -cF*m*g;
    
    if( vX <= 0 ) f_friction = 0;
    double f_traction = 0.5*torque/rho + f_friction;
    
    ml_matrix x_dot(6,1);
    
    // Kinematics
    x_dot(1,1)      = vX;
    x_dot(2,1)      = vY;
    x_dot(5,1)      = omega;
    
    // Cosine and sine of the wheel angles
    double c        = cos(delta);
    double s        = sin(delta);
    
    // Force components
    double fX1      = f_traction*c;
    double fY1      = f_traction*s;
    
    double fX2      = f_traction*c;
    double fY2      = f_traction*s;
    
    double fX3      = f_friction;
    double fY3      = 0;
    
    double fX4      = f_friction;
    double fY4      = 0;
    
    // Force summations
    double fX       = fX1 + fX2 + fX3 + fX4 - 0.5*1.225*c_d*a*pow(vX,2);
    double fY       = fY1 + fY2 + fY3 + fY4;
    
    // Dynamical equations in the body frame
    x_dot(3,1)      = fX/m + omega*vY;
    x_dot(4,1)      = fY/m - omega*vX;
    
    // Torque
    double T        = r.get(2,1)*fX1 - r.get(1,1)*fY1 + r.get(2,2)*fX2 - r.get(1,2)*fY2
                    + r.get(2,3)*fX3 - r.get(1,3)*fY3 + r.get(2,4)*fX4 - r.get(1,4)*fY4;
    
    // Yaw dynamics
    x_dot(6,1)      = T/inr;
   
    state_dsim.set_derivative(x_dot);
}

void automobile_simple::complete_timestep()
{
        dsim_model::complete_timestep();
    
    ml_matrix state = state_dsim.value_as_matrix();
    double theta    = state.get(5,1);
    double c        = cos(theta);
    double s        = sin(theta);
    
    ml_matrix yaw_matrix(2,2); yaw_matrix(1,1) = c; yaw_matrix(1,2) = s; yaw_matrix(2,1) = -s; yaw_matrix(2,2) = c;
    ml_matrix pos = yaw_matrix*state.sub_matrix(1,2,1,1);
    
    position_dsim.set_value_as_matrix(pos);
    yaw_matrix_dsim.set_value_as_matrix(yaw_matrix);

}

extern "C"
{
    dsim_model *AutomobileSimple(void *setup);

        dsim_model *AutomobileSimple(void *setup)
        {
                return new automobile_simple((dsim_model_setup *)setup);
        }       
}