attitudeControl.h

00001 /**
00002  *  @file attitudeControl.h
00003  *  @brief This file implements attitudeController function
00004  */
00005 
00006 #ifndef attitudeControl_H
00007 #define attitudeControl_H
00008 
00009 #include "mbed.h"
00010 #include "generalFunctions.h"
00011 
00012 class attitudeControl
00013 {
00014 public:
00015 
00016     /**
00017     *  @brief This function implements a sliding-mode controller to correct the pitch angle of the robot and maintiain a stable attitude during motion
00018     *
00019     *  @param [out] w_L        output angular vecolity for left drive wheel
00020     *  @param [out] w_R        output angular vecolity for right drive wheel
00021     *  @param [in] flag        Specify the control mode: flag = 1 trajectory tracking control; flag = 0 speed and yaw control
00022     *  @param [in] tc          Servo time in seconds
00023     *  @param [in] beta        pitch angle
00024     *  @param [in] dbeta       pitch angle change rate
00025     *  @param [in] gamma       cart heading angle
00026     *  @param [in] dgamma      cart heading angle change rate
00027     *  @param [in] theta_L     left wheel angle
00028     *  @param [in] theta_R     right wheel angle
00029     *  @param [in] dtheta_L    left wheel velocity
00030     *  @param [in] dtheta_R    right wheel velocity
00031     *  @param [in] ref_ddbeta  Parameter_Description
00032     *  @param [in] ref_dbeta   Parameter_Description
00033     *  @param [in] ref_beta    Parameter_Description
00034     *  @param [in] ref_ddtheta Parameter_Description
00035     *  @param [in] ref_dtheta  Parameter_Description
00036     *  @param [in] ref_theta   Parameter_Description
00037     *  @param [in] ref_ddgamma Parameter_Description
00038     *  @param [in] ref_dgamma  Parameter_Description
00039     *  @param [in] ref_gamma   Parameter_Description
00040     *  @return                 No value returned
00041     *
00042     *  @details Units: distance in mm, time in second, mass in gram, angle in radian
00043     */
00044 
00045     // Reference
00046     // ref_ddbeta, ref_dbeta, ref_beta,
00047     // ref_ddtheta, ref_dtheta, ref_dtheta,
00048     // ref_ddtheta, ref_dtheta, ref_theta
00049     
00050     attitudeControl();
00051     
00052     void GenWheelVelocities(float* w_L, float* w_R, int flag, float tc,
00053                 float beta, float dbeta, float gamma, float dgamma,
00054                 float theta_L, float theta_R, float dtheta_L, float dtheta_R,
00055                 float ref_ddbeta, float ref_dbeta, float ref_beta,
00056                 float ref_ddtheta, float ref_dtheta, float ref_theta,
00057                 float ref_ddgamma,  float ref_dgamma, float ref_gamma,
00058                 float* u1, float* u2);
00059 
00060 private:
00061     // System parameters
00062     // Sphere
00063     float ms, rs, rs_inner, Is;
00064     
00065     // Cart
00066     float mc, rc, ri;
00067     float Ic_x, Ic_z;
00068 
00069     // Drive wheel
00070     float mw, rw, Iw, h;
00071 
00072     // Gravity
00073     float gravity;
00074 
00075     // Theta and Beta Control
00076     // Control parameters
00077     float kx_beta[2];
00078     float kx_theta[2];
00079     float ks_beta[2];
00080     float ks_theta[2];
00081     float kt_beta[2]; 
00082     float kt_theta[2];
00083 
00084     // Gamma Control
00085     // Control gains
00086     float k1;
00087     float k2;
00088 };
00089 
00090 
00091 #endif