File content as of revision 11:49344285c82a:

/**
 *  @file attitudeControl.cpp
 *  @brief This file implements attitudeController function
 */

#include "attitudeControl.h"

attitudeControl::attitudeControl()
{
    // System parameters
    // Sphere
    ms = 20;
    rs = 30;
    rs_inner = 58.5/2;
    Is = ( (2.0/5.0) * ms * (pow(rs,5) - pow(rs_inner,5)) / (pow(rs,3) - pow(rs_inner,3)) );

    // Cart
    mc = 70;
    rc = 4.5;
    ri = (12.76 + 9.5/2);
    Ic_x = 15163.66;
    Ic_z = 13938.73;

    // Drive wheel
    mw = 0.19;
    rw = 9.5/2;
    Iw = 2.52;
    h = 14;

    // Gravity
    gravity = 9.82*1000;

    // Theta and Beta Control
    // Control parameters
    int i;
    i=0; //Control parameters for flag = 0
    kx_beta[i]=1000;
    kx_theta[i]=3;
    ks_beta[i]=30;
    ks_theta[i]=5;
    kt_beta[i]=20;
    kt_theta[i]=5;

    i=1; //Control parameters for flag = 1
    kx_beta[i]=1;
    kx_theta[i]=3;
    ks_beta[i]=30;
    ks_theta[i]=5;
    kt_beta[i]=20;
    kt_theta[i]=5;

    // Gamma Control
    // Control gains
    k1 = 2;
    k2 = k1*k1/4;
}

void attitudeControl::GenWheelVelocities(float* w_L, float* w_R, int flag, float tc,
        float beta, float dbeta, float gamma, float dgamma,
        float theta_L, float theta_R, float dtheta_L, float dtheta_R,
        float ref_ddbeta, float ref_dbeta, float ref_beta,
        float ref_ddtheta, float ref_dtheta, float ref_theta,
        float ref_ddgamma,  float ref_dgamma, float ref_gamma)
{
    // Theta and Beta Control
    if (!flag) ref_theta = 0.5*(theta_L + theta_R);
    
    float theta = 0.5*(theta_L + theta_R);
    float dtheta = 0.5*(dtheta_L + dtheta_R);
    float M11 = (ms+mc)*rs*rs + Is + mc*rc*rc + Ic_x - 2*mc*rc*rs*cos(beta);
    float M12 = ((ms+mc)*rs*rs + Is)*rw/ri + 2*Iw - mc*rc*rs*cos(beta)*rw/ri;
    float fx1 = -(mc*rc*rs*sin(beta)*dbeta*dbeta + mc*rc*gravity*sin(beta))/M11;
    float fx2 = 0;
    float gx1 = -M12/M11;
    float gx2 = 1;

    float s_beta = (ref_dbeta - dbeta) + kx_beta[flag]*(ref_beta - beta);
    float s_theta = (ref_dtheta - dtheta) + kx_theta[flag]*(ref_theta - theta);

    float temp1 = ref_ddbeta - fx1 + kx_beta[flag]*(ref_dbeta - dbeta) + ks_beta[flag]*tanh(kt_beta[flag]*s_beta);
    float temp2 = ref_ddtheta - fx2 + kx_theta[flag]*(ref_dtheta - dtheta) + ks_theta[flag]*tanh(kt_theta[flag]*s_theta);
    float u1 = (gx1*temp1 + gx2*temp2)/(gx1*gx1 + gx2*gx2);

    // Gamma Control
    float dphi = 0.5*(dtheta_R - dtheta_L);
    float gamma_e = ref_gamma - gamma;
    float dgamma_e = ref_dgamma - dgamma;
    float A = -rw*Is/(h*(Is+Ic_z));
    float u2 = (ref_ddgamma + k1*dgamma_e + k2*gamma_e)/A;

    // Control Inputs for the drive wheels
    float dtheta_input = dtheta + u1*tc;
    //float dphi_input = dphi + u2*tc;
    float dphi_input = 0.0;
    *w_L = dtheta_input - dphi_input;
    *w_R = dtheta_input + dphi_input;
}