main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 #include "Servo.h"
00004 #include "MPU9250.h"
00005 
00006 //**************************** PID control para declaration ************************************//
00007 #define RATE  0.005
00008 #define Kp_1    1.1            //1.1
00009 #define Ki_1    0.18            //0.2
00010 #define Kd_1    0.001
00011 //**************************** Angle Sonsor para declaration ************************************//
00012 #define Kp 0.5f         // proportional gain governs rate of convergence to accelerometer/magnetometer
00013 #define Ki 0.0f//0.005f       // integral gain governs rate of convergence of gyroscope biases
00014 //**************************** pragram debug declaration ************************************//
00015 int index_times = 0;
00016 double RunTime =0,lastTime =0;
00017 int do_measure_index=0;
00018 int Arm_enable_index;
00019 DigitalOut Arm_interrupt(D9);
00020 
00021 double t_MeasureAngularVelocity=0.001;
00022 double t_PIDcontrol_velocity =0.001;
00023 double t_LQR_control = 0.005;
00024 double t_MeasureRobotAttitudeAngle = 0.005;
00025 double t_quadratureDecoder = 7;
00026 double t_TrajectoryTracking_control = 0.1;
00027 //**************************** Motor para declaration ************************************//
00028 double radius_ball = 0.15;
00029 double radius_robot = 0.105;
00030 double r_wheel = 0.05;
00031 //**************************** Motor declaration ************************************//
00032 //DigitalOut ExtInt(PB_7);
00033 DigitalOut Break_1(A5);//D15
00034 DigitalOut Break_2(A4);//D14
00035 DigitalOut Break_3(A3);//D8
00036 DigitalOut AR_1(D15);//A5
00037 DigitalOut AR_2(D14);//A4
00038 DigitalOut AR_3(D8);//A3
00039 DigitalOut CW_CCW_1(D7);
00040 DigitalOut CW_CCW_2(D6);
00041 DigitalOut CW_CCW_3(D5);
00042 Servo PWM_Motor_1(D2);
00043 Servo PWM_Motor_2(D3);
00044 Servo PWM_Motor_3(D4);
00045 int control_Brake,control_StopRun=0;
00046 int cw_ccw_1=0,cw_ccw_2=0,cw_ccw_3=0;
00047 //**************************** Measuremant of angular velocity declaration ************************************//
00048 double Angle_1 =0,Angle_2 =0,Angle_3 =0;
00049 double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0;
00050 double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4;
00051 double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4;
00052 double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4;
00053 double Average_SectionAngle_1,Now_angularVelocity_1=0;
00054 double Average_SectionAngle_2,Now_angularVelocity_2=0;
00055 double Average_SectionAngle_3,Now_angularVelocity_3=0;
00056 double SectionTime =0;
00057 double NowTime_measureVelocity=0, LastTime_measureVelocity = 0;
00058 //**************************** PID control declaration ************************************//
00059 float Now_time_PID,Last_Time_PID;
00060     //Command
00061 double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1;
00062 double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1;
00063     //Control
00064 double Control_motor_1,Control_motor_2,Control_motor_3;
00065     //Control PWM value
00066 double Control_Motor_PWM_1,Control_Motor_PWM_2,Control_Motor_PWM_3;
00067 
00068 
00069 PID Motor_1 (&Now_angularVelocity_1, &Control_motor_1, &Command_AngularVel_1, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
00070 PID Motor_2 (&Now_angularVelocity_2, &Control_motor_2, &Command_AngularVel_2, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
00071 PID Motor_3 (&Now_angularVelocity_3, &Control_motor_3, &Command_AngularVel_3, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
00072 //****************************LQR_control declaration************************************//
00073 double Ka = 25;                //25.0
00074 double Kav = 10.7423;               //10.4423
00075 
00076 double Kt = 3.1 ;                   //3.1
00077 double Kt_y = 3.1;                    //3.98
00078 double Kv = 1.0;                    //1.0
00079 double Kv_y = 1.0;                    //1.1
00080 
00081 double Kz = 0.05;                   //0.05
00082 double Kii = 0.15;                  //0.2
00083 
00084 double KI_xy = 0.0085;                //0.02
00085 double KI_xy_y = 0.0085;                //0.025
00086 
00087 double Roll_offset = 2.1;          //2.88       4.0
00088 double Pitch_offset = 1.85;        //-0.05       2.1
00089 double Diff_Roll,Diff_Pitch,Diff_Yaw;
00090 double Integ_Roll,Integ_Pitch;
00091 double Integ_x,Integ_y;
00092 double Roll_last,Pitch_last;
00093 double Vx=0,Vy=0,Wz=0;
00094 double d_x=0,d_y=0;
00095 double u_x=0,u_y=0;
00096 double Point_x;
00097 double x_now,y_now;
00098 double x_pre_1,y_pre_1;
00099 double ax_now,ay_now;
00100 double ax_pre_1,ay_pre_1;
00101 double ax_pre_2,ay_pre_2;
00102 double Vx_pre_1,Vy_pre_1;
00103 double Diff_x,Diff_y;
00104 double Diff_x_pre,Diff_y_pre;
00105 double dot_diff_x,dot_diff_y;
00106 //****************************Trajectory Tracking declaration************************************//
00107 double x_trajectory,y_trajectory;
00108 //****************************Encoder declaration************************************//
00109 DigitalIn phaseA_1( PC_3 );
00110 DigitalIn phaseB_1( PC_2 );
00111 DigitalIn phaseA_2( PH_1 );
00112 DigitalIn phaseB_2( PB_7 );
00113 DigitalIn phaseA_3( PA_15 );
00114 DigitalIn phaseB_3( PA_14 );
00115 int encoderClickCount_1=0,previousEncoderState_1=0; 
00116 int encoderClickCount_2=0,previousEncoderState_2=0;
00117 int encoderClickCount_3=0,previousEncoderState_3=0;
00118 
00119 //****************************Angle Sensor declaration************************************//
00120 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
00121 float control_frequency = 800;//PPM_FREQU;         // frequency for the main loop in Hz
00122 int counter = 0;
00123 int divider = 20;
00124 float dt;                       // time for entire loop
00125 float dt_sensors;               // time only to read sensors
00126 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
00127 float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
00128 float exInt = 0, eyInt = 0, ezInt = 0;
00129 float OX = 0, OY = 0, OZ = 0;
00130 float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4;
00131 float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4;
00132 float Mag_x_pre,Mag_y_pre,Mag_z_pre;
00133 float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
00134 float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
00135 float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
00136 float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
00137 float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
00138 float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
00139 float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
00140 float Global_mag_vector_angle,Yaw_pre;
00141 float Global_mag_x_vector_angle,Mag_x_vector_angle;
00142 float Global_mag_y_vector_angle,Mag_y_vector_angle;
00143 int Count_mag_check=0;
00144 float angle[3];
00145 float Roll,Pitch,Yaw;
00146 float calibrated_values[3],magCalibrationp[3];
00147 float v_index[3];
00148 float dest1,dest2;
00149 int f=0;
00150 int j=0;
00151 int k=0;
00152 int g=0;
00153 int count1=0,count2=0,count3=0,count4=0,count5=0,count6=0,count7=0,count8=0,count9=0,count11=0,count12=0,count14=0;
00154 int Rot_index;
00155 float mRes = 10.*4912./32760.0;
00156 
00157 int AngleFilter_counter = 0;
00158 float Roll_total = 0,Pitch_total = 0;
00159 
00160 SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
00161 mpu9250_spi imu(spi,SPI_CS);   //define the mpu9250 object
00162 //****************************Timer declaration************************************//
00163 Timer NowTime;
00164 //**************************** Filter_IMUupdate ************************************//
00165 //**************************** Filter_IMUupdate ************************************//
00166 //**************************** Filter_IMUupdate ************************************//
00167 //**************************** Filter_IMUupdate ************************************//
00168 //**************************** Filter_IMUupdate ************************************//
00169 void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) {
00170     float norm;
00171     float vx, vy, vz;
00172     float ex, ey, ez;         
00173     
00174     // normalise the measurements
00175     norm = sqrt(ax*ax + ay*ay + az*az);
00176     if(norm == 0.0f) return;   
00177     ax /= norm;
00178     ay /= norm;
00179     az /= norm;      
00180     
00181     // estimated direction of gravity
00182     vx = 2*(q1*q3 - q0*q2);
00183     vy = 2*(q0*q1 + q2*q3);
00184     vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
00185     
00186     // error is sum of cross product between reference direction of field and direction measured by sensor
00187     ex = (ay*vz - az*vy);
00188     ey = (az*vx - ax*vz);
00189     ez = (ax*vy - ay*vx);
00190     
00191     // integral error scaled integral gain
00192     exInt += ex*Ki;
00193     eyInt += ey*Ki;
00194     ezInt += ez*Ki;
00195     
00196     // adjusted gyroscope measurements
00197     gx += Kp*ex + exInt;
00198     gy += Kp*ey + eyInt;
00199     gz += Kp*ez + ezInt;
00200     
00201     // integrate quaternion rate and normalise
00202     float q0o = q0; // he did the MATLAB to C error by not thinking of the beginning vector elements already being changed for the calculation of the rest!
00203     float q1o = q1;
00204     float q2o = q2;
00205     float q3o = q3;
00206     q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
00207     q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
00208     q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
00209     q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;  
00210     
00211     // normalise quaternion
00212     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00213     q0 = q0 / norm;
00214     q1 = q1 / norm;
00215     q2 = q2 / norm;
00216     q3 = q3 / norm;
00217 }
00218 //**************************** Filter_compute ************************************//
00219 //**************************** Filter_compute ************************************//
00220 //**************************** Filter_compute ************************************//
00221 //**************************** Filter_compute ************************************//
00222 //**************************** Filter_compute ************************************//
00223 void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
00224 {
00225     // IMU/AHRS
00226 
00227     float d_Gyro_angle[3];
00228     void get_Acc_angle(const float * Acc_data);
00229      // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)   
00230     float radGyro[3],Gyro_cal_data; // Gyro in radians per second
00231     
00232     for(int i=0; i<3; i++)
00233         radGyro[i] = Gyro_data[i] * 3.14159/ 180;
00234         
00235         Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
00236         //IMU_AHRSupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2], Comp_data[0], Comp_data[1], Comp_data[2]);
00237         
00238         float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
00239         
00240         rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
00241         rangle[1] = asin (2*q0*q2 - 2*q3*q1);
00242         rangle[2] = atan2(2*q0_A*q3_A + 2*q1_A*q2_A, 1 - 2*(q2_A*q2_A + q3_A*q3_A));            // Z-axis
00243         
00244         
00245         for(int i=0; i<2; i++){  // angle in degree
00246             angle[i] = rangle[i] * 180 / 3.14159;
00247         }
00248         /*Roll=angle[0]*0.2 + Roll_pre_1*0.2 + Roll_pre_2*0.2 +Roll_pre_3*0.2 + Roll_pre_4*0.2;
00249         Roll_pre_4 = Roll_pre_3;
00250         Roll_pre_3 = Roll_pre_2;
00251         Roll_pre_2 = Roll_pre_1;
00252         Roll_pre_1 = Roll;
00253     
00254         Pitch=angle[1]*0.2 + Pitch_pre_1*0.2 + Pitch_pre_2*0.2 + Pitch_pre_3*0.2 + Pitch_pre_4*0.2;
00255         Pitch_pre_4 = Pitch_pre_3;
00256         Pitch_pre_3 = Pitch_pre_2;
00257         Pitch_pre_2 = Pitch_pre_1;
00258         Pitch_pre_1 = Pitch;*/
00259 
00260         Roll_total += angle[0];
00261         Pitch_total += angle[1];
00262         AngleFilter_counter++;
00263         
00264         if( AngleFilter_counter > 1 ){                        // The average of the attitude angle for 100 times.
00265             Roll = Roll_total / AngleFilter_counter;
00266             Pitch = Pitch_total / AngleFilter_counter;
00267             AngleFilter_counter = 0;
00268             Roll_total = 0;
00269             Pitch_total = 0;
00270             Roll -= Roll_offset;
00271             Pitch -= Pitch_offset;
00272         }
00273         
00274 //**************************************************Gyro_data[2]  filter      start
00275         float GYRO_z=0;
00276 
00277         GYRO_z=Gyro_data[2]*0.15 + GYRO_z_pre*0.20 + GYRO_z_pre_L*0.20 + GYRO_z_pre_LL*0.25 + GYRO_z_pre_LLL*0.20;
00278         if( count4==1 ){
00279             GYRO_z_pre_L=GYRO_z_pre;
00280             
00281             count4=0;
00282         }
00283         if( count5==2 ){
00284             GYRO_z_pre_LL=GYRO_z_pre_L;
00285             
00286             count5=0;
00287         }
00288         if( count6==3 ){
00289             GYRO_z_pre_LLL=GYRO_z_pre_LL;
00290             
00291             count6=0;
00292         }
00293         
00294        
00295         
00296         count4++;
00297         count5++;
00298         count6++;
00299         GYRO_z_pre=Gyro_data[2];
00300         Global_GYRO_z=GYRO_z;
00301        /*printf("     GYRO_z:%10.3f     ,count8:%10d   \n", 
00302             GYRO_z,
00303             count8
00304             );*/
00305         if((count8>5)&&(count8<=2005)){
00306             GYRO_z_total+=GYRO_z;
00307         }
00308         if( count8==2005 ){
00309             GYRO_z_offset=GYRO_z_total/2000;
00310            /* printf("     GYRO_z_offset:%10.5f    \n ", 
00311             GYRO_z_offset
00312             );*/
00313             GYRO_z_total=0;
00314             count8=0;
00315         }
00316 
00317         count8++;
00318 //**************************************************Gyro_data[2]'s average  filter   :     answer=GYRO_Z is roughly = 0.74956
00319 
00320 //************************************************** calculate Yaw
00321     if( (count11==35) ){
00322         if( abs(Yaw_pre-Yaw)<1 ){
00323             Yaw_pre=Yaw_pre;
00324         }else{
00325             Yaw_pre=Yaw;
00326         }
00327         count11=0;
00328     }
00329     count11++;
00330         
00331     if( count12>=20 ){
00332         Yaw += (Gyro_data[2]-0.74936) *dt; 
00333     }
00334     count12++;
00335     //pc.printf("     Yaw:%10.5f     ", 
00336             //Yaw
00337     // );
00338 }
00339 //**************************** Mag_Complentary_Filter ************************************//
00340 //**************************** Mag_Complentary_Filter ************************************//
00341 //**************************** Mag_Complentary_Filter ************************************//
00342 //**************************** Mag_Complentary_Filter ************************************//
00343 //**************************** Mag_Complentary_Filter ************************************//
00344 void Mag_Complentary_Filter(float dt, const float * Comp_data)
00345 {
00346         float Mag_x=0,Mag_y=0,Mag_z=0;
00347         Mag_x=Comp_data[0]*0.15 + Mag_x_pre*0.20 + Mag_x_pre_L*0.20 + Mag_x_pre_LL*0.25 + Mag_x_pre_LLL*0.20;
00348         Mag_y=Comp_data[1]*0.15 + Mag_y_pre*0.20 + Mag_y_pre_L*0.20 + Mag_y_pre_LL*0.25 + Mag_y_pre_LLL*0.20;
00349         Mag_z=Comp_data[2]*0.15 + Mag_z_pre*0.20 + Mag_z_pre_L*0.20 + Mag_z_pre_LL*0.25 + Mag_z_pre_LLL*0.20;
00350         
00351         
00352         if( count1==1 ){
00353             Mag_x_pre_L=Mag_x_pre;
00354             Mag_y_pre_L=Mag_y_pre;
00355             Mag_z_pre_L=Mag_z_pre;
00356             Cal_Mag_x_pre=Cal_Mag_x;
00357             
00358             count1=0;
00359         }
00360         if( count2==2 ){
00361             Mag_x_pre_LL=Mag_x_pre_L;
00362             Mag_y_pre_LL=Mag_y_pre_L;
00363             Mag_z_pre_LL=Mag_z_pre_L;
00364             Cal_Mag_x_pre_L=Cal_Mag_x_pre;
00365             
00366             count2=0;
00367         }
00368         if( count7==3 ){
00369             Mag_x_pre_LLL=Mag_x_pre_LL;
00370             Mag_y_pre_LLL=Mag_y_pre_LL;
00371             Mag_z_pre_LLL=Mag_z_pre_LL;
00372             Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
00373         
00374             count7=0;
00375         }
00376 
00377         
00378         count1++;
00379         count2++;
00380         count7++;
00381         Mag_x_pre=Comp_data[0];
00382         Mag_y_pre=Comp_data[1];
00383         Mag_z_pre=Comp_data[2];
00384         if( count14>4 ){ 
00385             Cal_Mag_x=Mag_x;             
00386         }
00387         count14++;
00388         
00389         
00390 //*************************************Mag_ave calculate
00391         if(count3<=20){
00392             Mag_x_total+=Mag_x;
00393             Mag_y_total+=Mag_y;
00394         }
00395         if( count3==20){
00396             Mag_x_ave=Mag_x_total/21;
00397             Mag_y_ave=Mag_y_total/21;
00398             /*pc.printf("     Mag_x_ave:%10.5f    ,Mag_y_ave:%10.5f     ", 
00399             Mag_x_ave,
00400             Mag_y_ave
00401             );*/
00402             Mag_x_total=0;
00403             Mag_y_total=0;
00404             count3=0;
00405             
00406             
00407         }
00408         count3++;
00409         
00410 //********************************ROT_check  start
00411 
00412         float v_length,v_length_ave,MagVector_angle;
00413         v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y );
00414         v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave );
00415         
00416         MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3;
00417         
00418         if( count9==3 ){
00419             Global_mag_vector_angle=MagVector_angle;
00420             count9=0;
00421         }
00422         count9++;
00423         
00424         
00425         if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) ){
00426             Count_mag_check++;
00427             
00428         }else{ Count_mag_check=0; }
00429         
00430         if( Count_mag_check==30 ){
00431             Yaw=Yaw_pre;
00432             Count_mag_check=0;
00433         }
00434         float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
00435 //********************************Theta_check  end
00436         /*pc.printf("ABS_CHECK:%10.3f,Cal_Mag_x_pre_LL:%10.3f,Mag_x:%10.3f,Count_mag_check:%10d ,Yaw_pre:%10.3f,Yaw_filter:%10.3f    ", 
00437             ABS_CHECK,
00438             Cal_Mag_x_pre_LL,
00439             Mag_x,
00440             Count_mag_check,
00441             Yaw_pre,
00442             Yaw
00443             );*/
00444 
00445 }
00446 //****************************Motor_run************************************//
00447 //****************************Motor_run************************************//
00448 //****************************Motor_run************************************//
00449 //****************************Motor_run************************************//
00450 //****************************Motor_run************************************//
00451 int Motor_run(double control_value_PWM_1,double control_value_PWM_2,double control_value_PWM_3,int control_CW_CCW_1,int control_CW_CCW_2,int control_CW_CCW_3,int control_brake ,int control_stopRun){
00452     
00453     Break_1 = 0;
00454     Break_2 = 0;
00455     Break_3 = 0;
00456     //Brake = 1;
00457     /*CW_CCW_1 = 1;
00458     CW_CCW_2 = 1;
00459     CW_CCW_3 = 1;*/
00460     
00461     CW_CCW_1 = control_CW_CCW_1;
00462     CW_CCW_2 = control_CW_CCW_2;
00463     CW_CCW_3 = control_CW_CCW_3;
00464     double Control_value_PWM_1 = 1-control_value_PWM_1;
00465     double Control_value_PWM_2 = 1-control_value_PWM_2;
00466     double Control_value_PWM_3 = 1-control_value_PWM_3;
00467     //printf("Control_value_PWM_2: %f\n",Control_value_PWM_2);
00468     //printf("Control_value_PWM_3: %f\n",Control_value_PWM_3);
00469     //PWM_Motor_1.write(Control_value_PWM_1);
00470     /*control_CW_CCW_1 = 0;
00471     control_CW_CCW_2 = 0;
00472     control_CW_CCW_3 = 0;
00473     AR_1 = 1;
00474     AR_2 = 1;
00475     AR_3 = 1;
00476     CW_CCW_1 = 1;
00477     CW_CCW_2 = 1;
00478     CW_CCW_3 = 1;
00479     PWM_Motor_1.write(0.75);
00480     PWM_Motor_2.write(0.75);
00481     PWM_Motor_3.write(0.75);*/
00482     //wait_ms(0.5);
00483     PWM_Motor_1.write(Control_value_PWM_1);
00484     PWM_Motor_2.write(Control_value_PWM_2);
00485     PWM_Motor_3.write(Control_value_PWM_3);
00486 
00487 }
00488 //****************************quadratureDecoder_1************************************//
00489 //****************************quadratureDecoder_1************************************//
00490 //****************************quadratureDecoder_1************************************//
00491 //****************************quadratureDecoder_1************************************//
00492 //****************************quadratureDecoder_1************************************//
00493 void quadratureDecoder_1( void )
00494 {
00495   int  currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read(); 
00496   
00497 //****************************  1  ************************************//
00498     if( currentEncoderState_1 == previousEncoderState_1 )
00499     {
00500         return;
00501     }
00502     
00503     switch( previousEncoderState_1 )
00504     {
00505         case 0:
00506             if( currentEncoderState_1 == 2 )
00507             {
00508                 encoderClickCount_1--;
00509                 
00510             }
00511             else if( currentEncoderState_1 == 1 )
00512             {
00513                 encoderClickCount_1++;
00514                 
00515             }
00516             break;
00517             
00518         case 1:
00519             if( currentEncoderState_1 == 0 )
00520             {        
00521                 encoderClickCount_1--;
00522                 
00523             }
00524             else if( currentEncoderState_1 == 3 )
00525             {
00526                 encoderClickCount_1++;
00527                 
00528             }
00529             break;
00530             
00531         case 2:
00532             if( currentEncoderState_1 == 3 )
00533             {
00534                 encoderClickCount_1--;
00535                 
00536             }
00537             else if( currentEncoderState_1 == 0 )
00538             {
00539                 encoderClickCount_1++;
00540                 
00541             }
00542             break;
00543             
00544         case 3:
00545             if( currentEncoderState_1 == 1 )
00546             {
00547                 encoderClickCount_1--;
00548                 
00549             }
00550             else if( currentEncoderState_1 == 2 )
00551             {
00552                 encoderClickCount_1++;
00553                 
00554             }
00555             break;
00556 
00557         default:
00558             break;
00559     }
00560     previousEncoderState_1 = currentEncoderState_1;
00561 
00562 }
00563 void quadratureDecoder_2( void )
00564 {
00565   int  currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read(); 
00566 
00567 //****************************  2  ************************************//
00568     if( currentEncoderState_2 == previousEncoderState_2 )
00569     {
00570         return;
00571     }
00572     
00573     switch( previousEncoderState_2 )
00574     {
00575         case 0:
00576             if( currentEncoderState_2 == 2 )
00577             {
00578                 encoderClickCount_2--;
00579                 
00580             }
00581             else if( currentEncoderState_2 == 1 )
00582             {
00583                 encoderClickCount_2++;
00584                 
00585             }
00586             break;
00587             
00588         case 1:
00589             if( currentEncoderState_2 == 0 )
00590             {        
00591                 encoderClickCount_2--;
00592                 
00593             }
00594             else if( currentEncoderState_2 == 3 )
00595             {
00596                 encoderClickCount_2++;
00597                 
00598             }
00599             break;
00600             
00601         case 2:
00602             if( currentEncoderState_2 == 3 )
00603             {
00604                 encoderClickCount_2--;
00605                 
00606             }
00607             else if( currentEncoderState_2 == 0 )
00608             {
00609                 encoderClickCount_2++;
00610                 
00611             }
00612             break;
00613             
00614         case 3:
00615             if( currentEncoderState_2 == 1 )
00616             {
00617                 encoderClickCount_2--;
00618                 
00619             }
00620             else if( currentEncoderState_2 == 2 )
00621             {
00622                 encoderClickCount_2++;
00623                 
00624             }
00625             break;
00626 
00627         default:
00628             break;
00629     }
00630     previousEncoderState_2 = currentEncoderState_2;
00631 }
00632 void quadratureDecoder_3( void )
00633 {
00634   int  currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read(); 
00635   
00636 //****************************  3  ************************************//
00637     if( currentEncoderState_3 == previousEncoderState_3 )
00638     {
00639         return;
00640     }
00641     
00642     switch( previousEncoderState_3 )
00643     {
00644         case 0:
00645             if( currentEncoderState_3 == 2 )
00646             {
00647                 encoderClickCount_3--;
00648                 
00649             }
00650             else if( currentEncoderState_3 == 1 )
00651             {
00652                 encoderClickCount_3++;
00653                 
00654             }
00655             break;
00656             
00657         case 1:
00658             if( currentEncoderState_3 == 0 )
00659             {        
00660                 encoderClickCount_3--;
00661                 
00662             }
00663             else if( currentEncoderState_3 == 3 )
00664             {
00665                 encoderClickCount_3++;
00666                 
00667             }
00668             break;
00669             
00670         case 2:
00671             if( currentEncoderState_3 == 3 )
00672             {
00673                 encoderClickCount_3--;
00674                 
00675             }
00676             else if( currentEncoderState_3 == 0 )
00677             {
00678                 encoderClickCount_3++;
00679                 
00680             }
00681             break;
00682             
00683         case 3:
00684             if( currentEncoderState_3 == 1 )
00685             {
00686                 encoderClickCount_3--;
00687                 
00688             }
00689             else if( currentEncoderState_3 == 2 )
00690             {
00691                 encoderClickCount_3++;
00692                 
00693             }
00694             break;
00695 
00696         default:
00697             break;
00698     }
00699     previousEncoderState_3 = currentEncoderState_3;
00700 
00701 }
00702 //****************************getAngular************************************//
00703 //****************************getAngular************************************//
00704 //****************************getAngular************************************//
00705 //****************************getAngular************************************//
00706 //****************************getAngular************************************//
00707 void getAngular( void )
00708 {
00709     Angle_1 = (encoderClickCount_1*0.1499)/5; 
00710     Angle_2 = (encoderClickCount_2*0.1499)/5; 
00711     Angle_3 = (encoderClickCount_3*0.1499)/5; 
00712 
00713     
00714     _1_SectionAngle = Angle_1 - LastAngle_1;
00715     _2_SectionAngle = Angle_2 - LastAngle_2;
00716     _3_SectionAngle = Angle_3 - LastAngle_3;
00717     Average_SectionAngle_1 = (_1_SectionAngle*0.3 + _1_LastSectionAngle*0.3 + _1_LastSectionAngle_1*0.1 + _1_LastSectionAngle_2*0.1 + _1_LastSectionAngle_3*0.1 + _1_LastSectionAngle_4*0.1)/1;
00718     Average_SectionAngle_2 = (_2_SectionAngle*0.3 + _2_LastSectionAngle*0.3 + _2_LastSectionAngle_1*0.1 + _2_LastSectionAngle_2*0.1 + _2_LastSectionAngle_3*0.1 + _2_LastSectionAngle_4*0.1)/1;
00719     Average_SectionAngle_3 = (_3_SectionAngle*0.3 + _3_LastSectionAngle*0.3 + _3_LastSectionAngle_1*0.1 + _3_LastSectionAngle_2*0.1 + _3_LastSectionAngle_3*0.1 + _3_LastSectionAngle_4*0.1)/1;
00720     NowTime_measureVelocity = NowTime.read();
00721     SectionTime = NowTime_measureVelocity - LastTime_measureVelocity;
00722     Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime));
00723     Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime));
00724     Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime));
00725     
00726     
00727     LastTime_measureVelocity = NowTime_measureVelocity;
00728     LastAngle_1 = Angle_1;
00729     _1_LastSectionAngle_4 = _1_LastSectionAngle_3;
00730     _1_LastSectionAngle_3 = _1_LastSectionAngle_2;
00731     _1_LastSectionAngle_2 = _1_LastSectionAngle_1;
00732     _1_LastSectionAngle_1 = _1_LastSectionAngle;
00733     _1_LastSectionAngle = _1_SectionAngle;
00734     LastAngle_2 = Angle_2;
00735     _2_LastSectionAngle_4 = _2_LastSectionAngle_3;
00736     _2_LastSectionAngle_3 = _2_LastSectionAngle_2;
00737     _2_LastSectionAngle_2 = _2_LastSectionAngle_1;
00738     _2_LastSectionAngle_1 = _2_LastSectionAngle;
00739     _2_LastSectionAngle = _2_SectionAngle;
00740     LastAngle_3 = Angle_3;
00741     _3_LastSectionAngle_4 = _3_LastSectionAngle_3;
00742     _3_LastSectionAngle_3 = _3_LastSectionAngle_2;
00743     _3_LastSectionAngle_2 = _3_LastSectionAngle_1;
00744     _3_LastSectionAngle_1 = _3_LastSectionAngle;
00745     _3_LastSectionAngle = _3_SectionAngle;
00746 }
00747 //****************************PWM_commmand_transformation************************************//
00748 //****************************PWM_commmand_transformation************************************//
00749 //****************************PWM_commmand_transformation************************************//
00750 //****************************PWM_commmand_transformation************************************//
00751 //****************************PWM_commmand_transformation************************************//
00752 double PWM_commmand_transformation( double  Control_AngVel_Value ){
00753     double Control_PWM_Value = 0;
00754     //double control_CW_CCW = 0;
00755     //printf("*********************Control_AngVel_Value:%.3f\n",Control_AngVel_Value);
00756     if( Control_AngVel_Value > 0 ){
00757         Control_AngVel_Value = Control_AngVel_Value;
00758         //control_CW_CCW = 0;
00759         
00760     }else if( Control_AngVel_Value < 0 ){
00761         Control_AngVel_Value = -Control_AngVel_Value;
00762         //control_CW_CCW = 1;
00763         
00764     }
00765     if( Control_AngVel_Value > 325){
00766         if( Control_AngVel_Value < 466 ){
00767             if( Control_AngVel_Value < 393 ){
00768                 Control_PWM_Value =  Control_AngVel_Value/651.6   ;   //0.5~0.6
00769             }else {
00770                 Control_PWM_Value =  Control_AngVel_Value/658.8;   //0.6~0.7
00771             }
00772         }else{
00773             if( Control_AngVel_Value < 523 ){
00774                Control_PWM_Value =  Control_AngVel_Value/658.39; //0.7~0.8
00775             }else{
00776                 if( Control_AngVel_Value < 588 ){
00777                   Control_PWM_Value =  Control_AngVel_Value/652.36;  //0.8~0.9
00778                 }else{
00779                   Control_PWM_Value =  Control_AngVel_Value/655.11;  //0.9~1
00780                 }
00781             }
00782         }
00783     }else if( Control_AngVel_Value < 325){
00784         if( Control_AngVel_Value < 40 ){
00785            Control_PWM_Value =  Control_AngVel_Value/533.3; //0~0.075
00786         }else{
00787             if( Control_AngVel_Value < 59 ){
00788                Control_PWM_Value =  Control_AngVel_Value/560.65; //0.1~0.15
00789             }else{
00790                 if( Control_AngVel_Value < 131 ){
00791                   Control_PWM_Value =  Control_AngVel_Value/638.3;  //0.15~0.2
00792                 }else{
00793                     if( Control_AngVel_Value < 197 ){
00794                        Control_PWM_Value =  Control_AngVel_Value/651.6; //0.2~0.3
00795                     }else{
00796                         if( Control_AngVel_Value < 263 ){
00797                             Control_PWM_Value =  Control_AngVel_Value/654.16; //0.3~0.4
00798                         }else{
00799                             Control_PWM_Value =  Control_AngVel_Value/652.5; //0.4~0.5
00800                         }
00801                     }
00802                 }
00803             }
00804         }
00805     }
00806     return Control_PWM_Value;
00807 }
00808 //****************************PIDcontrol_compute_velocity************************************//
00809 //****************************PIDcontrol_compute_velocity************************************//
00810 //****************************PIDcontrol_compute_velocity************************************//
00811 //****************************PIDcontrol_compute_velocity************************************//
00812 //****************************PIDcontrol_compute_velocity************************************//
00813 void PIDcontrol_compute_velocity(void){
00814     if(command_AngularVel_1 >= 0){
00815         cw_ccw_1 = 0;
00816         Command_AngularVel_1 = command_AngularVel_1;
00817     }else{
00818         cw_ccw_1 = 1;
00819         Command_AngularVel_1 = -command_AngularVel_1;
00820     }
00821     if(command_AngularVel_2 >= 0){
00822         cw_ccw_2 = 0;
00823         Command_AngularVel_2 = command_AngularVel_2;
00824     }else{
00825         cw_ccw_2 = 1;
00826         Command_AngularVel_2 = -command_AngularVel_2;
00827     }
00828     if(command_AngularVel_3 >= 0){
00829         cw_ccw_3 = 0;
00830         Command_AngularVel_3 = command_AngularVel_3;
00831     }else{
00832         cw_ccw_3 = 1;
00833         Command_AngularVel_3 = -command_AngularVel_3;
00834     }
00835     Now_time_PID=NowTime.read();
00836     Motor_1.Compute(&Now_time_PID);
00837     Motor_2.Compute(&Now_time_PID);
00838     Motor_3.Compute(&Now_time_PID);
00839     Control_Motor_PWM_1 = PWM_commmand_transformation(Control_motor_1);
00840     Control_Motor_PWM_2 = PWM_commmand_transformation(Control_motor_2);
00841     Control_Motor_PWM_3 = PWM_commmand_transformation(Control_motor_3);
00842     //Control_Motor_PWM_1 = PWM_commmand_transformation(Command_AngularVel_1);
00843     //Control_Motor_PWM_2 = PWM_commmand_transformation(Command_AngularVel_2);
00844     //Control_Motor_PWM_3 = PWM_commmand_transformation(Command_AngularVel_3);
00845     //printf("Command_AngularVel_2: %f\n",Command_AngularVel_2);
00846     //printf("Control_motor_2: %f\n",Control_motor_2);
00847     
00848 }
00849 //****************************Compute_point************************************//
00850 //****************************Compute_point************************************//
00851 //****************************Compute_point************************************//
00852 //****************************Compute_point************************************//
00853 //****************************Compute_point************************************//
00854 void LQR_control_compute(void){
00855     //Now_point_x = (angle/57.295)*3;
00856     //Now_point_x = Angle_1;
00857     /*if( Roll > 0.5 ){
00858         d_x = Roll*0.01745*radius_ball;
00859         Vx = d_x/0.001;
00860     }else{
00861         Vx = 0;
00862     }
00863     if(Pitch > 0.5){
00864         d_y = Pitch*0.01745*radius_ball;
00865         Vy = d_y/0.001;
00866     }else{
00867         Vy = 0;
00868     }
00869     if( Yaw > 0 ){
00870         Wz = 0;
00871     }else{
00872         Wz = 0;
00873     }*/
00874     Diff_Roll = (Roll - Roll_last);
00875     Diff_Pitch = (Pitch - Pitch_last);
00876     Integ_Roll += Roll;
00877     //printf("Diff_Roll:%.3f\n",Diff_Roll);
00878     Integ_Pitch +=  Pitch;
00879     Roll_last = Roll;
00880     Pitch_last = Pitch;
00881     
00882     //Diff_Roll =0;
00883     //Roll -= 2.45;
00884     //Pitch -=2.5;
00885     Diff_x = x_now - x_trajectory;
00886     Diff_y = y_now - y_trajectory;
00887     dot_diff_x = Diff_x - Diff_x_pre;
00888     dot_diff_y = Diff_y - Diff_y_pre;
00889     Integ_x += Diff_x;
00890     Integ_y += Diff_y;
00891     
00892     //x_pre_1 = x_now;
00893     //y_pre_1 = y_now;
00894     Diff_x_pre = Diff_x;
00895     Diff_y_pre = Diff_y;
00896     
00897     u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x;
00898     //u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll - Kt*(10) - Kv*0;
00899     u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y;
00900     //u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch - Kt*0 - Kv*0;
00901     if(u_x > 100 ){u_x = 100;}else if(u_x < -100){u_x = -100;}
00902     if(u_y > 100 ){u_y = 100;}else if(u_y < -100){u_y = -100;}
00903     Vx = u_x;
00904     Vy = u_y;
00905     Wz = -Yaw;
00906     
00907     command_AngularVel_1 = (1/radius_ball)*(-Vy*(-0.5)+Kz*Wz);
00908     command_AngularVel_2 = (1/radius_ball)*((0.866*Vx+0.5*Vy)*(-0.5)+Kz*Wz);
00909     command_AngularVel_3 = (1/radius_ball)*((-0.866*Vx+0.5*Vy)*(-0.5)+Kz*Wz);
00910     //printf("Command_AngularVel_2: %f\n",Command_AngularVel_2);
00911     //printf("Command_AngularVel_3: %f\n",Command_AngularVel_3);
00912     
00913     /*ax_now = Vx - Vx_pre_1;
00914     ay_now = Vy - Vy_pre_1;
00915     ax_pre_2 = ax_pre_1;
00916     ax_pre_1 = ax_now;
00917     ay_pre_2 = ay_pre_1;
00918     ay_pre_1 = ay_now;
00919     Vx_pre_1 = Vx;
00920     Vy_pre_1 = Vy;*/
00921     
00922 }
00923 void MeasureRobotAttitudeAngle(void){
00924     dt = t_MeasureRobotAttitudeAngle;
00925     imu.read_all();
00926     Mag_Complentary_Filter(dt,imu.Magnetometer);
00927     Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
00928     /*x_now = x_pre_1 + (1/(dt*dt))*(ax_now-2*ax_pre_1+ax_pre_2);
00929     y_now = y_pre_1 + (1/(dt*dt))*(ay_now-2*ay_pre_1+ay_pre_2);
00930     x_pre_1 = x_now;
00931     y_pre_1 = y_now;*/
00932     x_now = -((2*r_wheel)/3)*(-1.732*(-Angle_2+Angle_3));
00933     y_now = ((2*r_wheel)/3)*((-2*Angle_1+Angle_2+Angle_3)/(-1));
00934     do_measure_index++;
00935 }
00936 void Trajectory_generator(void){
00937     double t_trajectory = NowTime.read();
00938     
00939     if( RunTime>=0 &&( RunTime<=10) ){
00940     //x_trajectory = t_trajectory * (0.5);
00941         x_trajectory = 0;
00942         y_trajectory = 0;
00943         Arm_enable_index = 0;
00944     }else if( RunTime>=10  ){
00945         x_trajectory = 0;
00946         y_trajectory = 0;
00947         Arm_enable_index = 1;
00948     }
00949 }
00950 void Check_Arm_interrupt( double x_Now, double y_Now, double X_trajectory, double Y_trajectory){
00951     if( sqrt((x_Now-X_trajectory)*(x_Now-X_trajectory)+(y_Now-Y_trajectory)*(y_Now-Y_trajectory)) < 5 ){
00952         if( Arm_enable_index == 1 ){
00953             Arm_interrupt = 1;
00954             //Roll_offset = 2.0; 
00955             //Pitch_offset = 2.5;
00956         }
00957     }
00958 }
00959 //****************************main function************************************//
00960 int main() {
00961     Serial pc( USBTX, USBRX );
00962     pc.baud(460800);
00963 //****************************Angle Sensor initialization************************************//    
00964     if(imu.init(1,BITS_DLPF_CFG_188HZ)){  //INIT the mpu9250
00965         //pc.printf("\nCouldn't initialize MPU9250 via SPI!");
00966     }    
00967     //pc.printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
00968     imu.whoami();
00969     //wait(1);    
00970     //pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
00971     imu.set_gyro_scale(BITS_FS_2000DPS);
00972     //wait(1);  
00973     //pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
00974     imu.set_acc_scale(BITS_FS_16G);
00975     //wait(1);
00976     //pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
00977     imu.AK8963_whoami();
00978     //wait(0.1);  
00979     imu.AK8963_calib_Magnetometer();
00980 //****************************Motor driver declare************************************//    
00981     //ExtInt = 0;            //number high, voltage high.
00982     AR_1 = 1;
00983     AR_2 = 1;
00984     AR_3 = 1;
00985     control_Brake = 1;        //1:Run   0:Stop
00986 //**************************** PWM ************************************//
00987     PWM_Motor_1.calibrate(0.02, 0*0.02, 1*0.02);
00988     PWM_Motor_2.calibrate(0.02, 0*0.02, 1*0.02);
00989     PWM_Motor_3.calibrate(0.02, 0*0.02, 1*0.02);
00990 //**************************** Encoder ************************************//
00991     phaseA_1.mode( PullUp );
00992     phaseB_1.mode( PullUp );
00993     phaseA_2.mode( PullUp );
00994     phaseB_2.mode( PullUp );
00995     phaseA_3.mode( PullUp );
00996     phaseB_3.mode( PullUp );
00997 //**************************** Ticker ************************************//
00998     Ticker Sample_Motor_encoder_1;                                // create a timer to sample the encoder.
00999     Ticker Sample_Motor_encoder_2;                                // create a timer to sample the encoder.
01000     Ticker Sample_Motor_encoder_3;                                // create a timer to sample the encoder.
01001     Ticker Sample_robotAngleSonsor;                                // create a timer to sample the robot attitude.
01002     Ticker MeasureAngularVelocity;                      // create a timer to measure the motor angular velocity.
01003     Ticker PIDcontrol_velocity;                         // create a timer to do the motor angular velocity PID control.
01004     Ticker LQR_control;                                 // create a timer to do the position control.
01005     Ticker TrajectoryTracking_control;                  // create a timer to do the TrajectoryTracking_control.
01006 
01007 //**************************** Create the motor encoder sampler. ************************************//
01008     Sample_Motor_encoder_1.attach_us( &quadratureDecoder_1, t_quadratureDecoder ); 
01009     Sample_Motor_encoder_2.attach_us( &quadratureDecoder_2, t_quadratureDecoder ); 
01010     Sample_Motor_encoder_3.attach_us( &quadratureDecoder_3, t_quadratureDecoder ); 
01011 //**************************** Create the motor encoder sampler. ************************************//
01012     Sample_robotAngleSonsor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle ); 
01013 //**************************** Create the motor angular velocity measurement. ************************************//
01014     MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity ); 
01015 //**************************** Create the motor angular velocity PID control. ************************************//
01016     PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity );
01017 //**************************** Create the robot LQR control. ************************************//
01018     LQR_control.attach( &LQR_control_compute, t_LQR_control );
01019 //**************************** Create the motor posistion control. ************************************//
01020     TrajectoryTracking_control.attach( &Trajectory_generator, t_TrajectoryTracking_control );
01021 //**************************** Motor settlement ************************************//
01022     Motor_1.SetMode(AUTOMATIC);
01023     Motor_2.SetMode(AUTOMATIC);
01024     Motor_3.SetMode(AUTOMATIC);
01025 
01026 
01027     Command_AngularVel_1 = 0;
01028     Command_AngularVel_2 = 0;
01029     Command_AngularVel_3 = 0;
01030 //**************************** Timers start ************************************//
01031     NowTime.start();
01032 
01033     while( 1 ){ 
01034             
01035             RunTime = NowTime.read();
01036    
01037             if( (RunTime-lastTime) > 0.1  ){
01038                 index_times++;
01039                 lastTime = RunTime;
01040             }
01041             
01042             if( index_times >= 0.1 ){
01043                /* pc.printf("  Now_angularVelocity_1 : %.3f ",Now_angularVelocity_1);
01044                 pc.printf("  Now_angularVelocity_2 : %.3f ",Now_angularVelocity_2);
01045                 pc.printf("  Now_angularVelocity_3 : %.3f\n ",Now_angularVelocity_3);
01046                 pc.printf("  Vx : %.3f ",Vx);
01047                 pc.printf("  Vy : %.3f ",Vy);
01048                 //pc.printf(", Vy : %.3f ",Vy);
01049                 pc.printf(", Command_AngularVel_1 : %.3f ",Command_AngularVel_1);
01050                 pc.printf(", Command_AngularVel_2 : %.3f ",Command_AngularVel_2);
01051                 pc.printf(", Command_AngularVel_3 : %.3f \n",Command_AngularVel_3);*/
01052                 
01053                 /*pc.printf("x_now: %.3f   , y_now: %.3f , x_trajectory: %f   , u_y: %f ",x_now,y_now,x_trajectory,u_y);
01054                 pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f   \n", 
01055                             Roll,
01056                             Pitch,
01057                             Yaw
01058                             );*/
01059                 pc.printf(" %.3f   ,  %.3f , %.3f   , %.3f ",x_now,y_now,x_trajectory,y_trajectory);
01060                 pc.printf(",  %10.3f,  %10.3f,  %10.3f , %10.3f   \n", 
01061                             Roll,
01062                             Pitch,
01063                             Yaw,
01064                             RunTime
01065                             );
01066                 //pc.printf(",Control_motor_1 : %.3f ",Control_motor_1);
01067                 //pc.printf(", %.3f  ", Command_gularVel_1);
01068                 
01069                 /*pc.printf(", %.3f ",Control_motor_1);*/
01070                 //pc.printf(",  %.3f\n ",Now_angularVelocity_1);
01071                 index_times = 0;
01072             }
01073     Motor_run(Control_Motor_PWM_1,Control_Motor_PWM_2,Control_Motor_PWM_3,cw_ccw_1,cw_ccw_2,cw_ccw_3,control_Brake,control_StopRun);
01074     
01075     Check_Arm_interrupt(x_now,y_now,x_trajectory,y_trajectory);
01076             
01077     //wait(0.1);
01078     }
01079 }