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@19:7345688260b2, 2016-06-07 (annotated)

Committer:
akashvibhute
Date:
Tue Jun 07 03:48:56 2016 +0000
Revision:
19:7345688260b2
Parent:
2:761e3c932ce0 
working code to control speedster under SMC with waypoint tracking

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 19:7345688260b2 10 #include "generalFunctions.h"
akashvibhute 2:761e3c932ce0 11
akashvibhute 2:761e3c932ce0 12 class attitudeControl
akashvibhute 2:761e3c932ce0 13 {
akashvibhute 2:761e3c932ce0 14 public:
akashvibhute 2:761e3c932ce0 15
akashvibhute 2:761e3c932ce0 16 /**
akashvibhute 2:761e3c932ce0 17 * @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 18 *
akashvibhute 2:761e3c932ce0 19 * @param [out] w_L output angular vecolity for left drive wheel
akashvibhute 2:761e3c932ce0 20 * @param [out] w_R output angular vecolity for right drive wheel
akashvibhute 2:761e3c932ce0 21 * @param [in] flag Specify the control mode: flag = 1 trajectory tracking control; flag = 0 speed and yaw control
akashvibhute 2:761e3c932ce0 22 * @param [in] tc Servo time in seconds
akashvibhute 2:761e3c932ce0 23 * @param [in] beta pitch angle
akashvibhute 2:761e3c932ce0 24 * @param [in] dbeta pitch angle change rate
akashvibhute 2:761e3c932ce0 25 * @param [in] gamma cart heading angle
akashvibhute 2:761e3c932ce0 26 * @param [in] dgamma cart heading angle change rate
akashvibhute 2:761e3c932ce0 27 * @param [in] theta_L left wheel angle
akashvibhute 2:761e3c932ce0 28 * @param [in] theta_R right wheel angle
akashvibhute 2:761e3c932ce0 29 * @param [in] dtheta_L left wheel velocity
akashvibhute 2:761e3c932ce0 30 * @param [in] dtheta_R right wheel velocity
akashvibhute 2:761e3c932ce0 31 * @param [in] ref_ddbeta Parameter_Description
akashvibhute 2:761e3c932ce0 32 * @param [in] ref_dbeta Parameter_Description
akashvibhute 2:761e3c932ce0 33 * @param [in] ref_beta Parameter_Description
akashvibhute 2:761e3c932ce0 34 * @param [in] ref_ddtheta Parameter_Description
akashvibhute 2:761e3c932ce0 35 * @param [in] ref_dtheta Parameter_Description
akashvibhute 2:761e3c932ce0 36 * @param [in] ref_theta Parameter_Description
akashvibhute 2:761e3c932ce0 37 * @param [in] ref_ddgamma Parameter_Description
akashvibhute 2:761e3c932ce0 38 * @param [in] ref_dgamma Parameter_Description
akashvibhute 2:761e3c932ce0 39 * @param [in] ref_gamma Parameter_Description
akashvibhute 2:761e3c932ce0 40 * @return No value returned
akashvibhute 2:761e3c932ce0 41 *
akashvibhute 2:761e3c932ce0 42 * @details Units: distance in mm, time in second, mass in gram, angle in radian
akashvibhute 2:761e3c932ce0 43 */
akashvibhute 2:761e3c932ce0 44
akashvibhute 2:761e3c932ce0 45 // Reference
akashvibhute 2:761e3c932ce0 46 // ref_ddbeta, ref_dbeta, ref_beta,
akashvibhute 2:761e3c932ce0 47 // ref_ddtheta, ref_dtheta, ref_dtheta,
akashvibhute 2:761e3c932ce0 48 // ref_ddtheta, ref_dtheta, ref_theta
akashvibhute 2:761e3c932ce0 49
akashvibhute 2:761e3c932ce0 50 attitudeControl();
akashvibhute 2:761e3c932ce0 51
akashvibhute 2:761e3c932ce0 52 void GenWheelVelocities(float* w_L, float* w_R, int flag, float tc,
akashvibhute 2:761e3c932ce0 53 float beta, float dbeta, float gamma, float dgamma,
akashvibhute 2:761e3c932ce0 54 float theta_L, float theta_R, float dtheta_L, float dtheta_R,
akashvibhute 2:761e3c932ce0 55 float ref_ddbeta, float ref_dbeta, float ref_beta,
akashvibhute 2:761e3c932ce0 56 float ref_ddtheta, float ref_dtheta, float ref_theta,
akashvibhute 19:7345688260b2 57 float ref_ddgamma, float ref_dgamma, float ref_gamma,
akashvibhute 19:7345688260b2 58 float* u1, float* u2);
akashvibhute 2:761e3c932ce0 59
akashvibhute 2:761e3c932ce0 60 private:
akashvibhute 2:761e3c932ce0 61 // System parameters
akashvibhute 2:761e3c932ce0 62 // Sphere
akashvibhute 2:761e3c932ce0 63 float ms, rs, rs_inner, Is;
akashvibhute 2:761e3c932ce0 64
akashvibhute 2:761e3c932ce0 65 // Cart
akashvibhute 2:761e3c932ce0 66 float mc, rc, ri;
akashvibhute 2:761e3c932ce0 67 float Ic_x, Ic_z;
akashvibhute 2:761e3c932ce0 68
akashvibhute 2:761e3c932ce0 69 // Drive wheel
akashvibhute 2:761e3c932ce0 70 float mw, rw, Iw, h;
akashvibhute 2:761e3c932ce0 71
akashvibhute 2:761e3c932ce0 72 // Gravity
akashvibhute 2:761e3c932ce0 73 float gravity;
akashvibhute 2:761e3c932ce0 74
akashvibhute 2:761e3c932ce0 75 // Theta and Beta Control
akashvibhute 2:761e3c932ce0 76 // Control parameters
akashvibhute 2:761e3c932ce0 77 float kx_beta[2];
akashvibhute 2:761e3c932ce0 78 float kx_theta[2];
akashvibhute 2:761e3c932ce0 79 float ks_beta[2];
akashvibhute 2:761e3c932ce0 80 float ks_theta[2];
akashvibhute 2:761e3c932ce0 81 float kt_beta[2];
akashvibhute 2:761e3c932ce0 82 float kt_theta[2];
akashvibhute 2:761e3c932ce0 83
akashvibhute 2:761e3c932ce0 84 // Gamma Control
akashvibhute 2:761e3c932ce0 85 // Control gains
akashvibhute 2:761e3c932ce0 86 float k1;
akashvibhute 2:761e3c932ce0 87 float k2;
akashvibhute 2:761e3c932ce0 88 };
akashvibhute 2:761e3c932ce0 89
akashvibhute 2:761e3c932ce0 90
akashvibhute 2:761e3c932ce0 91 #endif