Diff: 06_AttitudeControl/attitudeControl.h

Revision:

2:761e3c932ce0

Child:

19:7345688260b2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/06_AttitudeControl/attitudeControl.h	Mon Jan 18 06:00:43 2016 +0000
@@ -0,0 +1,89 @@
+/**
+ *  @file attitudeControl.h
+ *  @brief This file implements attitudeController function
+ */
+
+#ifndef attitudeControl_H
+#define attitudeControl_H
+
+#include "mbed.h"
+
+class attitudeControl
+{
+public:
+
+    /**
+    *  @brief This function implements a sliding-mode controller to correct the pitch angle of the robot and maintiain a stable attitude during motion
+    *
+    *  @param [out] w_L        output angular vecolity for left drive wheel
+    *  @param [out] w_R        output angular vecolity for right drive wheel
+    *  @param [in] flag        Specify the control mode: flag = 1 trajectory tracking control; flag = 0 speed and yaw control
+    *  @param [in] tc          Servo time in seconds
+    *  @param [in] beta        pitch angle
+    *  @param [in] dbeta       pitch angle change rate
+    *  @param [in] gamma       cart heading angle
+    *  @param [in] dgamma      cart heading angle change rate
+    *  @param [in] theta_L     left wheel angle
+    *  @param [in] theta_R     right wheel angle
+    *  @param [in] dtheta_L    left wheel velocity
+    *  @param [in] dtheta_R    right wheel velocity
+    *  @param [in] ref_ddbeta  Parameter_Description
+    *  @param [in] ref_dbeta   Parameter_Description
+    *  @param [in] ref_beta    Parameter_Description
+    *  @param [in] ref_ddtheta Parameter_Description
+    *  @param [in] ref_dtheta  Parameter_Description
+    *  @param [in] ref_theta   Parameter_Description
+    *  @param [in] ref_ddgamma Parameter_Description
+    *  @param [in] ref_dgamma  Parameter_Description
+    *  @param [in] ref_gamma   Parameter_Description
+    *  @return                 No value returned
+    *
+    *  @details Units: distance in mm, time in second, mass in gram, angle in radian
+    */
+
+    // Reference
+    // ref_ddbeta, ref_dbeta, ref_beta,
+    // ref_ddtheta, ref_dtheta, ref_dtheta,
+    // ref_ddtheta, ref_dtheta, ref_theta
+    
+    attitudeControl();
+    
+    void 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);
+
+private:
+    // System parameters
+    // Sphere
+    float ms, rs, rs_inner, Is;
+    
+    // Cart
+    float mc, rc, ri;
+    float Ic_x, Ic_z;
+
+    // Drive wheel
+    float mw, rw, Iw, h;
+
+    // Gravity
+    float gravity;
+
+    // Theta and Beta Control
+    // Control parameters
+    float kx_beta[2];
+    float kx_theta[2];
+    float ks_beta[2];
+    float ks_theta[2];
+    float kt_beta[2]; 
+    float kt_theta[2];
+
+    // Gamma Control
+    // Control gains
+    float k1;
+    float k2;
+};
+
+
+#endif
\ No newline at end of file