Samuel Lin
123
Dependencies: StationKeeping MPU9250 PID Servo mbed
Diff: main.cpp
- Revision:
- 0:4fecb14ffbb8
diff -r 000000000000 -r 4fecb14ffbb8 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jul 25 09:11:26 2017 +0000
@@ -0,0 +1,1079 @@
+#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);
+ }
+}