Diff: 06_AttitudeControl/attitudeControl.cpp

Revision:

2:761e3c932ce0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/06_AttitudeControl/attitudeControl.cpp	Mon Jan 18 06:00:43 2016 +0000
@@ -0,0 +1,97 @@
+/**
+ *  @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]=1;
+    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;
+}
\ No newline at end of file