Samuel Lin / Mbed 2 deprecated Task_1_BallRidingbot_KeepingStationCatching_layingDown_1230

123

Dependencies:   StationKeeping MPU9250 PID Servo mbed

main.cpp@0:4fecb14ffbb8, 2017-07-25 (annotated)

Committer:
d15321854
Date:
Tue Jul 25 09:11:26 2017 +0000
Revision:
0:4fecb14ffbb8
123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
d15321854 0:4fecb14ffbb8 1 #include "mbed.h"
d15321854 0:4fecb14ffbb8 2 #include "PID.h"
d15321854 0:4fecb14ffbb8 3 #include "Servo.h"
d15321854 0:4fecb14ffbb8 4 #include "MPU9250.h"
d15321854 0:4fecb14ffbb8 5
d15321854 0:4fecb14ffbb8 6 //**************************** PID control para declaration ************************************//
d15321854 0:4fecb14ffbb8 7 #define RATE 0.005
d15321854 0:4fecb14ffbb8 8 #define Kp_1 1.1 //1.1
d15321854 0:4fecb14ffbb8 9 #define Ki_1 0.18 //0.2
d15321854 0:4fecb14ffbb8 10 #define Kd_1 0.001
d15321854 0:4fecb14ffbb8 11 //**************************** Angle Sonsor para declaration ************************************//
d15321854 0:4fecb14ffbb8 12 #define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer
d15321854 0:4fecb14ffbb8 13 #define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases
d15321854 0:4fecb14ffbb8 14 //**************************** pragram debug declaration ************************************//
d15321854 0:4fecb14ffbb8 15 int index_times = 0;
d15321854 0:4fecb14ffbb8 16 double RunTime =0,lastTime =0;
d15321854 0:4fecb14ffbb8 17 int do_measure_index=0;
d15321854 0:4fecb14ffbb8 18 int Arm_enable_index;
d15321854 0:4fecb14ffbb8 19 DigitalOut Arm_interrupt(D9);
d15321854 0:4fecb14ffbb8 20
d15321854 0:4fecb14ffbb8 21 double t_MeasureAngularVelocity=0.001;
d15321854 0:4fecb14ffbb8 22 double t_PIDcontrol_velocity =0.001;
d15321854 0:4fecb14ffbb8 23 double t_LQR_control = 0.005;
d15321854 0:4fecb14ffbb8 24 double t_MeasureRobotAttitudeAngle = 0.005;
d15321854 0:4fecb14ffbb8 25 double t_quadratureDecoder = 7;
d15321854 0:4fecb14ffbb8 26 double t_TrajectoryTracking_control = 0.1;
d15321854 0:4fecb14ffbb8 27 //**************************** Motor para declaration ************************************//
d15321854 0:4fecb14ffbb8 28 double radius_ball = 0.15;
d15321854 0:4fecb14ffbb8 29 double radius_robot = 0.105;
d15321854 0:4fecb14ffbb8 30 double r_wheel = 0.05;
d15321854 0:4fecb14ffbb8 31 //**************************** Motor declaration ************************************//
d15321854 0:4fecb14ffbb8 32 //DigitalOut ExtInt(PB_7);
d15321854 0:4fecb14ffbb8 33 DigitalOut Break_1(A5);//D15
d15321854 0:4fecb14ffbb8 34 DigitalOut Break_2(A4);//D14
d15321854 0:4fecb14ffbb8 35 DigitalOut Break_3(A3);//D8
d15321854 0:4fecb14ffbb8 36 DigitalOut AR_1(D15);//A5
d15321854 0:4fecb14ffbb8 37 DigitalOut AR_2(D14);//A4
d15321854 0:4fecb14ffbb8 38 DigitalOut AR_3(D8);//A3
d15321854 0:4fecb14ffbb8 39 DigitalOut CW_CCW_1(D7);
d15321854 0:4fecb14ffbb8 40 DigitalOut CW_CCW_2(D6);
d15321854 0:4fecb14ffbb8 41 DigitalOut CW_CCW_3(D5);
d15321854 0:4fecb14ffbb8 42 Servo PWM_Motor_1(D2);
d15321854 0:4fecb14ffbb8 43 Servo PWM_Motor_2(D3);
d15321854 0:4fecb14ffbb8 44 Servo PWM_Motor_3(D4);
d15321854 0:4fecb14ffbb8 45 int control_Brake,control_StopRun=0;
d15321854 0:4fecb14ffbb8 46 int cw_ccw_1=0,cw_ccw_2=0,cw_ccw_3=0;
d15321854 0:4fecb14ffbb8 47 //**************************** Measuremant of angular velocity declaration ************************************//
d15321854 0:4fecb14ffbb8 48 double Angle_1 =0,Angle_2 =0,Angle_3 =0;
d15321854 0:4fecb14ffbb8 49 double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0;
d15321854 0:4fecb14ffbb8 50 double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4;
d15321854 0:4fecb14ffbb8 51 double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4;
d15321854 0:4fecb14ffbb8 52 double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4;
d15321854 0:4fecb14ffbb8 53 double Average_SectionAngle_1,Now_angularVelocity_1=0;
d15321854 0:4fecb14ffbb8 54 double Average_SectionAngle_2,Now_angularVelocity_2=0;
d15321854 0:4fecb14ffbb8 55 double Average_SectionAngle_3,Now_angularVelocity_3=0;
d15321854 0:4fecb14ffbb8 56 double SectionTime =0;
d15321854 0:4fecb14ffbb8 57 double NowTime_measureVelocity=0, LastTime_measureVelocity = 0;
d15321854 0:4fecb14ffbb8 58 //**************************** PID control declaration ************************************//
d15321854 0:4fecb14ffbb8 59 float Now_time_PID,Last_Time_PID;
d15321854 0:4fecb14ffbb8 60 //Command
d15321854 0:4fecb14ffbb8 61 double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1;
d15321854 0:4fecb14ffbb8 62 double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1;
d15321854 0:4fecb14ffbb8 63 //Control
d15321854 0:4fecb14ffbb8 64 double Control_motor_1,Control_motor_2,Control_motor_3;
d15321854 0:4fecb14ffbb8 65 //Control PWM value
d15321854 0:4fecb14ffbb8 66 double Control_Motor_PWM_1,Control_Motor_PWM_2,Control_Motor_PWM_3;
d15321854 0:4fecb14ffbb8 67
d15321854 0:4fecb14ffbb8 68
d15321854 0:4fecb14ffbb8 69 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);
d15321854 0:4fecb14ffbb8 70 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);
d15321854 0:4fecb14ffbb8 71 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);
d15321854 0:4fecb14ffbb8 72 //****************************LQR_control declaration************************************//
d15321854 0:4fecb14ffbb8 73 double Ka = 25; //25.0
d15321854 0:4fecb14ffbb8 74 double Kav = 10.7423; //10.4423
d15321854 0:4fecb14ffbb8 75
d15321854 0:4fecb14ffbb8 76 double Kt = 3.1 ; //3.1
d15321854 0:4fecb14ffbb8 77 double Kt_y = 3.1; //3.98
d15321854 0:4fecb14ffbb8 78 double Kv = 1.0; //1.0
d15321854 0:4fecb14ffbb8 79 double Kv_y = 1.0; //1.1
d15321854 0:4fecb14ffbb8 80
d15321854 0:4fecb14ffbb8 81 double Kz = 0.05; //0.05
d15321854 0:4fecb14ffbb8 82 double Kii = 0.15; //0.2
d15321854 0:4fecb14ffbb8 83
d15321854 0:4fecb14ffbb8 84 double KI_xy = 0.0085; //0.02
d15321854 0:4fecb14ffbb8 85 double KI_xy_y = 0.0085; //0.025
d15321854 0:4fecb14ffbb8 86
d15321854 0:4fecb14ffbb8 87 double Roll_offset = 2.1; //2.88 4.0
d15321854 0:4fecb14ffbb8 88 double Pitch_offset = 1.85; //-0.05 2.1
d15321854 0:4fecb14ffbb8 89 double Diff_Roll,Diff_Pitch,Diff_Yaw;
d15321854 0:4fecb14ffbb8 90 double Integ_Roll,Integ_Pitch;
d15321854 0:4fecb14ffbb8 91 double Integ_x,Integ_y;
d15321854 0:4fecb14ffbb8 92 double Roll_last,Pitch_last;
d15321854 0:4fecb14ffbb8 93 double Vx=0,Vy=0,Wz=0;
d15321854 0:4fecb14ffbb8 94 double d_x=0,d_y=0;
d15321854 0:4fecb14ffbb8 95 double u_x=0,u_y=0;
d15321854 0:4fecb14ffbb8 96 double Point_x;
d15321854 0:4fecb14ffbb8 97 double x_now,y_now;
d15321854 0:4fecb14ffbb8 98 double x_pre_1,y_pre_1;
d15321854 0:4fecb14ffbb8 99 double ax_now,ay_now;
d15321854 0:4fecb14ffbb8 100 double ax_pre_1,ay_pre_1;
d15321854 0:4fecb14ffbb8 101 double ax_pre_2,ay_pre_2;
d15321854 0:4fecb14ffbb8 102 double Vx_pre_1,Vy_pre_1;
d15321854 0:4fecb14ffbb8 103 double Diff_x,Diff_y;
d15321854 0:4fecb14ffbb8 104 double Diff_x_pre,Diff_y_pre;
d15321854 0:4fecb14ffbb8 105 double dot_diff_x,dot_diff_y;
d15321854 0:4fecb14ffbb8 106 //****************************Trajectory Tracking declaration************************************//
d15321854 0:4fecb14ffbb8 107 double x_trajectory,y_trajectory;
d15321854 0:4fecb14ffbb8 108 //****************************Encoder declaration************************************//
d15321854 0:4fecb14ffbb8 109 DigitalIn phaseA_1( PC_3 );
d15321854 0:4fecb14ffbb8 110 DigitalIn phaseB_1( PC_2 );
d15321854 0:4fecb14ffbb8 111 DigitalIn phaseA_2( PH_1 );
d15321854 0:4fecb14ffbb8 112 DigitalIn phaseB_2( PB_7 );
d15321854 0:4fecb14ffbb8 113 DigitalIn phaseA_3( PA_15 );
d15321854 0:4fecb14ffbb8 114 DigitalIn phaseB_3( PA_14 );
d15321854 0:4fecb14ffbb8 115 int encoderClickCount_1=0,previousEncoderState_1=0;
d15321854 0:4fecb14ffbb8 116 int encoderClickCount_2=0,previousEncoderState_2=0;
d15321854 0:4fecb14ffbb8 117 int encoderClickCount_3=0,previousEncoderState_3=0;
d15321854 0:4fecb14ffbb8 118
d15321854 0:4fecb14ffbb8 119 //****************************Angle Sensor declaration************************************//
d15321854 0:4fecb14ffbb8 120 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
d15321854 0:4fecb14ffbb8 121 float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz
d15321854 0:4fecb14ffbb8 122 int counter = 0;
d15321854 0:4fecb14ffbb8 123 int divider = 20;
d15321854 0:4fecb14ffbb8 124 float dt; // time for entire loop
d15321854 0:4fecb14ffbb8 125 float dt_sensors; // time only to read sensors
d15321854 0:4fecb14ffbb8 126 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
d15321854 0:4fecb14ffbb8 127 float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
d15321854 0:4fecb14ffbb8 128 float exInt = 0, eyInt = 0, ezInt = 0;
d15321854 0:4fecb14ffbb8 129 float OX = 0, OY = 0, OZ = 0;
d15321854 0:4fecb14ffbb8 130 float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4;
d15321854 0:4fecb14ffbb8 131 float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4;
d15321854 0:4fecb14ffbb8 132 float Mag_x_pre,Mag_y_pre,Mag_z_pre;
d15321854 0:4fecb14ffbb8 133 float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
d15321854 0:4fecb14ffbb8 134 float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
d15321854 0:4fecb14ffbb8 135 float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
d15321854 0:4fecb14ffbb8 136 float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
d15321854 0:4fecb14ffbb8 137 float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
d15321854 0:4fecb14ffbb8 138 float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
d15321854 0:4fecb14ffbb8 139 float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
d15321854 0:4fecb14ffbb8 140 float Global_mag_vector_angle,Yaw_pre;
d15321854 0:4fecb14ffbb8 141 float Global_mag_x_vector_angle,Mag_x_vector_angle;
d15321854 0:4fecb14ffbb8 142 float Global_mag_y_vector_angle,Mag_y_vector_angle;
d15321854 0:4fecb14ffbb8 143 int Count_mag_check=0;
d15321854 0:4fecb14ffbb8 144 float angle[3];
d15321854 0:4fecb14ffbb8 145 float Roll,Pitch,Yaw;
d15321854 0:4fecb14ffbb8 146 float calibrated_values[3],magCalibrationp[3];
d15321854 0:4fecb14ffbb8 147 float v_index[3];
d15321854 0:4fecb14ffbb8 148 float dest1,dest2;
d15321854 0:4fecb14ffbb8 149 int f=0;
d15321854 0:4fecb14ffbb8 150 int j=0;
d15321854 0:4fecb14ffbb8 151 int k=0;
d15321854 0:4fecb14ffbb8 152 int g=0;
d15321854 0:4fecb14ffbb8 153 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;
d15321854 0:4fecb14ffbb8 154 int Rot_index;
d15321854 0:4fecb14ffbb8 155 float mRes = 10.*4912./32760.0;
d15321854 0:4fecb14ffbb8 156
d15321854 0:4fecb14ffbb8 157 int AngleFilter_counter = 0;
d15321854 0:4fecb14ffbb8 158 float Roll_total = 0,Pitch_total = 0;
d15321854 0:4fecb14ffbb8 159
d15321854 0:4fecb14ffbb8 160 SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
d15321854 0:4fecb14ffbb8 161 mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object
d15321854 0:4fecb14ffbb8 162 //****************************Timer declaration************************************//
d15321854 0:4fecb14ffbb8 163 Timer NowTime;
d15321854 0:4fecb14ffbb8 164 //**************************** Filter_IMUupdate ************************************//
d15321854 0:4fecb14ffbb8 165 //**************************** Filter_IMUupdate ************************************//
d15321854 0:4fecb14ffbb8 166 //**************************** Filter_IMUupdate ************************************//
d15321854 0:4fecb14ffbb8 167 //**************************** Filter_IMUupdate ************************************//
d15321854 0:4fecb14ffbb8 168 //**************************** Filter_IMUupdate ************************************//
d15321854 0:4fecb14ffbb8 169 void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) {
d15321854 0:4fecb14ffbb8 170 float norm;
d15321854 0:4fecb14ffbb8 171 float vx, vy, vz;
d15321854 0:4fecb14ffbb8 172 float ex, ey, ez;
d15321854 0:4fecb14ffbb8 173
d15321854 0:4fecb14ffbb8 174 // normalise the measurements
d15321854 0:4fecb14ffbb8 175 norm = sqrt(ax*ax + ay*ay + az*az);
d15321854 0:4fecb14ffbb8 176 if(norm == 0.0f) return;
d15321854 0:4fecb14ffbb8 177 ax /= norm;
d15321854 0:4fecb14ffbb8 178 ay /= norm;
d15321854 0:4fecb14ffbb8 179 az /= norm;
d15321854 0:4fecb14ffbb8 180
d15321854 0:4fecb14ffbb8 181 // estimated direction of gravity
d15321854 0:4fecb14ffbb8 182 vx = 2*(q1*q3 - q0*q2);
d15321854 0:4fecb14ffbb8 183 vy = 2*(q0*q1 + q2*q3);
d15321854 0:4fecb14ffbb8 184 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
d15321854 0:4fecb14ffbb8 185
d15321854 0:4fecb14ffbb8 186 // error is sum of cross product between reference direction of field and direction measured by sensor
d15321854 0:4fecb14ffbb8 187 ex = (ay*vz - az*vy);
d15321854 0:4fecb14ffbb8 188 ey = (az*vx - ax*vz);
d15321854 0:4fecb14ffbb8 189 ez = (ax*vy - ay*vx);
d15321854 0:4fecb14ffbb8 190
d15321854 0:4fecb14ffbb8 191 // integral error scaled integral gain
d15321854 0:4fecb14ffbb8 192 exInt += ex*Ki;
d15321854 0:4fecb14ffbb8 193 eyInt += ey*Ki;
d15321854 0:4fecb14ffbb8 194 ezInt += ez*Ki;
d15321854 0:4fecb14ffbb8 195
d15321854 0:4fecb14ffbb8 196 // adjusted gyroscope measurements
d15321854 0:4fecb14ffbb8 197 gx += Kp*ex + exInt;
d15321854 0:4fecb14ffbb8 198 gy += Kp*ey + eyInt;
d15321854 0:4fecb14ffbb8 199 gz += Kp*ez + ezInt;
d15321854 0:4fecb14ffbb8 200
d15321854 0:4fecb14ffbb8 201 // integrate quaternion rate and normalise
d15321854 0:4fecb14ffbb8 202 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!
d15321854 0:4fecb14ffbb8 203 float q1o = q1;
d15321854 0:4fecb14ffbb8 204 float q2o = q2;
d15321854 0:4fecb14ffbb8 205 float q3o = q3;
d15321854 0:4fecb14ffbb8 206 q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
d15321854 0:4fecb14ffbb8 207 q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
d15321854 0:4fecb14ffbb8 208 q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
d15321854 0:4fecb14ffbb8 209 q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;
d15321854 0:4fecb14ffbb8 210
d15321854 0:4fecb14ffbb8 211 // normalise quaternion
d15321854 0:4fecb14ffbb8 212 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
d15321854 0:4fecb14ffbb8 213 q0 = q0 / norm;
d15321854 0:4fecb14ffbb8 214 q1 = q1 / norm;
d15321854 0:4fecb14ffbb8 215 q2 = q2 / norm;
d15321854 0:4fecb14ffbb8 216 q3 = q3 / norm;
d15321854 0:4fecb14ffbb8 217 }
d15321854 0:4fecb14ffbb8 218 //**************************** Filter_compute ************************************//
d15321854 0:4fecb14ffbb8 219 //**************************** Filter_compute ************************************//
d15321854 0:4fecb14ffbb8 220 //**************************** Filter_compute ************************************//
d15321854 0:4fecb14ffbb8 221 //**************************** Filter_compute ************************************//
d15321854 0:4fecb14ffbb8 222 //**************************** Filter_compute ************************************//
d15321854 0:4fecb14ffbb8 223 void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
d15321854 0:4fecb14ffbb8 224 {
d15321854 0:4fecb14ffbb8 225 // IMU/AHRS
d15321854 0:4fecb14ffbb8 226
d15321854 0:4fecb14ffbb8 227 float d_Gyro_angle[3];
d15321854 0:4fecb14ffbb8 228 void get_Acc_angle(const float * Acc_data);
d15321854 0:4fecb14ffbb8 229 // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
d15321854 0:4fecb14ffbb8 230 float radGyro[3],Gyro_cal_data; // Gyro in radians per second
d15321854 0:4fecb14ffbb8 231
d15321854 0:4fecb14ffbb8 232 for(int i=0; i<3; i++)
d15321854 0:4fecb14ffbb8 233 radGyro[i] = Gyro_data[i] * 3.14159/ 180;
d15321854 0:4fecb14ffbb8 234
d15321854 0:4fecb14ffbb8 235 Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
d15321854 0:4fecb14ffbb8 236 //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]);
d15321854 0:4fecb14ffbb8 237
d15321854 0:4fecb14ffbb8 238 float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
d15321854 0:4fecb14ffbb8 239
d15321854 0:4fecb14ffbb8 240 rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
d15321854 0:4fecb14ffbb8 241 rangle[1] = asin (2*q0*q2 - 2*q3*q1);
d15321854 0:4fecb14ffbb8 242 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
d15321854 0:4fecb14ffbb8 243
d15321854 0:4fecb14ffbb8 244
d15321854 0:4fecb14ffbb8 245 for(int i=0; i<2; i++){ // angle in degree
d15321854 0:4fecb14ffbb8 246 angle[i] = rangle[i] * 180 / 3.14159;
d15321854 0:4fecb14ffbb8 247 }
d15321854 0:4fecb14ffbb8 248 /*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;
d15321854 0:4fecb14ffbb8 249 Roll_pre_4 = Roll_pre_3;
d15321854 0:4fecb14ffbb8 250 Roll_pre_3 = Roll_pre_2;
d15321854 0:4fecb14ffbb8 251 Roll_pre_2 = Roll_pre_1;
d15321854 0:4fecb14ffbb8 252 Roll_pre_1 = Roll;
d15321854 0:4fecb14ffbb8 253
d15321854 0:4fecb14ffbb8 254 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;
d15321854 0:4fecb14ffbb8 255 Pitch_pre_4 = Pitch_pre_3;
d15321854 0:4fecb14ffbb8 256 Pitch_pre_3 = Pitch_pre_2;
d15321854 0:4fecb14ffbb8 257 Pitch_pre_2 = Pitch_pre_1;
d15321854 0:4fecb14ffbb8 258 Pitch_pre_1 = Pitch;*/
d15321854 0:4fecb14ffbb8 259
d15321854 0:4fecb14ffbb8 260 Roll_total += angle[0];
d15321854 0:4fecb14ffbb8 261 Pitch_total += angle[1];
d15321854 0:4fecb14ffbb8 262 AngleFilter_counter++;
d15321854 0:4fecb14ffbb8 263
d15321854 0:4fecb14ffbb8 264 if( AngleFilter_counter > 1 ){ // The average of the attitude angle for 100 times.
d15321854 0:4fecb14ffbb8 265 Roll = Roll_total / AngleFilter_counter;
d15321854 0:4fecb14ffbb8 266 Pitch = Pitch_total / AngleFilter_counter;
d15321854 0:4fecb14ffbb8 267 AngleFilter_counter = 0;
d15321854 0:4fecb14ffbb8 268 Roll_total = 0;
d15321854 0:4fecb14ffbb8 269 Pitch_total = 0;
d15321854 0:4fecb14ffbb8 270 Roll -= Roll_offset;
d15321854 0:4fecb14ffbb8 271 Pitch -= Pitch_offset;
d15321854 0:4fecb14ffbb8 272 }
d15321854 0:4fecb14ffbb8 273
d15321854 0:4fecb14ffbb8 274 //**************************************************Gyro_data[2] filter start
d15321854 0:4fecb14ffbb8 275 float GYRO_z=0;
d15321854 0:4fecb14ffbb8 276
d15321854 0:4fecb14ffbb8 277 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;
d15321854 0:4fecb14ffbb8 278 if( count4==1 ){
d15321854 0:4fecb14ffbb8 279 GYRO_z_pre_L=GYRO_z_pre;
d15321854 0:4fecb14ffbb8 280
d15321854 0:4fecb14ffbb8 281 count4=0;
d15321854 0:4fecb14ffbb8 282 }
d15321854 0:4fecb14ffbb8 283 if( count5==2 ){
d15321854 0:4fecb14ffbb8 284 GYRO_z_pre_LL=GYRO_z_pre_L;
d15321854 0:4fecb14ffbb8 285
d15321854 0:4fecb14ffbb8 286 count5=0;
d15321854 0:4fecb14ffbb8 287 }
d15321854 0:4fecb14ffbb8 288 if( count6==3 ){
d15321854 0:4fecb14ffbb8 289 GYRO_z_pre_LLL=GYRO_z_pre_LL;
d15321854 0:4fecb14ffbb8 290
d15321854 0:4fecb14ffbb8 291 count6=0;
d15321854 0:4fecb14ffbb8 292 }
d15321854 0:4fecb14ffbb8 293
d15321854 0:4fecb14ffbb8 294
d15321854 0:4fecb14ffbb8 295
d15321854 0:4fecb14ffbb8 296 count4++;
d15321854 0:4fecb14ffbb8 297 count5++;
d15321854 0:4fecb14ffbb8 298 count6++;
d15321854 0:4fecb14ffbb8 299 GYRO_z_pre=Gyro_data[2];
d15321854 0:4fecb14ffbb8 300 Global_GYRO_z=GYRO_z;
d15321854 0:4fecb14ffbb8 301 /*printf(" GYRO_z:%10.3f ,count8:%10d \n",
d15321854 0:4fecb14ffbb8 302 GYRO_z,
d15321854 0:4fecb14ffbb8 303 count8
d15321854 0:4fecb14ffbb8 304 );*/
d15321854 0:4fecb14ffbb8 305 if((count8>5)&&(count8<=2005)){
d15321854 0:4fecb14ffbb8 306 GYRO_z_total+=GYRO_z;
d15321854 0:4fecb14ffbb8 307 }
d15321854 0:4fecb14ffbb8 308 if( count8==2005 ){
d15321854 0:4fecb14ffbb8 309 GYRO_z_offset=GYRO_z_total/2000;
d15321854 0:4fecb14ffbb8 310 /* printf(" GYRO_z_offset:%10.5f \n ",
d15321854 0:4fecb14ffbb8 311 GYRO_z_offset
d15321854 0:4fecb14ffbb8 312 );*/
d15321854 0:4fecb14ffbb8 313 GYRO_z_total=0;
d15321854 0:4fecb14ffbb8 314 count8=0;
d15321854 0:4fecb14ffbb8 315 }
d15321854 0:4fecb14ffbb8 316
d15321854 0:4fecb14ffbb8 317 count8++;
d15321854 0:4fecb14ffbb8 318 //**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 0.74956
d15321854 0:4fecb14ffbb8 319
d15321854 0:4fecb14ffbb8 320 //************************************************** calculate Yaw
d15321854 0:4fecb14ffbb8 321 if( (count11==35) ){
d15321854 0:4fecb14ffbb8 322 if( abs(Yaw_pre-Yaw)<1 ){
d15321854 0:4fecb14ffbb8 323 Yaw_pre=Yaw_pre;
d15321854 0:4fecb14ffbb8 324 }else{
d15321854 0:4fecb14ffbb8 325 Yaw_pre=Yaw;
d15321854 0:4fecb14ffbb8 326 }
d15321854 0:4fecb14ffbb8 327 count11=0;
d15321854 0:4fecb14ffbb8 328 }
d15321854 0:4fecb14ffbb8 329 count11++;
d15321854 0:4fecb14ffbb8 330
d15321854 0:4fecb14ffbb8 331 if( count12>=20 ){
d15321854 0:4fecb14ffbb8 332 Yaw += (Gyro_data[2]-0.74936) *dt;
d15321854 0:4fecb14ffbb8 333 }
d15321854 0:4fecb14ffbb8 334 count12++;
d15321854 0:4fecb14ffbb8 335 //pc.printf(" Yaw:%10.5f ",
d15321854 0:4fecb14ffbb8 336 //Yaw
d15321854 0:4fecb14ffbb8 337 // );
d15321854 0:4fecb14ffbb8 338 }
d15321854 0:4fecb14ffbb8 339 //**************************** Mag_Complentary_Filter ************************************//
d15321854 0:4fecb14ffbb8 340 //**************************** Mag_Complentary_Filter ************************************//
d15321854 0:4fecb14ffbb8 341 //**************************** Mag_Complentary_Filter ************************************//
d15321854 0:4fecb14ffbb8 342 //**************************** Mag_Complentary_Filter ************************************//
d15321854 0:4fecb14ffbb8 343 //**************************** Mag_Complentary_Filter ************************************//
d15321854 0:4fecb14ffbb8 344 void Mag_Complentary_Filter(float dt, const float * Comp_data)
d15321854 0:4fecb14ffbb8 345 {
d15321854 0:4fecb14ffbb8 346 float Mag_x=0,Mag_y=0,Mag_z=0;
d15321854 0:4fecb14ffbb8 347 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;
d15321854 0:4fecb14ffbb8 348 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;
d15321854 0:4fecb14ffbb8 349 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;
d15321854 0:4fecb14ffbb8 350
d15321854 0:4fecb14ffbb8 351
d15321854 0:4fecb14ffbb8 352 if( count1==1 ){
d15321854 0:4fecb14ffbb8 353 Mag_x_pre_L=Mag_x_pre;
d15321854 0:4fecb14ffbb8 354 Mag_y_pre_L=Mag_y_pre;
d15321854 0:4fecb14ffbb8 355 Mag_z_pre_L=Mag_z_pre;
d15321854 0:4fecb14ffbb8 356 Cal_Mag_x_pre=Cal_Mag_x;
d15321854 0:4fecb14ffbb8 357
d15321854 0:4fecb14ffbb8 358 count1=0;
d15321854 0:4fecb14ffbb8 359 }
d15321854 0:4fecb14ffbb8 360 if( count2==2 ){
d15321854 0:4fecb14ffbb8 361 Mag_x_pre_LL=Mag_x_pre_L;
d15321854 0:4fecb14ffbb8 362 Mag_y_pre_LL=Mag_y_pre_L;
d15321854 0:4fecb14ffbb8 363 Mag_z_pre_LL=Mag_z_pre_L;
d15321854 0:4fecb14ffbb8 364 Cal_Mag_x_pre_L=Cal_Mag_x_pre;
d15321854 0:4fecb14ffbb8 365
d15321854 0:4fecb14ffbb8 366 count2=0;
d15321854 0:4fecb14ffbb8 367 }
d15321854 0:4fecb14ffbb8 368 if( count7==3 ){
d15321854 0:4fecb14ffbb8 369 Mag_x_pre_LLL=Mag_x_pre_LL;
d15321854 0:4fecb14ffbb8 370 Mag_y_pre_LLL=Mag_y_pre_LL;
d15321854 0:4fecb14ffbb8 371 Mag_z_pre_LLL=Mag_z_pre_LL;
d15321854 0:4fecb14ffbb8 372 Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
d15321854 0:4fecb14ffbb8 373
d15321854 0:4fecb14ffbb8 374 count7=0;
d15321854 0:4fecb14ffbb8 375 }
d15321854 0:4fecb14ffbb8 376
d15321854 0:4fecb14ffbb8 377
d15321854 0:4fecb14ffbb8 378 count1++;
d15321854 0:4fecb14ffbb8 379 count2++;
d15321854 0:4fecb14ffbb8 380 count7++;
d15321854 0:4fecb14ffbb8 381 Mag_x_pre=Comp_data[0];
d15321854 0:4fecb14ffbb8 382 Mag_y_pre=Comp_data[1];
d15321854 0:4fecb14ffbb8 383 Mag_z_pre=Comp_data[2];
d15321854 0:4fecb14ffbb8 384 if( count14>4 ){
d15321854 0:4fecb14ffbb8 385 Cal_Mag_x=Mag_x;
d15321854 0:4fecb14ffbb8 386 }
d15321854 0:4fecb14ffbb8 387 count14++;
d15321854 0:4fecb14ffbb8 388
d15321854 0:4fecb14ffbb8 389
d15321854 0:4fecb14ffbb8 390 //*************************************Mag_ave calculate
d15321854 0:4fecb14ffbb8 391 if(count3<=20){
d15321854 0:4fecb14ffbb8 392 Mag_x_total+=Mag_x;
d15321854 0:4fecb14ffbb8 393 Mag_y_total+=Mag_y;
d15321854 0:4fecb14ffbb8 394 }
d15321854 0:4fecb14ffbb8 395 if( count3==20){
d15321854 0:4fecb14ffbb8 396 Mag_x_ave=Mag_x_total/21;
d15321854 0:4fecb14ffbb8 397 Mag_y_ave=Mag_y_total/21;
d15321854 0:4fecb14ffbb8 398 /*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ",
d15321854 0:4fecb14ffbb8 399 Mag_x_ave,
d15321854 0:4fecb14ffbb8 400 Mag_y_ave
d15321854 0:4fecb14ffbb8 401 );*/
d15321854 0:4fecb14ffbb8 402 Mag_x_total=0;
d15321854 0:4fecb14ffbb8 403 Mag_y_total=0;
d15321854 0:4fecb14ffbb8 404 count3=0;
d15321854 0:4fecb14ffbb8 405
d15321854 0:4fecb14ffbb8 406
d15321854 0:4fecb14ffbb8 407 }
d15321854 0:4fecb14ffbb8 408 count3++;
d15321854 0:4fecb14ffbb8 409
d15321854 0:4fecb14ffbb8 410 //********************************ROT_check start
d15321854 0:4fecb14ffbb8 411
d15321854 0:4fecb14ffbb8 412 float v_length,v_length_ave,MagVector_angle;
d15321854 0:4fecb14ffbb8 413 v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y );
d15321854 0:4fecb14ffbb8 414 v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave );
d15321854 0:4fecb14ffbb8 415
d15321854 0:4fecb14ffbb8 416 MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3;
d15321854 0:4fecb14ffbb8 417
d15321854 0:4fecb14ffbb8 418 if( count9==3 ){
d15321854 0:4fecb14ffbb8 419 Global_mag_vector_angle=MagVector_angle;
d15321854 0:4fecb14ffbb8 420 count9=0;
d15321854 0:4fecb14ffbb8 421 }
d15321854 0:4fecb14ffbb8 422 count9++;
d15321854 0:4fecb14ffbb8 423
d15321854 0:4fecb14ffbb8 424
d15321854 0:4fecb14ffbb8 425 if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) ){
d15321854 0:4fecb14ffbb8 426 Count_mag_check++;
d15321854 0:4fecb14ffbb8 427
d15321854 0:4fecb14ffbb8 428 }else{ Count_mag_check=0; }
d15321854 0:4fecb14ffbb8 429
d15321854 0:4fecb14ffbb8 430 if( Count_mag_check==30 ){
d15321854 0:4fecb14ffbb8 431 Yaw=Yaw_pre;
d15321854 0:4fecb14ffbb8 432 Count_mag_check=0;
d15321854 0:4fecb14ffbb8 433 }
d15321854 0:4fecb14ffbb8 434 float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
d15321854 0:4fecb14ffbb8 435 //********************************Theta_check end
d15321854 0:4fecb14ffbb8 436 /*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 ",
d15321854 0:4fecb14ffbb8 437 ABS_CHECK,
d15321854 0:4fecb14ffbb8 438 Cal_Mag_x_pre_LL,
d15321854 0:4fecb14ffbb8 439 Mag_x,
d15321854 0:4fecb14ffbb8 440 Count_mag_check,
d15321854 0:4fecb14ffbb8 441 Yaw_pre,
d15321854 0:4fecb14ffbb8 442 Yaw
d15321854 0:4fecb14ffbb8 443 );*/
d15321854 0:4fecb14ffbb8 444
d15321854 0:4fecb14ffbb8 445 }
d15321854 0:4fecb14ffbb8 446 //****************************Motor_run************************************//
d15321854 0:4fecb14ffbb8 447 //****************************Motor_run************************************//
d15321854 0:4fecb14ffbb8 448 //****************************Motor_run************************************//
d15321854 0:4fecb14ffbb8 449 //****************************Motor_run************************************//
d15321854 0:4fecb14ffbb8 450 //****************************Motor_run************************************//
d15321854 0:4fecb14ffbb8 451 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){
d15321854 0:4fecb14ffbb8 452
d15321854 0:4fecb14ffbb8 453 Break_1 = 0;
d15321854 0:4fecb14ffbb8 454 Break_2 = 0;
d15321854 0:4fecb14ffbb8 455 Break_3 = 0;
d15321854 0:4fecb14ffbb8 456 //Brake = 1;
d15321854 0:4fecb14ffbb8 457 /*CW_CCW_1 = 1;
d15321854 0:4fecb14ffbb8 458 CW_CCW_2 = 1;
d15321854 0:4fecb14ffbb8 459 CW_CCW_3 = 1;*/
d15321854 0:4fecb14ffbb8 460
d15321854 0:4fecb14ffbb8 461 CW_CCW_1 = control_CW_CCW_1;
d15321854 0:4fecb14ffbb8 462 CW_CCW_2 = control_CW_CCW_2;
d15321854 0:4fecb14ffbb8 463 CW_CCW_3 = control_CW_CCW_3;
d15321854 0:4fecb14ffbb8 464 double Control_value_PWM_1 = 1-control_value_PWM_1;
d15321854 0:4fecb14ffbb8 465 double Control_value_PWM_2 = 1-control_value_PWM_2;
d15321854 0:4fecb14ffbb8 466 double Control_value_PWM_3 = 1-control_value_PWM_3;
d15321854 0:4fecb14ffbb8 467 //printf("Control_value_PWM_2: %f\n",Control_value_PWM_2);
d15321854 0:4fecb14ffbb8 468 //printf("Control_value_PWM_3: %f\n",Control_value_PWM_3);
d15321854 0:4fecb14ffbb8 469 //PWM_Motor_1.write(Control_value_PWM_1);
d15321854 0:4fecb14ffbb8 470 /*control_CW_CCW_1 = 0;
d15321854 0:4fecb14ffbb8 471 control_CW_CCW_2 = 0;
d15321854 0:4fecb14ffbb8 472 control_CW_CCW_3 = 0;
d15321854 0:4fecb14ffbb8 473 AR_1 = 1;
d15321854 0:4fecb14ffbb8 474 AR_2 = 1;
d15321854 0:4fecb14ffbb8 475 AR_3 = 1;
d15321854 0:4fecb14ffbb8 476 CW_CCW_1 = 1;
d15321854 0:4fecb14ffbb8 477 CW_CCW_2 = 1;
d15321854 0:4fecb14ffbb8 478 CW_CCW_3 = 1;
d15321854 0:4fecb14ffbb8 479 PWM_Motor_1.write(0.75);
d15321854 0:4fecb14ffbb8 480 PWM_Motor_2.write(0.75);
d15321854 0:4fecb14ffbb8 481 PWM_Motor_3.write(0.75);*/
d15321854 0:4fecb14ffbb8 482 //wait_ms(0.5);
d15321854 0:4fecb14ffbb8 483 PWM_Motor_1.write(Control_value_PWM_1);
d15321854 0:4fecb14ffbb8 484 PWM_Motor_2.write(Control_value_PWM_2);
d15321854 0:4fecb14ffbb8 485 PWM_Motor_3.write(Control_value_PWM_3);
d15321854 0:4fecb14ffbb8 486
d15321854 0:4fecb14ffbb8 487 }
d15321854 0:4fecb14ffbb8 488 //****************************quadratureDecoder_1************************************//
d15321854 0:4fecb14ffbb8 489 //****************************quadratureDecoder_1************************************//
d15321854 0:4fecb14ffbb8 490 //****************************quadratureDecoder_1************************************//
d15321854 0:4fecb14ffbb8 491 //****************************quadratureDecoder_1************************************//
d15321854 0:4fecb14ffbb8 492 //****************************quadratureDecoder_1************************************//
d15321854 0:4fecb14ffbb8 493 void quadratureDecoder_1( void )
d15321854 0:4fecb14ffbb8 494 {
d15321854 0:4fecb14ffbb8 495 int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read();
d15321854 0:4fecb14ffbb8 496
d15321854 0:4fecb14ffbb8 497 //**************************** 1 ************************************//
d15321854 0:4fecb14ffbb8 498 if( currentEncoderState_1 == previousEncoderState_1 )
d15321854 0:4fecb14ffbb8 499 {
d15321854 0:4fecb14ffbb8 500 return;
d15321854 0:4fecb14ffbb8 501 }
d15321854 0:4fecb14ffbb8 502
d15321854 0:4fecb14ffbb8 503 switch( previousEncoderState_1 )
d15321854 0:4fecb14ffbb8 504 {
d15321854 0:4fecb14ffbb8 505 case 0:
d15321854 0:4fecb14ffbb8 506 if( currentEncoderState_1 == 2 )
d15321854 0:4fecb14ffbb8 507 {
d15321854 0:4fecb14ffbb8 508 encoderClickCount_1--;
d15321854 0:4fecb14ffbb8 509
d15321854 0:4fecb14ffbb8 510 }
d15321854 0:4fecb14ffbb8 511 else if( currentEncoderState_1 == 1 )
d15321854 0:4fecb14ffbb8 512 {
d15321854 0:4fecb14ffbb8 513 encoderClickCount_1++;
d15321854 0:4fecb14ffbb8 514
d15321854 0:4fecb14ffbb8 515 }
d15321854 0:4fecb14ffbb8 516 break;
d15321854 0:4fecb14ffbb8 517
d15321854 0:4fecb14ffbb8 518 case 1:
d15321854 0:4fecb14ffbb8 519 if( currentEncoderState_1 == 0 )
d15321854 0:4fecb14ffbb8 520 {
d15321854 0:4fecb14ffbb8 521 encoderClickCount_1--;
d15321854 0:4fecb14ffbb8 522
d15321854 0:4fecb14ffbb8 523 }
d15321854 0:4fecb14ffbb8 524 else if( currentEncoderState_1 == 3 )
d15321854 0:4fecb14ffbb8 525 {
d15321854 0:4fecb14ffbb8 526 encoderClickCount_1++;
d15321854 0:4fecb14ffbb8 527
d15321854 0:4fecb14ffbb8 528 }
d15321854 0:4fecb14ffbb8 529 break;
d15321854 0:4fecb14ffbb8 530
d15321854 0:4fecb14ffbb8 531 case 2:
d15321854 0:4fecb14ffbb8 532 if( currentEncoderState_1 == 3 )
d15321854 0:4fecb14ffbb8 533 {
d15321854 0:4fecb14ffbb8 534 encoderClickCount_1--;
d15321854 0:4fecb14ffbb8 535
d15321854 0:4fecb14ffbb8 536 }
d15321854 0:4fecb14ffbb8 537 else if( currentEncoderState_1 == 0 )
d15321854 0:4fecb14ffbb8 538 {
d15321854 0:4fecb14ffbb8 539 encoderClickCount_1++;
d15321854 0:4fecb14ffbb8 540
d15321854 0:4fecb14ffbb8 541 }
d15321854 0:4fecb14ffbb8 542 break;
d15321854 0:4fecb14ffbb8 543
d15321854 0:4fecb14ffbb8 544 case 3:
d15321854 0:4fecb14ffbb8 545 if( currentEncoderState_1 == 1 )
d15321854 0:4fecb14ffbb8 546 {
d15321854 0:4fecb14ffbb8 547 encoderClickCount_1--;
d15321854 0:4fecb14ffbb8 548
d15321854 0:4fecb14ffbb8 549 }
d15321854 0:4fecb14ffbb8 550 else if( currentEncoderState_1 == 2 )
d15321854 0:4fecb14ffbb8 551 {
d15321854 0:4fecb14ffbb8 552 encoderClickCount_1++;
d15321854 0:4fecb14ffbb8 553
d15321854 0:4fecb14ffbb8 554 }
d15321854 0:4fecb14ffbb8 555 break;
d15321854 0:4fecb14ffbb8 556
d15321854 0:4fecb14ffbb8 557 default:
d15321854 0:4fecb14ffbb8 558 break;
d15321854 0:4fecb14ffbb8 559 }
d15321854 0:4fecb14ffbb8 560 previousEncoderState_1 = currentEncoderState_1;
d15321854 0:4fecb14ffbb8 561
d15321854 0:4fecb14ffbb8 562 }
d15321854 0:4fecb14ffbb8 563 void quadratureDecoder_2( void )
d15321854 0:4fecb14ffbb8 564 {
d15321854 0:4fecb14ffbb8 565 int currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read();
d15321854 0:4fecb14ffbb8 566
d15321854 0:4fecb14ffbb8 567 //**************************** 2 ************************************//
d15321854 0:4fecb14ffbb8 568 if( currentEncoderState_2 == previousEncoderState_2 )
d15321854 0:4fecb14ffbb8 569 {
d15321854 0:4fecb14ffbb8 570 return;
d15321854 0:4fecb14ffbb8 571 }
d15321854 0:4fecb14ffbb8 572
d15321854 0:4fecb14ffbb8 573 switch( previousEncoderState_2 )
d15321854 0:4fecb14ffbb8 574 {
d15321854 0:4fecb14ffbb8 575 case 0:
d15321854 0:4fecb14ffbb8 576 if( currentEncoderState_2 == 2 )
d15321854 0:4fecb14ffbb8 577 {
d15321854 0:4fecb14ffbb8 578 encoderClickCount_2--;
d15321854 0:4fecb14ffbb8 579
d15321854 0:4fecb14ffbb8 580 }
d15321854 0:4fecb14ffbb8 581 else if( currentEncoderState_2 == 1 )
d15321854 0:4fecb14ffbb8 582 {
d15321854 0:4fecb14ffbb8 583 encoderClickCount_2++;
d15321854 0:4fecb14ffbb8 584
d15321854 0:4fecb14ffbb8 585 }
d15321854 0:4fecb14ffbb8 586 break;
d15321854 0:4fecb14ffbb8 587
d15321854 0:4fecb14ffbb8 588 case 1:
d15321854 0:4fecb14ffbb8 589 if( currentEncoderState_2 == 0 )
d15321854 0:4fecb14ffbb8 590 {
d15321854 0:4fecb14ffbb8 591 encoderClickCount_2--;
d15321854 0:4fecb14ffbb8 592
d15321854 0:4fecb14ffbb8 593 }
d15321854 0:4fecb14ffbb8 594 else if( currentEncoderState_2 == 3 )
d15321854 0:4fecb14ffbb8 595 {
d15321854 0:4fecb14ffbb8 596 encoderClickCount_2++;
d15321854 0:4fecb14ffbb8 597
d15321854 0:4fecb14ffbb8 598 }
d15321854 0:4fecb14ffbb8 599 break;
d15321854 0:4fecb14ffbb8 600
d15321854 0:4fecb14ffbb8 601 case 2:
d15321854 0:4fecb14ffbb8 602 if( currentEncoderState_2 == 3 )
d15321854 0:4fecb14ffbb8 603 {
d15321854 0:4fecb14ffbb8 604 encoderClickCount_2--;
d15321854 0:4fecb14ffbb8 605
d15321854 0:4fecb14ffbb8 606 }
d15321854 0:4fecb14ffbb8 607 else if( currentEncoderState_2 == 0 )
d15321854 0:4fecb14ffbb8 608 {
d15321854 0:4fecb14ffbb8 609 encoderClickCount_2++;
d15321854 0:4fecb14ffbb8 610
d15321854 0:4fecb14ffbb8 611 }
d15321854 0:4fecb14ffbb8 612 break;
d15321854 0:4fecb14ffbb8 613
d15321854 0:4fecb14ffbb8 614 case 3:
d15321854 0:4fecb14ffbb8 615 if( currentEncoderState_2 == 1 )
d15321854 0:4fecb14ffbb8 616 {
d15321854 0:4fecb14ffbb8 617 encoderClickCount_2--;
d15321854 0:4fecb14ffbb8 618
d15321854 0:4fecb14ffbb8 619 }
d15321854 0:4fecb14ffbb8 620 else if( currentEncoderState_2 == 2 )
d15321854 0:4fecb14ffbb8 621 {
d15321854 0:4fecb14ffbb8 622 encoderClickCount_2++;
d15321854 0:4fecb14ffbb8 623
d15321854 0:4fecb14ffbb8 624 }
d15321854 0:4fecb14ffbb8 625 break;
d15321854 0:4fecb14ffbb8 626
d15321854 0:4fecb14ffbb8 627 default:
d15321854 0:4fecb14ffbb8 628 break;
d15321854 0:4fecb14ffbb8 629 }
d15321854 0:4fecb14ffbb8 630 previousEncoderState_2 = currentEncoderState_2;
d15321854 0:4fecb14ffbb8 631 }
d15321854 0:4fecb14ffbb8 632 void quadratureDecoder_3( void )
d15321854 0:4fecb14ffbb8 633 {
d15321854 0:4fecb14ffbb8 634 int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read();
d15321854 0:4fecb14ffbb8 635
d15321854 0:4fecb14ffbb8 636 //**************************** 3 ************************************//
d15321854 0:4fecb14ffbb8 637 if( currentEncoderState_3 == previousEncoderState_3 )
d15321854 0:4fecb14ffbb8 638 {
d15321854 0:4fecb14ffbb8 639 return;
d15321854 0:4fecb14ffbb8 640 }
d15321854 0:4fecb14ffbb8 641
d15321854 0:4fecb14ffbb8 642 switch( previousEncoderState_3 )
d15321854 0:4fecb14ffbb8 643 {
d15321854 0:4fecb14ffbb8 644 case 0:
d15321854 0:4fecb14ffbb8 645 if( currentEncoderState_3 == 2 )
d15321854 0:4fecb14ffbb8 646 {
d15321854 0:4fecb14ffbb8 647 encoderClickCount_3--;
d15321854 0:4fecb14ffbb8 648
d15321854 0:4fecb14ffbb8 649 }
d15321854 0:4fecb14ffbb8 650 else if( currentEncoderState_3 == 1 )
d15321854 0:4fecb14ffbb8 651 {
d15321854 0:4fecb14ffbb8 652 encoderClickCount_3++;
d15321854 0:4fecb14ffbb8 653
d15321854 0:4fecb14ffbb8 654 }
d15321854 0:4fecb14ffbb8 655 break;
d15321854 0:4fecb14ffbb8 656
d15321854 0:4fecb14ffbb8 657 case 1:
d15321854 0:4fecb14ffbb8 658 if( currentEncoderState_3 == 0 )
d15321854 0:4fecb14ffbb8 659 {
d15321854 0:4fecb14ffbb8 660 encoderClickCount_3--;
d15321854 0:4fecb14ffbb8 661
d15321854 0:4fecb14ffbb8 662 }
d15321854 0:4fecb14ffbb8 663 else if( currentEncoderState_3 == 3 )
d15321854 0:4fecb14ffbb8 664 {
d15321854 0:4fecb14ffbb8 665 encoderClickCount_3++;
d15321854 0:4fecb14ffbb8 666
d15321854 0:4fecb14ffbb8 667 }
d15321854 0:4fecb14ffbb8 668 break;
d15321854 0:4fecb14ffbb8 669
d15321854 0:4fecb14ffbb8 670 case 2:
d15321854 0:4fecb14ffbb8 671 if( currentEncoderState_3 == 3 )
d15321854 0:4fecb14ffbb8 672 {
d15321854 0:4fecb14ffbb8 673 encoderClickCount_3--;
d15321854 0:4fecb14ffbb8 674
d15321854 0:4fecb14ffbb8 675 }
d15321854 0:4fecb14ffbb8 676 else if( currentEncoderState_3 == 0 )
d15321854 0:4fecb14ffbb8 677 {
d15321854 0:4fecb14ffbb8 678 encoderClickCount_3++;
d15321854 0:4fecb14ffbb8 679
d15321854 0:4fecb14ffbb8 680 }
d15321854 0:4fecb14ffbb8 681 break;
d15321854 0:4fecb14ffbb8 682
d15321854 0:4fecb14ffbb8 683 case 3:
d15321854 0:4fecb14ffbb8 684 if( currentEncoderState_3 == 1 )
d15321854 0:4fecb14ffbb8 685 {
d15321854 0:4fecb14ffbb8 686 encoderClickCount_3--;
d15321854 0:4fecb14ffbb8 687
d15321854 0:4fecb14ffbb8 688 }
d15321854 0:4fecb14ffbb8 689 else if( currentEncoderState_3 == 2 )
d15321854 0:4fecb14ffbb8 690 {
d15321854 0:4fecb14ffbb8 691 encoderClickCount_3++;
d15321854 0:4fecb14ffbb8 692
d15321854 0:4fecb14ffbb8 693 }
d15321854 0:4fecb14ffbb8 694 break;
d15321854 0:4fecb14ffbb8 695
d15321854 0:4fecb14ffbb8 696 default:
d15321854 0:4fecb14ffbb8 697 break;
d15321854 0:4fecb14ffbb8 698 }
d15321854 0:4fecb14ffbb8 699 previousEncoderState_3 = currentEncoderState_3;
d15321854 0:4fecb14ffbb8 700
d15321854 0:4fecb14ffbb8 701 }
d15321854 0:4fecb14ffbb8 702 //****************************getAngular************************************//
d15321854 0:4fecb14ffbb8 703 //****************************getAngular************************************//
d15321854 0:4fecb14ffbb8 704 //****************************getAngular************************************//
d15321854 0:4fecb14ffbb8 705 //****************************getAngular************************************//
d15321854 0:4fecb14ffbb8 706 //****************************getAngular************************************//
d15321854 0:4fecb14ffbb8 707 void getAngular( void )
d15321854 0:4fecb14ffbb8 708 {
d15321854 0:4fecb14ffbb8 709 Angle_1 = (encoderClickCount_1*0.1499)/5;
d15321854 0:4fecb14ffbb8 710 Angle_2 = (encoderClickCount_2*0.1499)/5;
d15321854 0:4fecb14ffbb8 711 Angle_3 = (encoderClickCount_3*0.1499)/5;
d15321854 0:4fecb14ffbb8 712
d15321854 0:4fecb14ffbb8 713
d15321854 0:4fecb14ffbb8 714 _1_SectionAngle = Angle_1 - LastAngle_1;
d15321854 0:4fecb14ffbb8 715 _2_SectionAngle = Angle_2 - LastAngle_2;
d15321854 0:4fecb14ffbb8 716 _3_SectionAngle = Angle_3 - LastAngle_3;
d15321854 0:4fecb14ffbb8 717 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;
d15321854 0:4fecb14ffbb8 718 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;
d15321854 0:4fecb14ffbb8 719 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;
d15321854 0:4fecb14ffbb8 720 NowTime_measureVelocity = NowTime.read();
d15321854 0:4fecb14ffbb8 721 SectionTime = NowTime_measureVelocity - LastTime_measureVelocity;
d15321854 0:4fecb14ffbb8 722 Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime));
d15321854 0:4fecb14ffbb8 723 Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime));
d15321854 0:4fecb14ffbb8 724 Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime));
d15321854 0:4fecb14ffbb8 725
d15321854 0:4fecb14ffbb8 726
d15321854 0:4fecb14ffbb8 727 LastTime_measureVelocity = NowTime_measureVelocity;
d15321854 0:4fecb14ffbb8 728 LastAngle_1 = Angle_1;
d15321854 0:4fecb14ffbb8 729 _1_LastSectionAngle_4 = _1_LastSectionAngle_3;
d15321854 0:4fecb14ffbb8 730 _1_LastSectionAngle_3 = _1_LastSectionAngle_2;
d15321854 0:4fecb14ffbb8 731 _1_LastSectionAngle_2 = _1_LastSectionAngle_1;
d15321854 0:4fecb14ffbb8 732 _1_LastSectionAngle_1 = _1_LastSectionAngle;
d15321854 0:4fecb14ffbb8 733 _1_LastSectionAngle = _1_SectionAngle;
d15321854 0:4fecb14ffbb8 734 LastAngle_2 = Angle_2;
d15321854 0:4fecb14ffbb8 735 _2_LastSectionAngle_4 = _2_LastSectionAngle_3;
d15321854 0:4fecb14ffbb8 736 _2_LastSectionAngle_3 = _2_LastSectionAngle_2;
d15321854 0:4fecb14ffbb8 737 _2_LastSectionAngle_2 = _2_LastSectionAngle_1;
d15321854 0:4fecb14ffbb8 738 _2_LastSectionAngle_1 = _2_LastSectionAngle;
d15321854 0:4fecb14ffbb8 739 _2_LastSectionAngle = _2_SectionAngle;
d15321854 0:4fecb14ffbb8 740 LastAngle_3 = Angle_3;
d15321854 0:4fecb14ffbb8 741 _3_LastSectionAngle_4 = _3_LastSectionAngle_3;
d15321854 0:4fecb14ffbb8 742 _3_LastSectionAngle_3 = _3_LastSectionAngle_2;
d15321854 0:4fecb14ffbb8 743 _3_LastSectionAngle_2 = _3_LastSectionAngle_1;
d15321854 0:4fecb14ffbb8 744 _3_LastSectionAngle_1 = _3_LastSectionAngle;
d15321854 0:4fecb14ffbb8 745 _3_LastSectionAngle = _3_SectionAngle;
d15321854 0:4fecb14ffbb8 746 }
d15321854 0:4fecb14ffbb8 747 //****************************PWM_commmand_transformation************************************//
d15321854 0:4fecb14ffbb8 748 //****************************PWM_commmand_transformation************************************//
d15321854 0:4fecb14ffbb8 749 //****************************PWM_commmand_transformation************************************//
d15321854 0:4fecb14ffbb8 750 //****************************PWM_commmand_transformation************************************//
d15321854 0:4fecb14ffbb8 751 //****************************PWM_commmand_transformation************************************//
d15321854 0:4fecb14ffbb8 752 double PWM_commmand_transformation( double Control_AngVel_Value ){
d15321854 0:4fecb14ffbb8 753 double Control_PWM_Value = 0;
d15321854 0:4fecb14ffbb8 754 //double control_CW_CCW = 0;
d15321854 0:4fecb14ffbb8 755 //printf("*********************Control_AngVel_Value:%.3f\n",Control_AngVel_Value);
d15321854 0:4fecb14ffbb8 756 if( Control_AngVel_Value > 0 ){
d15321854 0:4fecb14ffbb8 757 Control_AngVel_Value = Control_AngVel_Value;
d15321854 0:4fecb14ffbb8 758 //control_CW_CCW = 0;
d15321854 0:4fecb14ffbb8 759
d15321854 0:4fecb14ffbb8 760 }else if( Control_AngVel_Value < 0 ){
d15321854 0:4fecb14ffbb8 761 Control_AngVel_Value = -Control_AngVel_Value;
d15321854 0:4fecb14ffbb8 762 //control_CW_CCW = 1;
d15321854 0:4fecb14ffbb8 763
d15321854 0:4fecb14ffbb8 764 }
d15321854 0:4fecb14ffbb8 765 if( Control_AngVel_Value > 325){
d15321854 0:4fecb14ffbb8 766 if( Control_AngVel_Value < 466 ){
d15321854 0:4fecb14ffbb8 767 if( Control_AngVel_Value < 393 ){
d15321854 0:4fecb14ffbb8 768 Control_PWM_Value = Control_AngVel_Value/651.6 ; //0.5~0.6
d15321854 0:4fecb14ffbb8 769 }else {
d15321854 0:4fecb14ffbb8 770 Control_PWM_Value = Control_AngVel_Value/658.8; //0.6~0.7
d15321854 0:4fecb14ffbb8 771 }
d15321854 0:4fecb14ffbb8 772 }else{
d15321854 0:4fecb14ffbb8 773 if( Control_AngVel_Value < 523 ){
d15321854 0:4fecb14ffbb8 774 Control_PWM_Value = Control_AngVel_Value/658.39; //0.7~0.8
d15321854 0:4fecb14ffbb8 775 }else{
d15321854 0:4fecb14ffbb8 776 if( Control_AngVel_Value < 588 ){
d15321854 0:4fecb14ffbb8 777 Control_PWM_Value = Control_AngVel_Value/652.36; //0.8~0.9
d15321854 0:4fecb14ffbb8 778 }else{
d15321854 0:4fecb14ffbb8 779 Control_PWM_Value = Control_AngVel_Value/655.11; //0.9~1
d15321854 0:4fecb14ffbb8 780 }
d15321854 0:4fecb14ffbb8 781 }
d15321854 0:4fecb14ffbb8 782 }
d15321854 0:4fecb14ffbb8 783 }else if( Control_AngVel_Value < 325){
d15321854 0:4fecb14ffbb8 784 if( Control_AngVel_Value < 40 ){
d15321854 0:4fecb14ffbb8 785 Control_PWM_Value = Control_AngVel_Value/533.3; //0~0.075
d15321854 0:4fecb14ffbb8 786 }else{
d15321854 0:4fecb14ffbb8 787 if( Control_AngVel_Value < 59 ){
d15321854 0:4fecb14ffbb8 788 Control_PWM_Value = Control_AngVel_Value/560.65; //0.1~0.15
d15321854 0:4fecb14ffbb8 789 }else{
d15321854 0:4fecb14ffbb8 790 if( Control_AngVel_Value < 131 ){
d15321854 0:4fecb14ffbb8 791 Control_PWM_Value = Control_AngVel_Value/638.3; //0.15~0.2
d15321854 0:4fecb14ffbb8 792 }else{
d15321854 0:4fecb14ffbb8 793 if( Control_AngVel_Value < 197 ){
d15321854 0:4fecb14ffbb8 794 Control_PWM_Value = Control_AngVel_Value/651.6; //0.2~0.3
d15321854 0:4fecb14ffbb8 795 }else{
d15321854 0:4fecb14ffbb8 796 if( Control_AngVel_Value < 263 ){
d15321854 0:4fecb14ffbb8 797 Control_PWM_Value = Control_AngVel_Value/654.16; //0.3~0.4
d15321854 0:4fecb14ffbb8 798 }else{
d15321854 0:4fecb14ffbb8 799 Control_PWM_Value = Control_AngVel_Value/652.5; //0.4~0.5
d15321854 0:4fecb14ffbb8 800 }
d15321854 0:4fecb14ffbb8 801 }
d15321854 0:4fecb14ffbb8 802 }
d15321854 0:4fecb14ffbb8 803 }
d15321854 0:4fecb14ffbb8 804 }
d15321854 0:4fecb14ffbb8 805 }
d15321854 0:4fecb14ffbb8 806 return Control_PWM_Value;
d15321854 0:4fecb14ffbb8 807 }
d15321854 0:4fecb14ffbb8 808 //****************************PIDcontrol_compute_velocity************************************//
d15321854 0:4fecb14ffbb8 809 //****************************PIDcontrol_compute_velocity************************************//
d15321854 0:4fecb14ffbb8 810 //****************************PIDcontrol_compute_velocity************************************//
d15321854 0:4fecb14ffbb8 811 //****************************PIDcontrol_compute_velocity************************************//
d15321854 0:4fecb14ffbb8 812 //****************************PIDcontrol_compute_velocity************************************//
d15321854 0:4fecb14ffbb8 813 void PIDcontrol_compute_velocity(void){
d15321854 0:4fecb14ffbb8 814 if(command_AngularVel_1 >= 0){
d15321854 0:4fecb14ffbb8 815 cw_ccw_1 = 0;
d15321854 0:4fecb14ffbb8 816 Command_AngularVel_1 = command_AngularVel_1;
d15321854 0:4fecb14ffbb8 817 }else{
d15321854 0:4fecb14ffbb8 818 cw_ccw_1 = 1;
d15321854 0:4fecb14ffbb8 819 Command_AngularVel_1 = -command_AngularVel_1;
d15321854 0:4fecb14ffbb8 820 }
d15321854 0:4fecb14ffbb8 821 if(command_AngularVel_2 >= 0){
d15321854 0:4fecb14ffbb8 822 cw_ccw_2 = 0;
d15321854 0:4fecb14ffbb8 823 Command_AngularVel_2 = command_AngularVel_2;
d15321854 0:4fecb14ffbb8 824 }else{
d15321854 0:4fecb14ffbb8 825 cw_ccw_2 = 1;
d15321854 0:4fecb14ffbb8 826 Command_AngularVel_2 = -command_AngularVel_2;
d15321854 0:4fecb14ffbb8 827 }
d15321854 0:4fecb14ffbb8 828 if(command_AngularVel_3 >= 0){
d15321854 0:4fecb14ffbb8 829 cw_ccw_3 = 0;
d15321854 0:4fecb14ffbb8 830 Command_AngularVel_3 = command_AngularVel_3;
d15321854 0:4fecb14ffbb8 831 }else{
d15321854 0:4fecb14ffbb8 832 cw_ccw_3 = 1;
d15321854 0:4fecb14ffbb8 833 Command_AngularVel_3 = -command_AngularVel_3;
d15321854 0:4fecb14ffbb8 834 }
d15321854 0:4fecb14ffbb8 835 Now_time_PID=NowTime.read();
d15321854 0:4fecb14ffbb8 836 Motor_1.Compute(&Now_time_PID);
d15321854 0:4fecb14ffbb8 837 Motor_2.Compute(&Now_time_PID);
d15321854 0:4fecb14ffbb8 838 Motor_3.Compute(&Now_time_PID);
d15321854 0:4fecb14ffbb8 839 Control_Motor_PWM_1 = PWM_commmand_transformation(Control_motor_1);
d15321854 0:4fecb14ffbb8 840 Control_Motor_PWM_2 = PWM_commmand_transformation(Control_motor_2);
d15321854 0:4fecb14ffbb8 841 Control_Motor_PWM_3 = PWM_commmand_transformation(Control_motor_3);
d15321854 0:4fecb14ffbb8 842 //Control_Motor_PWM_1 = PWM_commmand_transformation(Command_AngularVel_1);
d15321854 0:4fecb14ffbb8 843 //Control_Motor_PWM_2 = PWM_commmand_transformation(Command_AngularVel_2);
d15321854 0:4fecb14ffbb8 844 //Control_Motor_PWM_3 = PWM_commmand_transformation(Command_AngularVel_3);
d15321854 0:4fecb14ffbb8 845 //printf("Command_AngularVel_2: %f\n",Command_AngularVel_2);
d15321854 0:4fecb14ffbb8 846 //printf("Control_motor_2: %f\n",Control_motor_2);
d15321854 0:4fecb14ffbb8 847
d15321854 0:4fecb14ffbb8 848 }
d15321854 0:4fecb14ffbb8 849 //****************************Compute_point************************************//
d15321854 0:4fecb14ffbb8 850 //****************************Compute_point************************************//
d15321854 0:4fecb14ffbb8 851 //****************************Compute_point************************************//
d15321854 0:4fecb14ffbb8 852 //****************************Compute_point************************************//
d15321854 0:4fecb14ffbb8 853 //****************************Compute_point************************************//
d15321854 0:4fecb14ffbb8 854 void LQR_control_compute(void){
d15321854 0:4fecb14ffbb8 855 //Now_point_x = (angle/57.295)*3;
d15321854 0:4fecb14ffbb8 856 //Now_point_x = Angle_1;
d15321854 0:4fecb14ffbb8 857 /*if( Roll > 0.5 ){
d15321854 0:4fecb14ffbb8 858 d_x = Roll*0.01745*radius_ball;
d15321854 0:4fecb14ffbb8 859 Vx = d_x/0.001;
d15321854 0:4fecb14ffbb8 860 }else{
d15321854 0:4fecb14ffbb8 861 Vx = 0;
d15321854 0:4fecb14ffbb8 862 }
d15321854 0:4fecb14ffbb8 863 if(Pitch > 0.5){
d15321854 0:4fecb14ffbb8 864 d_y = Pitch*0.01745*radius_ball;
d15321854 0:4fecb14ffbb8 865 Vy = d_y/0.001;
d15321854 0:4fecb14ffbb8 866 }else{
d15321854 0:4fecb14ffbb8 867 Vy = 0;
d15321854 0:4fecb14ffbb8 868 }
d15321854 0:4fecb14ffbb8 869 if( Yaw > 0 ){
d15321854 0:4fecb14ffbb8 870 Wz = 0;
d15321854 0:4fecb14ffbb8 871 }else{
d15321854 0:4fecb14ffbb8 872 Wz = 0;
d15321854 0:4fecb14ffbb8 873 }*/
d15321854 0:4fecb14ffbb8 874 Diff_Roll = (Roll - Roll_last);
d15321854 0:4fecb14ffbb8 875 Diff_Pitch = (Pitch - Pitch_last);
d15321854 0:4fecb14ffbb8 876 Integ_Roll += Roll;
d15321854 0:4fecb14ffbb8 877 //printf("Diff_Roll:%.3f\n",Diff_Roll);
d15321854 0:4fecb14ffbb8 878 Integ_Pitch += Pitch;
d15321854 0:4fecb14ffbb8 879 Roll_last = Roll;
d15321854 0:4fecb14ffbb8 880 Pitch_last = Pitch;
d15321854 0:4fecb14ffbb8 881
d15321854 0:4fecb14ffbb8 882 //Diff_Roll =0;
d15321854 0:4fecb14ffbb8 883 //Roll -= 2.45;
d15321854 0:4fecb14ffbb8 884 //Pitch -=2.5;
d15321854 0:4fecb14ffbb8 885 Diff_x = x_now - x_trajectory;
d15321854 0:4fecb14ffbb8 886 Diff_y = y_now - y_trajectory;
d15321854 0:4fecb14ffbb8 887 dot_diff_x = Diff_x - Diff_x_pre;
d15321854 0:4fecb14ffbb8 888 dot_diff_y = Diff_y - Diff_y_pre;
d15321854 0:4fecb14ffbb8 889 Integ_x += Diff_x;
d15321854 0:4fecb14ffbb8 890 Integ_y += Diff_y;
d15321854 0:4fecb14ffbb8 891
d15321854 0:4fecb14ffbb8 892 //x_pre_1 = x_now;
d15321854 0:4fecb14ffbb8 893 //y_pre_1 = y_now;
d15321854 0:4fecb14ffbb8 894 Diff_x_pre = Diff_x;
d15321854 0:4fecb14ffbb8 895 Diff_y_pre = Diff_y;
d15321854 0:4fecb14ffbb8 896
d15321854 0:4fecb14ffbb8 897 u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x;
d15321854 0:4fecb14ffbb8 898 //u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll - Kt*(10) - Kv*0;
d15321854 0:4fecb14ffbb8 899 u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y;
d15321854 0:4fecb14ffbb8 900 //u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch - Kt*0 - Kv*0;
d15321854 0:4fecb14ffbb8 901 if(u_x > 100 ){u_x = 100;}else if(u_x < -100){u_x = -100;}
d15321854 0:4fecb14ffbb8 902 if(u_y > 100 ){u_y = 100;}else if(u_y < -100){u_y = -100;}
d15321854 0:4fecb14ffbb8 903 Vx = u_x;
d15321854 0:4fecb14ffbb8 904 Vy = u_y;
d15321854 0:4fecb14ffbb8 905 Wz = -Yaw;
d15321854 0:4fecb14ffbb8 906
d15321854 0:4fecb14ffbb8 907 command_AngularVel_1 = (1/radius_ball)*(-Vy*(-0.5)+Kz*Wz);
d15321854 0:4fecb14ffbb8 908 command_AngularVel_2 = (1/radius_ball)*((0.866*Vx+0.5*Vy)*(-0.5)+Kz*Wz);
d15321854 0:4fecb14ffbb8 909 command_AngularVel_3 = (1/radius_ball)*((-0.866*Vx+0.5*Vy)*(-0.5)+Kz*Wz);
d15321854 0:4fecb14ffbb8 910 //printf("Command_AngularVel_2: %f\n",Command_AngularVel_2);
d15321854 0:4fecb14ffbb8 911 //printf("Command_AngularVel_3: %f\n",Command_AngularVel_3);
d15321854 0:4fecb14ffbb8 912
d15321854 0:4fecb14ffbb8 913 /*ax_now = Vx - Vx_pre_1;
d15321854 0:4fecb14ffbb8 914 ay_now = Vy - Vy_pre_1;
d15321854 0:4fecb14ffbb8 915 ax_pre_2 = ax_pre_1;
d15321854 0:4fecb14ffbb8 916 ax_pre_1 = ax_now;
d15321854 0:4fecb14ffbb8 917 ay_pre_2 = ay_pre_1;
d15321854 0:4fecb14ffbb8 918 ay_pre_1 = ay_now;
d15321854 0:4fecb14ffbb8 919 Vx_pre_1 = Vx;
d15321854 0:4fecb14ffbb8 920 Vy_pre_1 = Vy;*/
d15321854 0:4fecb14ffbb8 921
d15321854 0:4fecb14ffbb8 922 }
d15321854 0:4fecb14ffbb8 923 void MeasureRobotAttitudeAngle(void){
d15321854 0:4fecb14ffbb8 924 dt = t_MeasureRobotAttitudeAngle;
d15321854 0:4fecb14ffbb8 925 imu.read_all();
d15321854 0:4fecb14ffbb8 926 Mag_Complentary_Filter(dt,imu.Magnetometer);
d15321854 0:4fecb14ffbb8 927 Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
d15321854 0:4fecb14ffbb8 928 /*x_now = x_pre_1 + (1/(dt*dt))*(ax_now-2*ax_pre_1+ax_pre_2);
d15321854 0:4fecb14ffbb8 929 y_now = y_pre_1 + (1/(dt*dt))*(ay_now-2*ay_pre_1+ay_pre_2);
d15321854 0:4fecb14ffbb8 930 x_pre_1 = x_now;
d15321854 0:4fecb14ffbb8 931 y_pre_1 = y_now;*/
d15321854 0:4fecb14ffbb8 932 x_now = -((2*r_wheel)/3)*(-1.732*(-Angle_2+Angle_3));
d15321854 0:4fecb14ffbb8 933 y_now = ((2*r_wheel)/3)*((-2*Angle_1+Angle_2+Angle_3)/(-1));
d15321854 0:4fecb14ffbb8 934 do_measure_index++;
d15321854 0:4fecb14ffbb8 935 }
d15321854 0:4fecb14ffbb8 936 void Trajectory_generator(void){
d15321854 0:4fecb14ffbb8 937 double t_trajectory = NowTime.read();
d15321854 0:4fecb14ffbb8 938
d15321854 0:4fecb14ffbb8 939 if( RunTime>=0 &&( RunTime<=10) ){
d15321854 0:4fecb14ffbb8 940 //x_trajectory = t_trajectory * (0.5);
d15321854 0:4fecb14ffbb8 941 x_trajectory = 0;
d15321854 0:4fecb14ffbb8 942 y_trajectory = 0;
d15321854 0:4fecb14ffbb8 943 Arm_enable_index = 0;
d15321854 0:4fecb14ffbb8 944 }else if( RunTime>=10 ){
d15321854 0:4fecb14ffbb8 945 x_trajectory = 0;
d15321854 0:4fecb14ffbb8 946 y_trajectory = 0;
d15321854 0:4fecb14ffbb8 947 Arm_enable_index = 1;
d15321854 0:4fecb14ffbb8 948 }
d15321854 0:4fecb14ffbb8 949 }
d15321854 0:4fecb14ffbb8 950 void Check_Arm_interrupt( double x_Now, double y_Now, double X_trajectory, double Y_trajectory){
d15321854 0:4fecb14ffbb8 951 if( sqrt((x_Now-X_trajectory)*(x_Now-X_trajectory)+(y_Now-Y_trajectory)*(y_Now-Y_trajectory)) < 5 ){
d15321854 0:4fecb14ffbb8 952 if( Arm_enable_index == 1 ){
d15321854 0:4fecb14ffbb8 953 Arm_interrupt = 1;
d15321854 0:4fecb14ffbb8 954 //Roll_offset = 2.0;
d15321854 0:4fecb14ffbb8 955 //Pitch_offset = 2.5;
d15321854 0:4fecb14ffbb8 956 }
d15321854 0:4fecb14ffbb8 957 }
d15321854 0:4fecb14ffbb8 958 }
d15321854 0:4fecb14ffbb8 959 //****************************main function************************************//
d15321854 0:4fecb14ffbb8 960 int main() {
d15321854 0:4fecb14ffbb8 961 Serial pc( USBTX, USBRX );
d15321854 0:4fecb14ffbb8 962 pc.baud(460800);
d15321854 0:4fecb14ffbb8 963 //****************************Angle Sensor initialization************************************//
d15321854 0:4fecb14ffbb8 964 if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250
d15321854 0:4fecb14ffbb8 965 //pc.printf("\nCouldn't initialize MPU9250 via SPI!");
d15321854 0:4fecb14ffbb8 966 }
d15321854 0:4fecb14ffbb8 967 //pc.printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
d15321854 0:4fecb14ffbb8 968 imu.whoami();
d15321854 0:4fecb14ffbb8 969 //wait(1);
d15321854 0:4fecb14ffbb8 970 //pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
d15321854 0:4fecb14ffbb8 971 imu.set_gyro_scale(BITS_FS_2000DPS);
d15321854 0:4fecb14ffbb8 972 //wait(1);
d15321854 0:4fecb14ffbb8 973 //pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
d15321854 0:4fecb14ffbb8 974 imu.set_acc_scale(BITS_FS_16G);
d15321854 0:4fecb14ffbb8 975 //wait(1);
d15321854 0:4fecb14ffbb8 976 //pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
d15321854 0:4fecb14ffbb8 977 imu.AK8963_whoami();
d15321854 0:4fecb14ffbb8 978 //wait(0.1);
d15321854 0:4fecb14ffbb8 979 imu.AK8963_calib_Magnetometer();
d15321854 0:4fecb14ffbb8 980 //****************************Motor driver declare************************************//
d15321854 0:4fecb14ffbb8 981 //ExtInt = 0; //number high, voltage high.
d15321854 0:4fecb14ffbb8 982 AR_1 = 1;
d15321854 0:4fecb14ffbb8 983 AR_2 = 1;
d15321854 0:4fecb14ffbb8 984 AR_3 = 1;
d15321854 0:4fecb14ffbb8 985 control_Brake = 1; //1:Run 0:Stop
d15321854 0:4fecb14ffbb8 986 //**************************** PWM ************************************//
d15321854 0:4fecb14ffbb8 987 PWM_Motor_1.calibrate(0.02, 0*0.02, 1*0.02);
d15321854 0:4fecb14ffbb8 988 PWM_Motor_2.calibrate(0.02, 0*0.02, 1*0.02);
d15321854 0:4fecb14ffbb8 989 PWM_Motor_3.calibrate(0.02, 0*0.02, 1*0.02);
d15321854 0:4fecb14ffbb8 990 //**************************** Encoder ************************************//
d15321854 0:4fecb14ffbb8 991 phaseA_1.mode( PullUp );
d15321854 0:4fecb14ffbb8 992 phaseB_1.mode( PullUp );
d15321854 0:4fecb14ffbb8 993 phaseA_2.mode( PullUp );
d15321854 0:4fecb14ffbb8 994 phaseB_2.mode( PullUp );
d15321854 0:4fecb14ffbb8 995 phaseA_3.mode( PullUp );
d15321854 0:4fecb14ffbb8 996 phaseB_3.mode( PullUp );
d15321854 0:4fecb14ffbb8 997 //**************************** Ticker ************************************//
d15321854 0:4fecb14ffbb8 998 Ticker Sample_Motor_encoder_1; // create a timer to sample the encoder.
d15321854 0:4fecb14ffbb8 999 Ticker Sample_Motor_encoder_2; // create a timer to sample the encoder.
d15321854 0:4fecb14ffbb8 1000 Ticker Sample_Motor_encoder_3; // create a timer to sample the encoder.
d15321854 0:4fecb14ffbb8 1001 Ticker Sample_robotAngleSonsor; // create a timer to sample the robot attitude.
d15321854 0:4fecb14ffbb8 1002 Ticker MeasureAngularVelocity; // create a timer to measure the motor angular velocity.
d15321854 0:4fecb14ffbb8 1003 Ticker PIDcontrol_velocity; // create a timer to do the motor angular velocity PID control.
d15321854 0:4fecb14ffbb8 1004 Ticker LQR_control; // create a timer to do the position control.
d15321854 0:4fecb14ffbb8 1005 Ticker TrajectoryTracking_control; // create a timer to do the TrajectoryTracking_control.
d15321854 0:4fecb14ffbb8 1006
d15321854 0:4fecb14ffbb8 1007 //**************************** Create the motor encoder sampler. ************************************//
d15321854 0:4fecb14ffbb8 1008 Sample_Motor_encoder_1.attach_us( &quadratureDecoder_1, t_quadratureDecoder );
d15321854 0:4fecb14ffbb8 1009 Sample_Motor_encoder_2.attach_us( &quadratureDecoder_2, t_quadratureDecoder );
d15321854 0:4fecb14ffbb8 1010 Sample_Motor_encoder_3.attach_us( &quadratureDecoder_3, t_quadratureDecoder );
d15321854 0:4fecb14ffbb8 1011 //**************************** Create the motor encoder sampler. ************************************//
d15321854 0:4fecb14ffbb8 1012 Sample_robotAngleSonsor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle );
d15321854 0:4fecb14ffbb8 1013 //**************************** Create the motor angular velocity measurement. ************************************//
d15321854 0:4fecb14ffbb8 1014 MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity );
d15321854 0:4fecb14ffbb8 1015 //**************************** Create the motor angular velocity PID control. ************************************//
d15321854 0:4fecb14ffbb8 1016 PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity );
d15321854 0:4fecb14ffbb8 1017 //**************************** Create the robot LQR control. ************************************//
d15321854 0:4fecb14ffbb8 1018 LQR_control.attach( &LQR_control_compute, t_LQR_control );
d15321854 0:4fecb14ffbb8 1019 //**************************** Create the motor posistion control. ************************************//
d15321854 0:4fecb14ffbb8 1020 TrajectoryTracking_control.attach( &Trajectory_generator, t_TrajectoryTracking_control );
d15321854 0:4fecb14ffbb8 1021 //**************************** Motor settlement ************************************//
d15321854 0:4fecb14ffbb8 1022 Motor_1.SetMode(AUTOMATIC);
d15321854 0:4fecb14ffbb8 1023 Motor_2.SetMode(AUTOMATIC);
d15321854 0:4fecb14ffbb8 1024 Motor_3.SetMode(AUTOMATIC);
d15321854 0:4fecb14ffbb8 1025
d15321854 0:4fecb14ffbb8 1026
d15321854 0:4fecb14ffbb8 1027 Command_AngularVel_1 = 0;
d15321854 0:4fecb14ffbb8 1028 Command_AngularVel_2 = 0;
d15321854 0:4fecb14ffbb8 1029 Command_AngularVel_3 = 0;
d15321854 0:4fecb14ffbb8 1030 //**************************** Timers start ************************************//
d15321854 0:4fecb14ffbb8 1031 NowTime.start();
d15321854 0:4fecb14ffbb8 1032
d15321854 0:4fecb14ffbb8 1033 while( 1 ){
d15321854 0:4fecb14ffbb8 1034
d15321854 0:4fecb14ffbb8 1035 RunTime = NowTime.read();
d15321854 0:4fecb14ffbb8 1036
d15321854 0:4fecb14ffbb8 1037 if( (RunTime-lastTime) > 0.1 ){
d15321854 0:4fecb14ffbb8 1038 index_times++;
d15321854 0:4fecb14ffbb8 1039 lastTime = RunTime;
d15321854 0:4fecb14ffbb8 1040 }
d15321854 0:4fecb14ffbb8 1041
d15321854 0:4fecb14ffbb8 1042 if( index_times >= 0.1 ){
d15321854 0:4fecb14ffbb8 1043 /* pc.printf(" Now_angularVelocity_1 : %.3f ",Now_angularVelocity_1);
d15321854 0:4fecb14ffbb8 1044 pc.printf(" Now_angularVelocity_2 : %.3f ",Now_angularVelocity_2);
d15321854 0:4fecb14ffbb8 1045 pc.printf(" Now_angularVelocity_3 : %.3f\n ",Now_angularVelocity_3);
d15321854 0:4fecb14ffbb8 1046 pc.printf(" Vx : %.3f ",Vx);
d15321854 0:4fecb14ffbb8 1047 pc.printf(" Vy : %.3f ",Vy);
d15321854 0:4fecb14ffbb8 1048 //pc.printf(", Vy : %.3f ",Vy);
d15321854 0:4fecb14ffbb8 1049 pc.printf(", Command_AngularVel_1 : %.3f ",Command_AngularVel_1);
d15321854 0:4fecb14ffbb8 1050 pc.printf(", Command_AngularVel_2 : %.3f ",Command_AngularVel_2);
d15321854 0:4fecb14ffbb8 1051 pc.printf(", Command_AngularVel_3 : %.3f \n",Command_AngularVel_3);*/
d15321854 0:4fecb14ffbb8 1052
d15321854 0:4fecb14ffbb8 1053 /*pc.printf("x_now: %.3f , y_now: %.3f , x_trajectory: %f , u_y: %f ",x_now,y_now,x_trajectory,u_y);
d15321854 0:4fecb14ffbb8 1054 pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f \n",
d15321854 0:4fecb14ffbb8 1055 Roll,
d15321854 0:4fecb14ffbb8 1056 Pitch,
d15321854 0:4fecb14ffbb8 1057 Yaw
d15321854 0:4fecb14ffbb8 1058 );*/
d15321854 0:4fecb14ffbb8 1059 pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory);
d15321854 0:4fecb14ffbb8 1060 pc.printf(", %10.3f, %10.3f, %10.3f , %10.3f \n",
d15321854 0:4fecb14ffbb8 1061 Roll,
d15321854 0:4fecb14ffbb8 1062 Pitch,
d15321854 0:4fecb14ffbb8 1063 Yaw,
d15321854 0:4fecb14ffbb8 1064 RunTime
d15321854 0:4fecb14ffbb8 1065 );
d15321854 0:4fecb14ffbb8 1066 //pc.printf(",Control_motor_1 : %.3f ",Control_motor_1);
d15321854 0:4fecb14ffbb8 1067 //pc.printf(", %.3f ", Command_gularVel_1);
d15321854 0:4fecb14ffbb8 1068
d15321854 0:4fecb14ffbb8 1069 /*pc.printf(", %.3f ",Control_motor_1);*/
d15321854 0:4fecb14ffbb8 1070 //pc.printf(", %.3f\n ",Now_angularVelocity_1);
d15321854 0:4fecb14ffbb8 1071 index_times = 0;
d15321854 0:4fecb14ffbb8 1072 }
d15321854 0:4fecb14ffbb8 1073 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);
d15321854 0:4fecb14ffbb8 1074
d15321854 0:4fecb14ffbb8 1075 Check_Arm_interrupt(x_now,y_now,x_trajectory,y_trajectory);
d15321854 0:4fecb14ffbb8 1076
d15321854 0:4fecb14ffbb8 1077 //wait(0.1);
d15321854 0:4fecb14ffbb8 1078 }
d15321854 0:4fecb14ffbb8 1079 }