Team Virgo v3 / Orion_newPCB_test_LV

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 Team Virgo v3

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?

UserRevisionLine numberNew 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