Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
06_AttitudeControl/attitudeControl.h@2:761e3c932ce0, 2016-01-18 (annotated)
- Committer:
- akashvibhute
- Date:
- Mon Jan 18 06:00:43 2016 +0000
- Revision:
- 2:761e3c932ce0
- Child:
- 19:7345688260b2
guess this is the initial commit :P
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 2:761e3c932ce0 | 1 | /** |
akashvibhute | 2:761e3c932ce0 | 2 | * @file attitudeControl.h |
akashvibhute | 2:761e3c932ce0 | 3 | * @brief This file implements attitudeController function |
akashvibhute | 2:761e3c932ce0 | 4 | */ |
akashvibhute | 2:761e3c932ce0 | 5 | |
akashvibhute | 2:761e3c932ce0 | 6 | #ifndef attitudeControl_H |
akashvibhute | 2:761e3c932ce0 | 7 | #define attitudeControl_H |
akashvibhute | 2:761e3c932ce0 | 8 | |
akashvibhute | 2:761e3c932ce0 | 9 | #include "mbed.h" |
akashvibhute | 2:761e3c932ce0 | 10 | |
akashvibhute | 2:761e3c932ce0 | 11 | class attitudeControl |
akashvibhute | 2:761e3c932ce0 | 12 | { |
akashvibhute | 2:761e3c932ce0 | 13 | public: |
akashvibhute | 2:761e3c932ce0 | 14 | |
akashvibhute | 2:761e3c932ce0 | 15 | /** |
akashvibhute | 2:761e3c932ce0 | 16 | * @brief This function implements a sliding-mode controller to correct the pitch angle of the robot and maintiain a stable attitude during motion |
akashvibhute | 2:761e3c932ce0 | 17 | * |
akashvibhute | 2:761e3c932ce0 | 18 | * @param [out] w_L output angular vecolity for left drive wheel |
akashvibhute | 2:761e3c932ce0 | 19 | * @param [out] w_R output angular vecolity for right drive wheel |
akashvibhute | 2:761e3c932ce0 | 20 | * @param [in] flag Specify the control mode: flag = 1 trajectory tracking control; flag = 0 speed and yaw control |
akashvibhute | 2:761e3c932ce0 | 21 | * @param [in] tc Servo time in seconds |
akashvibhute | 2:761e3c932ce0 | 22 | * @param [in] beta pitch angle |
akashvibhute | 2:761e3c932ce0 | 23 | * @param [in] dbeta pitch angle change rate |
akashvibhute | 2:761e3c932ce0 | 24 | * @param [in] gamma cart heading angle |
akashvibhute | 2:761e3c932ce0 | 25 | * @param [in] dgamma cart heading angle change rate |
akashvibhute | 2:761e3c932ce0 | 26 | * @param [in] theta_L left wheel angle |
akashvibhute | 2:761e3c932ce0 | 27 | * @param [in] theta_R right wheel angle |
akashvibhute | 2:761e3c932ce0 | 28 | * @param [in] dtheta_L left wheel velocity |
akashvibhute | 2:761e3c932ce0 | 29 | * @param [in] dtheta_R right wheel velocity |
akashvibhute | 2:761e3c932ce0 | 30 | * @param [in] ref_ddbeta Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 31 | * @param [in] ref_dbeta Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 32 | * @param [in] ref_beta Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 33 | * @param [in] ref_ddtheta Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 34 | * @param [in] ref_dtheta Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 35 | * @param [in] ref_theta Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 36 | * @param [in] ref_ddgamma Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 37 | * @param [in] ref_dgamma Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 38 | * @param [in] ref_gamma Parameter_Description |
akashvibhute | 2:761e3c932ce0 | 39 | * @return No value returned |
akashvibhute | 2:761e3c932ce0 | 40 | * |
akashvibhute | 2:761e3c932ce0 | 41 | * @details Units: distance in mm, time in second, mass in gram, angle in radian |
akashvibhute | 2:761e3c932ce0 | 42 | */ |
akashvibhute | 2:761e3c932ce0 | 43 | |
akashvibhute | 2:761e3c932ce0 | 44 | // Reference |
akashvibhute | 2:761e3c932ce0 | 45 | // ref_ddbeta, ref_dbeta, ref_beta, |
akashvibhute | 2:761e3c932ce0 | 46 | // ref_ddtheta, ref_dtheta, ref_dtheta, |
akashvibhute | 2:761e3c932ce0 | 47 | // ref_ddtheta, ref_dtheta, ref_theta |
akashvibhute | 2:761e3c932ce0 | 48 | |
akashvibhute | 2:761e3c932ce0 | 49 | attitudeControl(); |
akashvibhute | 2:761e3c932ce0 | 50 | |
akashvibhute | 2:761e3c932ce0 | 51 | void GenWheelVelocities(float* w_L, float* w_R, int flag, float tc, |
akashvibhute | 2:761e3c932ce0 | 52 | float beta, float dbeta, float gamma, float dgamma, |
akashvibhute | 2:761e3c932ce0 | 53 | float theta_L, float theta_R, float dtheta_L, float dtheta_R, |
akashvibhute | 2:761e3c932ce0 | 54 | float ref_ddbeta, float ref_dbeta, float ref_beta, |
akashvibhute | 2:761e3c932ce0 | 55 | float ref_ddtheta, float ref_dtheta, float ref_theta, |
akashvibhute | 2:761e3c932ce0 | 56 | float ref_ddgamma, float ref_dgamma, float ref_gamma); |
akashvibhute | 2:761e3c932ce0 | 57 | |
akashvibhute | 2:761e3c932ce0 | 58 | private: |
akashvibhute | 2:761e3c932ce0 | 59 | // System parameters |
akashvibhute | 2:761e3c932ce0 | 60 | // Sphere |
akashvibhute | 2:761e3c932ce0 | 61 | float ms, rs, rs_inner, Is; |
akashvibhute | 2:761e3c932ce0 | 62 | |
akashvibhute | 2:761e3c932ce0 | 63 | // Cart |
akashvibhute | 2:761e3c932ce0 | 64 | float mc, rc, ri; |
akashvibhute | 2:761e3c932ce0 | 65 | float Ic_x, Ic_z; |
akashvibhute | 2:761e3c932ce0 | 66 | |
akashvibhute | 2:761e3c932ce0 | 67 | // Drive wheel |
akashvibhute | 2:761e3c932ce0 | 68 | float mw, rw, Iw, h; |
akashvibhute | 2:761e3c932ce0 | 69 | |
akashvibhute | 2:761e3c932ce0 | 70 | // Gravity |
akashvibhute | 2:761e3c932ce0 | 71 | float gravity; |
akashvibhute | 2:761e3c932ce0 | 72 | |
akashvibhute | 2:761e3c932ce0 | 73 | // Theta and Beta Control |
akashvibhute | 2:761e3c932ce0 | 74 | // Control parameters |
akashvibhute | 2:761e3c932ce0 | 75 | float kx_beta[2]; |
akashvibhute | 2:761e3c932ce0 | 76 | float kx_theta[2]; |
akashvibhute | 2:761e3c932ce0 | 77 | float ks_beta[2]; |
akashvibhute | 2:761e3c932ce0 | 78 | float ks_theta[2]; |
akashvibhute | 2:761e3c932ce0 | 79 | float kt_beta[2]; |
akashvibhute | 2:761e3c932ce0 | 80 | float kt_theta[2]; |
akashvibhute | 2:761e3c932ce0 | 81 | |
akashvibhute | 2:761e3c932ce0 | 82 | // Gamma Control |
akashvibhute | 2:761e3c932ce0 | 83 | // Control gains |
akashvibhute | 2:761e3c932ce0 | 84 | float k1; |
akashvibhute | 2:761e3c932ce0 | 85 | float k2; |
akashvibhute | 2:761e3c932ce0 | 86 | }; |
akashvibhute | 2:761e3c932ce0 | 87 | |
akashvibhute | 2:761e3c932ce0 | 88 | |
akashvibhute | 2:761e3c932ce0 | 89 | #endif |