123
Dependencies: StationKeeping MPU9250 PID Servo mbed
main.cpp
- Committer:
- d15321854
- Date:
- 2017-07-25
- Revision:
- 0:4fecb14ffbb8
File content as of revision 0:4fecb14ffbb8:
#include "mbed.h" #include "PID.h" #include "Servo.h" #include "MPU9250.h" //**************************** PID control para declaration ************************************// #define RATE 0.005 #define Kp_1 1.1 //1.1 #define Ki_1 0.18 //0.2 #define Kd_1 0.001 //**************************** Angle Sonsor para declaration ************************************// #define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases //**************************** pragram debug declaration ************************************// int index_times = 0; double RunTime =0,lastTime =0; int do_measure_index=0; int Arm_enable_index; DigitalOut Arm_interrupt(D9); double t_MeasureAngularVelocity=0.001; double t_PIDcontrol_velocity =0.001; double t_LQR_control = 0.005; double t_MeasureRobotAttitudeAngle = 0.005; double t_quadratureDecoder = 7; double t_TrajectoryTracking_control = 0.1; //**************************** Motor para declaration ************************************// double radius_ball = 0.15; double radius_robot = 0.105; double r_wheel = 0.05; //**************************** Motor declaration ************************************// //DigitalOut ExtInt(PB_7); DigitalOut Break_1(A5);//D15 DigitalOut Break_2(A4);//D14 DigitalOut Break_3(A3);//D8 DigitalOut AR_1(D15);//A5 DigitalOut AR_2(D14);//A4 DigitalOut AR_3(D8);//A3 DigitalOut CW_CCW_1(D7); DigitalOut CW_CCW_2(D6); DigitalOut CW_CCW_3(D5); Servo PWM_Motor_1(D2); Servo PWM_Motor_2(D3); Servo PWM_Motor_3(D4); int control_Brake,control_StopRun=0; int cw_ccw_1=0,cw_ccw_2=0,cw_ccw_3=0; //**************************** Measuremant of angular velocity declaration ************************************// double Angle_1 =0,Angle_2 =0,Angle_3 =0; double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0; double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4; double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4; double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4; double Average_SectionAngle_1,Now_angularVelocity_1=0; double Average_SectionAngle_2,Now_angularVelocity_2=0; double Average_SectionAngle_3,Now_angularVelocity_3=0; double SectionTime =0; double NowTime_measureVelocity=0, LastTime_measureVelocity = 0; //**************************** PID control declaration ************************************// float Now_time_PID,Last_Time_PID; //Command double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1; double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1; //Control double Control_motor_1,Control_motor_2,Control_motor_3; //Control PWM value double Control_Motor_PWM_1,Control_Motor_PWM_2,Control_Motor_PWM_3; 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); 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); 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); //****************************LQR_control declaration************************************// double Ka = 25; //25.0 double Kav = 10.7423; //10.4423 double Kt = 3.1 ; //3.1 double Kt_y = 3.1; //3.98 double Kv = 1.0; //1.0 double Kv_y = 1.0; //1.1 double Kz = 0.05; //0.05 double Kii = 0.15; //0.2 double KI_xy = 0.0085; //0.02 double KI_xy_y = 0.0085; //0.025 double Roll_offset = 2.1; //2.88 4.0 double Pitch_offset = 1.85; //-0.05 2.1 double Diff_Roll,Diff_Pitch,Diff_Yaw; double Integ_Roll,Integ_Pitch; double Integ_x,Integ_y; double Roll_last,Pitch_last; double Vx=0,Vy=0,Wz=0; double d_x=0,d_y=0; double u_x=0,u_y=0; double Point_x; double x_now,y_now; double x_pre_1,y_pre_1; double ax_now,ay_now; double ax_pre_1,ay_pre_1; double ax_pre_2,ay_pre_2; double Vx_pre_1,Vy_pre_1; double Diff_x,Diff_y; double Diff_x_pre,Diff_y_pre; double dot_diff_x,dot_diff_y; //****************************Trajectory Tracking declaration************************************// double x_trajectory,y_trajectory; //****************************Encoder declaration************************************// DigitalIn phaseA_1( PC_3 ); DigitalIn phaseB_1( PC_2 ); DigitalIn phaseA_2( PH_1 ); DigitalIn phaseB_2( PB_7 ); DigitalIn phaseA_3( PA_15 ); DigitalIn phaseB_3( PA_14 ); int encoderClickCount_1=0,previousEncoderState_1=0; int encoderClickCount_2=0,previousEncoderState_2=0; int encoderClickCount_3=0,previousEncoderState_3=0; //****************************Angle Sensor declaration************************************// float Times[10] = {0,0,0,0,0,0,0,0,0,0}; float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz int counter = 0; int divider = 20; float dt; // time for entire loop float dt_sensors; // time only to read sensors float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0; float exInt = 0, eyInt = 0, ezInt = 0; float OX = 0, OY = 0, OZ = 0; float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4; float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4; float Mag_x_pre,Mag_y_pre,Mag_z_pre; float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L; float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL; float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL; float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total; float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x; float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL; float GYRO_z_total,GYRO_z_offset,Global_GYRO_z; float Global_mag_vector_angle,Yaw_pre; float Global_mag_x_vector_angle,Mag_x_vector_angle; float Global_mag_y_vector_angle,Mag_y_vector_angle; int Count_mag_check=0; float angle[3]; float Roll,Pitch,Yaw; float calibrated_values[3],magCalibrationp[3]; float v_index[3]; float dest1,dest2; int f=0; int j=0; int k=0; int g=0; 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; int Rot_index; float mRes = 10.*4912./32760.0; int AngleFilter_counter = 0; float Roll_total = 0,Pitch_total = 0; SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK); mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object //****************************Timer declaration************************************// Timer NowTime; //**************************** Filter_IMUupdate ************************************// //**************************** Filter_IMUupdate ************************************// //**************************** Filter_IMUupdate ************************************// //**************************** Filter_IMUupdate ************************************// //**************************** Filter_IMUupdate ************************************// void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; // normalise the measurements norm = sqrt(ax*ax + ay*ay + az*az); if(norm == 0.0f) return; ax /= norm; ay /= norm; az /= norm; // estimated direction of gravity vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // error is sum of cross product between reference direction of field and direction measured by sensor ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); // integral error scaled integral gain exInt += ex*Ki; eyInt += ey*Ki; ezInt += ez*Ki; // adjusted gyroscope measurements gx += Kp*ex + exInt; gy += Kp*ey + eyInt; gz += Kp*ez + ezInt; // integrate quaternion rate and normalise 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! float q1o = q1; float q2o = q2; float q3o = q3; q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT; q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT; q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT; q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT; // normalise quaternion norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; } //**************************** Filter_compute ************************************// //**************************** Filter_compute ************************************// //**************************** Filter_compute ************************************// //**************************** Filter_compute ************************************// //**************************** Filter_compute ************************************// void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data) { // IMU/AHRS float d_Gyro_angle[3]; void get_Acc_angle(const float * Acc_data); // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/) float radGyro[3],Gyro_cal_data; // Gyro in radians per second for(int i=0; i<3; i++) radGyro[i] = Gyro_data[i] * 3.14159/ 180; Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]); //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]); float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2)); rangle[1] = asin (2*q0*q2 - 2*q3*q1); 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 for(int i=0; i<2; i++){ // angle in degree angle[i] = rangle[i] * 180 / 3.14159; } /*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; Roll_pre_4 = Roll_pre_3; Roll_pre_3 = Roll_pre_2; Roll_pre_2 = Roll_pre_1; Roll_pre_1 = Roll; 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; Pitch_pre_4 = Pitch_pre_3; Pitch_pre_3 = Pitch_pre_2; Pitch_pre_2 = Pitch_pre_1; Pitch_pre_1 = Pitch;*/ Roll_total += angle[0]; Pitch_total += angle[1]; AngleFilter_counter++; if( AngleFilter_counter > 1 ){ // The average of the attitude angle for 100 times. Roll = Roll_total / AngleFilter_counter; Pitch = Pitch_total / AngleFilter_counter; AngleFilter_counter = 0; Roll_total = 0; Pitch_total = 0; Roll -= Roll_offset; Pitch -= Pitch_offset; } //**************************************************Gyro_data[2] filter start float GYRO_z=0; 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; if( count4==1 ){ GYRO_z_pre_L=GYRO_z_pre; count4=0; } if( count5==2 ){ GYRO_z_pre_LL=GYRO_z_pre_L; count5=0; } if( count6==3 ){ GYRO_z_pre_LLL=GYRO_z_pre_LL; count6=0; } count4++; count5++; count6++; GYRO_z_pre=Gyro_data[2]; Global_GYRO_z=GYRO_z; /*printf(" GYRO_z:%10.3f ,count8:%10d \n", GYRO_z, count8 );*/ if((count8>5)&&(count8<=2005)){ GYRO_z_total+=GYRO_z; } if( count8==2005 ){ GYRO_z_offset=GYRO_z_total/2000; /* printf(" GYRO_z_offset:%10.5f \n ", GYRO_z_offset );*/ GYRO_z_total=0; count8=0; } count8++; //**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 0.74956 //************************************************** calculate Yaw if( (count11==35) ){ if( abs(Yaw_pre-Yaw)<1 ){ Yaw_pre=Yaw_pre; }else{ Yaw_pre=Yaw; } count11=0; } count11++; if( count12>=20 ){ Yaw += (Gyro_data[2]-0.74936) *dt; } count12++; //pc.printf(" Yaw:%10.5f ", //Yaw // ); } //**************************** Mag_Complentary_Filter ************************************// //**************************** Mag_Complentary_Filter ************************************// //**************************** Mag_Complentary_Filter ************************************// //**************************** Mag_Complentary_Filter ************************************// //**************************** Mag_Complentary_Filter ************************************// void Mag_Complentary_Filter(float dt, const float * Comp_data) { float Mag_x=0,Mag_y=0,Mag_z=0; 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; 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; 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; if( count1==1 ){ Mag_x_pre_L=Mag_x_pre; Mag_y_pre_L=Mag_y_pre; Mag_z_pre_L=Mag_z_pre; Cal_Mag_x_pre=Cal_Mag_x; count1=0; } if( count2==2 ){ Mag_x_pre_LL=Mag_x_pre_L; Mag_y_pre_LL=Mag_y_pre_L; Mag_z_pre_LL=Mag_z_pre_L; Cal_Mag_x_pre_L=Cal_Mag_x_pre; count2=0; } if( count7==3 ){ Mag_x_pre_LLL=Mag_x_pre_LL; Mag_y_pre_LLL=Mag_y_pre_LL; Mag_z_pre_LLL=Mag_z_pre_LL; Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L; count7=0; } count1++; count2++; count7++; Mag_x_pre=Comp_data[0]; Mag_y_pre=Comp_data[1]; Mag_z_pre=Comp_data[2]; if( count14>4 ){ Cal_Mag_x=Mag_x; } count14++; //*************************************Mag_ave calculate if(count3<=20){ Mag_x_total+=Mag_x; Mag_y_total+=Mag_y; } if( count3==20){ Mag_x_ave=Mag_x_total/21; Mag_y_ave=Mag_y_total/21; /*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ", Mag_x_ave, Mag_y_ave );*/ Mag_x_total=0; Mag_y_total=0; count3=0; } count3++; //********************************ROT_check start float v_length,v_length_ave,MagVector_angle; v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y ); v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave ); MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3; if( count9==3 ){ Global_mag_vector_angle=MagVector_angle; count9=0; } count9++; if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) ){ Count_mag_check++; }else{ Count_mag_check=0; } if( Count_mag_check==30 ){ Yaw=Yaw_pre; Count_mag_check=0; } float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle); //********************************Theta_check end /*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 ", ABS_CHECK, Cal_Mag_x_pre_LL, Mag_x, Count_mag_check, Yaw_pre, Yaw );*/ } //****************************Motor_run************************************// //****************************Motor_run************************************// //****************************Motor_run************************************// //****************************Motor_run************************************// //****************************Motor_run************************************// 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){ Break_1 = 0; Break_2 = 0; Break_3 = 0; //Brake = 1; /*CW_CCW_1 = 1; CW_CCW_2 = 1; CW_CCW_3 = 1;*/ CW_CCW_1 = control_CW_CCW_1; CW_CCW_2 = control_CW_CCW_2; CW_CCW_3 = control_CW_CCW_3; double Control_value_PWM_1 = 1-control_value_PWM_1; double Control_value_PWM_2 = 1-control_value_PWM_2; double Control_value_PWM_3 = 1-control_value_PWM_3; //printf("Control_value_PWM_2: %f\n",Control_value_PWM_2); //printf("Control_value_PWM_3: %f\n",Control_value_PWM_3); //PWM_Motor_1.write(Control_value_PWM_1); /*control_CW_CCW_1 = 0; control_CW_CCW_2 = 0; control_CW_CCW_3 = 0; AR_1 = 1; AR_2 = 1; AR_3 = 1; CW_CCW_1 = 1; CW_CCW_2 = 1; CW_CCW_3 = 1; PWM_Motor_1.write(0.75); PWM_Motor_2.write(0.75); PWM_Motor_3.write(0.75);*/ //wait_ms(0.5); PWM_Motor_1.write(Control_value_PWM_1); PWM_Motor_2.write(Control_value_PWM_2); PWM_Motor_3.write(Control_value_PWM_3); } //****************************quadratureDecoder_1************************************// //****************************quadratureDecoder_1************************************// //****************************quadratureDecoder_1************************************// //****************************quadratureDecoder_1************************************// //****************************quadratureDecoder_1************************************// void quadratureDecoder_1( void ) { int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read(); //**************************** 1 ************************************// if( currentEncoderState_1 == previousEncoderState_1 ) { return; } switch( previousEncoderState_1 ) { case 0: if( currentEncoderState_1 == 2 ) { encoderClickCount_1--; } else if( currentEncoderState_1 == 1 ) { encoderClickCount_1++; } break; case 1: if( currentEncoderState_1 == 0 ) { encoderClickCount_1--; } else if( currentEncoderState_1 == 3 ) { encoderClickCount_1++; } break; case 2: if( currentEncoderState_1 == 3 ) { encoderClickCount_1--; } else if( currentEncoderState_1 == 0 ) { encoderClickCount_1++; } break; case 3: if( currentEncoderState_1 == 1 ) { encoderClickCount_1--; } else if( currentEncoderState_1 == 2 ) { encoderClickCount_1++; } break; default: break; } previousEncoderState_1 = currentEncoderState_1; } void quadratureDecoder_2( void ) { int currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read(); //**************************** 2 ************************************// if( currentEncoderState_2 == previousEncoderState_2 ) { return; } switch( previousEncoderState_2 ) { case 0: if( currentEncoderState_2 == 2 ) { encoderClickCount_2--; } else if( currentEncoderState_2 == 1 ) { encoderClickCount_2++; } break; case 1: if( currentEncoderState_2 == 0 ) { encoderClickCount_2--; } else if( currentEncoderState_2 == 3 ) { encoderClickCount_2++; } break; case 2: if( currentEncoderState_2 == 3 ) { encoderClickCount_2--; } else if( currentEncoderState_2 == 0 ) { encoderClickCount_2++; } break; case 3: if( currentEncoderState_2 == 1 ) { encoderClickCount_2--; } else if( currentEncoderState_2 == 2 ) { encoderClickCount_2++; } break; default: break; } previousEncoderState_2 = currentEncoderState_2; } void quadratureDecoder_3( void ) { int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read(); //**************************** 3 ************************************// if( currentEncoderState_3 == previousEncoderState_3 ) { return; } switch( previousEncoderState_3 ) { case 0: if( currentEncoderState_3 == 2 ) { encoderClickCount_3--; } else if( currentEncoderState_3 == 1 ) { encoderClickCount_3++; } break; case 1: if( currentEncoderState_3 == 0 ) { encoderClickCount_3--; } else if( currentEncoderState_3 == 3 ) { encoderClickCount_3++; } break; case 2: if( currentEncoderState_3 == 3 ) { encoderClickCount_3--; } else if( currentEncoderState_3 == 0 ) { encoderClickCount_3++; } break; case 3: if( currentEncoderState_3 == 1 ) { encoderClickCount_3--; } else if( currentEncoderState_3 == 2 ) { encoderClickCount_3++; } break; default: break; } previousEncoderState_3 = currentEncoderState_3; } //****************************getAngular************************************// //****************************getAngular************************************// //****************************getAngular************************************// //****************************getAngular************************************// //****************************getAngular************************************// void getAngular( void ) { Angle_1 = (encoderClickCount_1*0.1499)/5; Angle_2 = (encoderClickCount_2*0.1499)/5; Angle_3 = (encoderClickCount_3*0.1499)/5; _1_SectionAngle = Angle_1 - LastAngle_1; _2_SectionAngle = Angle_2 - LastAngle_2; _3_SectionAngle = Angle_3 - LastAngle_3; 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; 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; 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; NowTime_measureVelocity = NowTime.read(); SectionTime = NowTime_measureVelocity - LastTime_measureVelocity; Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime)); Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime)); Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime)); LastTime_measureVelocity = NowTime_measureVelocity; LastAngle_1 = Angle_1; _1_LastSectionAngle_4 = _1_LastSectionAngle_3; _1_LastSectionAngle_3 = _1_LastSectionAngle_2; _1_LastSectionAngle_2 = _1_LastSectionAngle_1; _1_LastSectionAngle_1 = _1_LastSectionAngle; _1_LastSectionAngle = _1_SectionAngle; LastAngle_2 = Angle_2; _2_LastSectionAngle_4 = _2_LastSectionAngle_3; _2_LastSectionAngle_3 = _2_LastSectionAngle_2; _2_LastSectionAngle_2 = _2_LastSectionAngle_1; _2_LastSectionAngle_1 = _2_LastSectionAngle; _2_LastSectionAngle = _2_SectionAngle; LastAngle_3 = Angle_3; _3_LastSectionAngle_4 = _3_LastSectionAngle_3; _3_LastSectionAngle_3 = _3_LastSectionAngle_2; _3_LastSectionAngle_2 = _3_LastSectionAngle_1; _3_LastSectionAngle_1 = _3_LastSectionAngle; _3_LastSectionAngle = _3_SectionAngle; } //****************************PWM_commmand_transformation************************************// //****************************PWM_commmand_transformation************************************// //****************************PWM_commmand_transformation************************************// //****************************PWM_commmand_transformation************************************// //****************************PWM_commmand_transformation************************************// double PWM_commmand_transformation( double Control_AngVel_Value ){ double Control_PWM_Value = 0; //double control_CW_CCW = 0; //printf("*********************Control_AngVel_Value:%.3f\n",Control_AngVel_Value); if( Control_AngVel_Value > 0 ){ Control_AngVel_Value = Control_AngVel_Value; //control_CW_CCW = 0; }else if( Control_AngVel_Value < 0 ){ Control_AngVel_Value = -Control_AngVel_Value; //control_CW_CCW = 1; } if( Control_AngVel_Value > 325){ if( Control_AngVel_Value < 466 ){ if( Control_AngVel_Value < 393 ){ Control_PWM_Value = Control_AngVel_Value/651.6 ; //0.5~0.6 }else { Control_PWM_Value = Control_AngVel_Value/658.8; //0.6~0.7 } }else{ if( Control_AngVel_Value < 523 ){ Control_PWM_Value = Control_AngVel_Value/658.39; //0.7~0.8 }else{ if( Control_AngVel_Value < 588 ){ Control_PWM_Value = Control_AngVel_Value/652.36; //0.8~0.9 }else{ Control_PWM_Value = Control_AngVel_Value/655.11; //0.9~1 } } } }else if( Control_AngVel_Value < 325){ if( Control_AngVel_Value < 40 ){ Control_PWM_Value = Control_AngVel_Value/533.3; //0~0.075 }else{ if( Control_AngVel_Value < 59 ){ Control_PWM_Value = Control_AngVel_Value/560.65; //0.1~0.15 }else{ if( Control_AngVel_Value < 131 ){ Control_PWM_Value = Control_AngVel_Value/638.3; //0.15~0.2 }else{ if( Control_AngVel_Value < 197 ){ Control_PWM_Value = Control_AngVel_Value/651.6; //0.2~0.3 }else{ if( Control_AngVel_Value < 263 ){ Control_PWM_Value = Control_AngVel_Value/654.16; //0.3~0.4 }else{ Control_PWM_Value = Control_AngVel_Value/652.5; //0.4~0.5 } } } } } } return Control_PWM_Value; } //****************************PIDcontrol_compute_velocity************************************// //****************************PIDcontrol_compute_velocity************************************// //****************************PIDcontrol_compute_velocity************************************// //****************************PIDcontrol_compute_velocity************************************// //****************************PIDcontrol_compute_velocity************************************// void PIDcontrol_compute_velocity(void){ if(command_AngularVel_1 >= 0){ cw_ccw_1 = 0; Command_AngularVel_1 = command_AngularVel_1; }else{ cw_ccw_1 = 1; Command_AngularVel_1 = -command_AngularVel_1; } if(command_AngularVel_2 >= 0){ cw_ccw_2 = 0; Command_AngularVel_2 = command_AngularVel_2; }else{ cw_ccw_2 = 1; Command_AngularVel_2 = -command_AngularVel_2; } if(command_AngularVel_3 >= 0){ cw_ccw_3 = 0; Command_AngularVel_3 = command_AngularVel_3; }else{ cw_ccw_3 = 1; Command_AngularVel_3 = -command_AngularVel_3; } Now_time_PID=NowTime.read(); Motor_1.Compute(&Now_time_PID); Motor_2.Compute(&Now_time_PID); Motor_3.Compute(&Now_time_PID); Control_Motor_PWM_1 = PWM_commmand_transformation(Control_motor_1); Control_Motor_PWM_2 = PWM_commmand_transformation(Control_motor_2); Control_Motor_PWM_3 = PWM_commmand_transformation(Control_motor_3); //Control_Motor_PWM_1 = PWM_commmand_transformation(Command_AngularVel_1); //Control_Motor_PWM_2 = PWM_commmand_transformation(Command_AngularVel_2); //Control_Motor_PWM_3 = PWM_commmand_transformation(Command_AngularVel_3); //printf("Command_AngularVel_2: %f\n",Command_AngularVel_2); //printf("Control_motor_2: %f\n",Control_motor_2); } //****************************Compute_point************************************// //****************************Compute_point************************************// //****************************Compute_point************************************// //****************************Compute_point************************************// //****************************Compute_point************************************// void LQR_control_compute(void){ //Now_point_x = (angle/57.295)*3; //Now_point_x = Angle_1; /*if( Roll > 0.5 ){ d_x = Roll*0.01745*radius_ball; Vx = d_x/0.001; }else{ Vx = 0; } if(Pitch > 0.5){ d_y = Pitch*0.01745*radius_ball; Vy = d_y/0.001; }else{ Vy = 0; } if( Yaw > 0 ){ Wz = 0; }else{ Wz = 0; }*/ Diff_Roll = (Roll - Roll_last); Diff_Pitch = (Pitch - Pitch_last); Integ_Roll += Roll; //printf("Diff_Roll:%.3f\n",Diff_Roll); Integ_Pitch += Pitch; Roll_last = Roll; Pitch_last = Pitch; //Diff_Roll =0; //Roll -= 2.45; //Pitch -=2.5; Diff_x = x_now - x_trajectory; Diff_y = y_now - y_trajectory; dot_diff_x = Diff_x - Diff_x_pre; dot_diff_y = Diff_y - Diff_y_pre; Integ_x += Diff_x; Integ_y += Diff_y; //x_pre_1 = x_now; //y_pre_1 = y_now; Diff_x_pre = Diff_x; Diff_y_pre = Diff_y; u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x; //u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll - Kt*(10) - Kv*0; u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y; //u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch - Kt*0 - Kv*0; if(u_x > 100 ){u_x = 100;}else if(u_x < -100){u_x = -100;} if(u_y > 100 ){u_y = 100;}else if(u_y < -100){u_y = -100;} Vx = u_x; Vy = u_y; Wz = -Yaw; command_AngularVel_1 = (1/radius_ball)*(-Vy*(-0.5)+Kz*Wz); command_AngularVel_2 = (1/radius_ball)*((0.866*Vx+0.5*Vy)*(-0.5)+Kz*Wz); command_AngularVel_3 = (1/radius_ball)*((-0.866*Vx+0.5*Vy)*(-0.5)+Kz*Wz); //printf("Command_AngularVel_2: %f\n",Command_AngularVel_2); //printf("Command_AngularVel_3: %f\n",Command_AngularVel_3); /*ax_now = Vx - Vx_pre_1; ay_now = Vy - Vy_pre_1; ax_pre_2 = ax_pre_1; ax_pre_1 = ax_now; ay_pre_2 = ay_pre_1; ay_pre_1 = ay_now; Vx_pre_1 = Vx; Vy_pre_1 = Vy;*/ } void MeasureRobotAttitudeAngle(void){ dt = t_MeasureRobotAttitudeAngle; imu.read_all(); Mag_Complentary_Filter(dt,imu.Magnetometer); Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer); /*x_now = x_pre_1 + (1/(dt*dt))*(ax_now-2*ax_pre_1+ax_pre_2); y_now = y_pre_1 + (1/(dt*dt))*(ay_now-2*ay_pre_1+ay_pre_2); x_pre_1 = x_now; y_pre_1 = y_now;*/ x_now = -((2*r_wheel)/3)*(-1.732*(-Angle_2+Angle_3)); y_now = ((2*r_wheel)/3)*((-2*Angle_1+Angle_2+Angle_3)/(-1)); do_measure_index++; } void Trajectory_generator(void){ double t_trajectory = NowTime.read(); if( RunTime>=0 &&( RunTime<=10) ){ //x_trajectory = t_trajectory * (0.5); x_trajectory = 0; y_trajectory = 0; Arm_enable_index = 0; }else if( RunTime>=10 ){ x_trajectory = 0; y_trajectory = 0; Arm_enable_index = 1; } } void Check_Arm_interrupt( double x_Now, double y_Now, double X_trajectory, double Y_trajectory){ if( sqrt((x_Now-X_trajectory)*(x_Now-X_trajectory)+(y_Now-Y_trajectory)*(y_Now-Y_trajectory)) < 5 ){ if( Arm_enable_index == 1 ){ Arm_interrupt = 1; //Roll_offset = 2.0; //Pitch_offset = 2.5; } } } //****************************main function************************************// int main() { Serial pc( USBTX, USBRX ); pc.baud(460800); //****************************Angle Sensor initialization************************************// if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250 //pc.printf("\nCouldn't initialize MPU9250 via SPI!"); } //pc.printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104 imu.whoami(); //wait(1); //pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros imu.set_gyro_scale(BITS_FS_2000DPS); //wait(1); //pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs imu.set_acc_scale(BITS_FS_16G); //wait(1); //pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami()); imu.AK8963_whoami(); //wait(0.1); imu.AK8963_calib_Magnetometer(); //****************************Motor driver declare************************************// //ExtInt = 0; //number high, voltage high. AR_1 = 1; AR_2 = 1; AR_3 = 1; control_Brake = 1; //1:Run 0:Stop //**************************** PWM ************************************// PWM_Motor_1.calibrate(0.02, 0*0.02, 1*0.02); PWM_Motor_2.calibrate(0.02, 0*0.02, 1*0.02); PWM_Motor_3.calibrate(0.02, 0*0.02, 1*0.02); //**************************** Encoder ************************************// phaseA_1.mode( PullUp ); phaseB_1.mode( PullUp ); phaseA_2.mode( PullUp ); phaseB_2.mode( PullUp ); phaseA_3.mode( PullUp ); phaseB_3.mode( PullUp ); //**************************** Ticker ************************************// Ticker Sample_Motor_encoder_1; // create a timer to sample the encoder. Ticker Sample_Motor_encoder_2; // create a timer to sample the encoder. Ticker Sample_Motor_encoder_3; // create a timer to sample the encoder. Ticker Sample_robotAngleSonsor; // create a timer to sample the robot attitude. Ticker MeasureAngularVelocity; // create a timer to measure the motor angular velocity. Ticker PIDcontrol_velocity; // create a timer to do the motor angular velocity PID control. Ticker LQR_control; // create a timer to do the position control. Ticker TrajectoryTracking_control; // create a timer to do the TrajectoryTracking_control. //**************************** Create the motor encoder sampler. ************************************// Sample_Motor_encoder_1.attach_us( &quadratureDecoder_1, t_quadratureDecoder ); Sample_Motor_encoder_2.attach_us( &quadratureDecoder_2, t_quadratureDecoder ); Sample_Motor_encoder_3.attach_us( &quadratureDecoder_3, t_quadratureDecoder ); //**************************** Create the motor encoder sampler. ************************************// Sample_robotAngleSonsor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle ); //**************************** Create the motor angular velocity measurement. ************************************// MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity ); //**************************** Create the motor angular velocity PID control. ************************************// PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity ); //**************************** Create the robot LQR control. ************************************// LQR_control.attach( &LQR_control_compute, t_LQR_control ); //**************************** Create the motor posistion control. ************************************// TrajectoryTracking_control.attach( &Trajectory_generator, t_TrajectoryTracking_control ); //**************************** Motor settlement ************************************// Motor_1.SetMode(AUTOMATIC); Motor_2.SetMode(AUTOMATIC); Motor_3.SetMode(AUTOMATIC); Command_AngularVel_1 = 0; Command_AngularVel_2 = 0; Command_AngularVel_3 = 0; //**************************** Timers start ************************************// NowTime.start(); while( 1 ){ RunTime = NowTime.read(); if( (RunTime-lastTime) > 0.1 ){ index_times++; lastTime = RunTime; } if( index_times >= 0.1 ){ /* pc.printf(" Now_angularVelocity_1 : %.3f ",Now_angularVelocity_1); pc.printf(" Now_angularVelocity_2 : %.3f ",Now_angularVelocity_2); pc.printf(" Now_angularVelocity_3 : %.3f\n ",Now_angularVelocity_3); pc.printf(" Vx : %.3f ",Vx); pc.printf(" Vy : %.3f ",Vy); //pc.printf(", Vy : %.3f ",Vy); pc.printf(", Command_AngularVel_1 : %.3f ",Command_AngularVel_1); pc.printf(", Command_AngularVel_2 : %.3f ",Command_AngularVel_2); pc.printf(", Command_AngularVel_3 : %.3f \n",Command_AngularVel_3);*/ /*pc.printf("x_now: %.3f , y_now: %.3f , x_trajectory: %f , u_y: %f ",x_now,y_now,x_trajectory,u_y); pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f \n", Roll, Pitch, Yaw );*/ pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory); pc.printf(", %10.3f, %10.3f, %10.3f , %10.3f \n", Roll, Pitch, Yaw, RunTime ); //pc.printf(",Control_motor_1 : %.3f ",Control_motor_1); //pc.printf(", %.3f ", Command_gularVel_1); /*pc.printf(", %.3f ",Control_motor_1);*/ //pc.printf(", %.3f\n ",Now_angularVelocity_1); index_times = 0; } 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); Check_Arm_interrupt(x_now,y_now,x_trajectory,y_trajectory); //wait(0.1); } }