Amber Tang / Mbed 2 deprecated testSSWMR_StationKeeping_OK

20170830

Dependencies:   MPU9250_SPI PID01 SDFileSystem00 Servo mbed

main.cpp@0:54f408fdeada, 2017-08-30 (annotated)

Committer:
Amber77
Date:
Wed Aug 30 02:07:22 2017 +0000
Revision:
0:54f408fdeada
20170830

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Amber77 0:54f408fdeada 1 #include "mbed.h"
Amber77 0:54f408fdeada 2 #include "PID.h"
Amber77 0:54f408fdeada 3 #include "Servo.h"
Amber77 0:54f408fdeada 4 #include "MPU9250.h"
Amber77 0:54f408fdeada 5 #include "SDFileSystem.h"
Amber77 0:54f408fdeada 6 /* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
Amber77 0:54f408fdeada 7 /* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
Amber77 0:54f408fdeada 8 // SDFileSystem---1. https://developer.mbed.org/users/mbed_official/code/SDFileSystem/
Amber77 0:54f408fdeada 9 // 2. https://developer.mbed.org/users/simon/code/SDCardTest/
Amber77 0:54f408fdeada 10 // SD care---3.3v
Amber77 0:54f408fdeada 11 // Ctrl+? =>註解
Amber77 0:54f408fdeada 12 #define pi 3.14159265359
Amber77 0:54f408fdeada 13 //**************************** PID control para declaration ************************************//
Amber77 0:54f408fdeada 14 #define RATE 0.005
Amber77 0:54f408fdeada 15 #define Kp_1 1.1 //1.1
Amber77 0:54f408fdeada 16 #define Ki_1 0.18 //0.2
Amber77 0:54f408fdeada 17 #define Kd_1 0.001
Amber77 0:54f408fdeada 18 //**************************** Angle Sonsor para declaration ************************************//
Amber77 0:54f408fdeada 19 #define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer
Amber77 0:54f408fdeada 20 #define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases
Amber77 0:54f408fdeada 21 //**************************** pragram debug declaration ************************************//
Amber77 0:54f408fdeada 22 int index_times = 0;
Amber77 0:54f408fdeada 23 double RunTime =0,lastTime =0;
Amber77 0:54f408fdeada 24 double t_trajectory=0;
Amber77 0:54f408fdeada 25 int do_measure_index=0;
Amber77 0:54f408fdeada 26
Amber77 0:54f408fdeada 27 double t_MeasureAngularVelocity=0.03;
Amber77 0:54f408fdeada 28 double t_PIDcontrol_velocity =0.03;
Amber77 0:54f408fdeada 29 double t_LQR_control = 0.05;
Amber77 0:54f408fdeada 30 double t_MeasureRobotAttitudeAngle = 0.05;
Amber77 0:54f408fdeada 31 double t_quadratureDecoder = 15;
Amber77 0:54f408fdeada 32 //double t_TrajectoryTracking = 0.1;
Amber77 0:54f408fdeada 33 //**************************** Motor para declaration ************************************//
Amber77 0:54f408fdeada 34 double r_ball = 0.1; // 球的半徑
Amber77 0:54f408fdeada 35 double r_grounder = 0.018; //滾球的半徑
Amber77 0:54f408fdeada 36 //**************************** Ticker ************************************//
Amber77 0:54f408fdeada 37 Serial pc(USBTX, USBRX);
Amber77 0:54f408fdeada 38 Ticker Sample_Motor_encoder; // create a timer to sample the encoder.
Amber77 0:54f408fdeada 39 Ticker Sample_robotAngleSensor; // create a timer to sample the robot attitude.
Amber77 0:54f408fdeada 40 Ticker MeasureAngularVelocity; // create a timer to measure the motor angular velocity.
Amber77 0:54f408fdeada 41 Ticker PIDcontrol_velocity; // create a timer to do the motor angular velocity PID control.
Amber77 0:54f408fdeada 42 Ticker LQR_control; // create a timer to do the position control.
Amber77 0:54f408fdeada 43 //Ticker TrajectoryTracking_control;
Amber77 0:54f408fdeada 44 //**************************** Measuremant of angular velocity declaration ************************************//
Amber77 0:54f408fdeada 45 double Angle_1 =0,Angle_2 =0,Angle_3 =0,Angle_4 =0;
Amber77 0:54f408fdeada 46 double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0,LastAngle_4 =0;
Amber77 0:54f408fdeada 47 double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4;
Amber77 0:54f408fdeada 48 double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4;
Amber77 0:54f408fdeada 49 double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4;
Amber77 0:54f408fdeada 50 double _4_SectionAngle=0,_4_LastSectionAngle,_4_LastSectionAngle_1,_4_LastSectionAngle_2,_4_LastSectionAngle_3,_4_LastSectionAngle_4;
Amber77 0:54f408fdeada 51 double Average_SectionAngle_1,Now_angularVelocity_1=0;
Amber77 0:54f408fdeada 52 double Average_SectionAngle_2,Now_angularVelocity_2=0;
Amber77 0:54f408fdeada 53 double Average_SectionAngle_3,Now_angularVelocity_3=0;
Amber77 0:54f408fdeada 54 double Average_SectionAngle_4,Now_angularVelocity_4=0;
Amber77 0:54f408fdeada 55 double Now_angularVelocity_X=0,Now_angularVelocity_Y=0;
Amber77 0:54f408fdeada 56 double Angle_X=0, Angle_Y=0;
Amber77 0:54f408fdeada 57 double SectionTime =0;
Amber77 0:54f408fdeada 58 double NowTime_measureVelocity=0, LastTime_measureVelocity = 0;
Amber77 0:54f408fdeada 59 //**************************** PID control declaration ************************************//
Amber77 0:54f408fdeada 60 float Now_time_PID,Last_Time_PID;
Amber77 0:54f408fdeada 61 //Command
Amber77 0:54f408fdeada 62 //double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1,Command_AngularVel_4=1;
Amber77 0:54f408fdeada 63 //double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1,command_AngularVel_4=1;
Amber77 0:54f408fdeada 64 double Command_AngularVel_X=1,Command_AngularVel_Y=1;
Amber77 0:54f408fdeada 65 double command_AngularVel_X=1,command_AngularVel_Y=1;
Amber77 0:54f408fdeada 66 //Control
Amber77 0:54f408fdeada 67 double Control_motor_X,Control_motor_Y;
Amber77 0:54f408fdeada 68 //Control PWM value
Amber77 0:54f408fdeada 69 double Control_Motor_PWM_X,Control_Motor_PWM_Y;
Amber77 0:54f408fdeada 70
Amber77 0:54f408fdeada 71
Amber77 0:54f408fdeada 72 PID Motor_X (&Now_angularVelocity_X, &Control_motor_X, &Command_AngularVel_X, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
Amber77 0:54f408fdeada 73 PID Motor_Y (&Now_angularVelocity_Y, &Control_motor_Y, &Command_AngularVel_Y, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
Amber77 0:54f408fdeada 74 //PID Motor_X (&Command_AngularVel_X, &Control_motor_X, &Now_angularVelocity_X, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
Amber77 0:54f408fdeada 75 //PID Motor_Y (&Command_AngularVel_Y, &Control_motor_Y, &Now_angularVelocity_Y, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
Amber77 0:54f408fdeada 76
Amber77 0:54f408fdeada 77 //****************************LQR_control declaration************************************//
Amber77 0:54f408fdeada 78 double Ka = 60; //16.9573 25.0
Amber77 0:54f408fdeada 79 double Kav = 10; //10.4423 10.7423
Amber77 0:54f408fdeada 80
Amber77 0:54f408fdeada 81 double Kt = 4.0 ; //3.0 3.1
Amber77 0:54f408fdeada 82 double Kt_y = 4.0; //4.0 3.98
Amber77 0:54f408fdeada 83
Amber77 0:54f408fdeada 84 double Kv = 1.0; //1.0
Amber77 0:54f408fdeada 85 double Kv_y = 1.1; //1.1
Amber77 0:54f408fdeada 86
Amber77 0:54f408fdeada 87 double Kz = 0.05; //0.05
Amber77 0:54f408fdeada 88 double Kii = 0.2; //0.2
Amber77 0:54f408fdeada 89
Amber77 0:54f408fdeada 90 double KI_xy = 0.9; //0.02
Amber77 0:54f408fdeada 91 double KI_xy_y = 0.9; //0.025
Amber77 0:54f408fdeada 92
Amber77 0:54f408fdeada 93 double Roll_offset = -5; //2.88 4.0 3.5
Amber77 0:54f408fdeada 94 double Pitch_offset = -4; //-0.05 2.1 2.1
Amber77 0:54f408fdeada 95 double Diff_Roll,Diff_Pitch,Diff_Yaw;
Amber77 0:54f408fdeada 96 double Integ_Roll,Integ_Pitch;
Amber77 0:54f408fdeada 97 double Integ_x,Integ_y;
Amber77 0:54f408fdeada 98 double Roll_last,Pitch_last;
Amber77 0:54f408fdeada 99 double Vx=0,Vy=0,Wz=0;
Amber77 0:54f408fdeada 100 double d_x=0,d_y=0;
Amber77 0:54f408fdeada 101 double u_x=0,u_y=0;
Amber77 0:54f408fdeada 102 double Point_x;
Amber77 0:54f408fdeada 103 double x_now,y_now;
Amber77 0:54f408fdeada 104 double x_pre_1,y_pre_1;
Amber77 0:54f408fdeada 105 double ax_now,ay_now;
Amber77 0:54f408fdeada 106 double ax_pre_1,ay_pre_1;
Amber77 0:54f408fdeada 107 double ax_pre_2,ay_pre_2;
Amber77 0:54f408fdeada 108 double Vx_pre_1,Vy_pre_1;
Amber77 0:54f408fdeada 109 double Diff_x,Diff_y;
Amber77 0:54f408fdeada 110 double Diff_x_pre,Diff_y_pre;
Amber77 0:54f408fdeada 111 double dot_diff_x,dot_diff_y;
Amber77 0:54f408fdeada 112 //****************************Trajectory Tracking declaration************************************//
Amber77 0:54f408fdeada 113 double x_trajectory=0,y_trajectory=0;
Amber77 0:54f408fdeada 114 //****************************Encoder declaration************************************//
Amber77 0:54f408fdeada 115 // phase a of the quadrature encoder
Amber77 0:54f408fdeada 116 // phase b of the quadrature encoder
Amber77 0:54f408fdeada 117 DigitalIn phaseA_1( PA_0 );
Amber77 0:54f408fdeada 118 DigitalIn phaseB_1( PA_1 );
Amber77 0:54f408fdeada 119 DigitalIn phaseA_2( PA_8 );
Amber77 0:54f408fdeada 120 DigitalIn phaseB_2( PA_9 );
Amber77 0:54f408fdeada 121 DigitalIn phaseA_3( PC_6 );
Amber77 0:54f408fdeada 122 DigitalIn phaseB_3( PC_7 );
Amber77 0:54f408fdeada 123 DigitalIn phaseA_4( PB_8 );
Amber77 0:54f408fdeada 124 DigitalIn phaseB_4( PB_9 );
Amber77 0:54f408fdeada 125 // hold the signed value corresponding to the number of clicks left or right since last sample
Amber77 0:54f408fdeada 126 // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
Amber77 0:54f408fdeada 127 int encoderClickCount_1 = 0;
Amber77 0:54f408fdeada 128 int previousEncoderState_1 = 0;
Amber77 0:54f408fdeada 129 int encoderClickCount_2 = 0;
Amber77 0:54f408fdeada 130 int previousEncoderState_2 = 0;
Amber77 0:54f408fdeada 131 int encoderClickCount_3 = 0;
Amber77 0:54f408fdeada 132 int previousEncoderState_3 = 0;
Amber77 0:54f408fdeada 133 int encoderClickCount_4 = 0;
Amber77 0:54f408fdeada 134 int previousEncoderState_4 = 0;
Amber77 0:54f408fdeada 135 //****************************Angle Sensor declaration************************************//
Amber77 0:54f408fdeada 136 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
Amber77 0:54f408fdeada 137 float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz
Amber77 0:54f408fdeada 138 int counter = 0;
Amber77 0:54f408fdeada 139 int divider = 20;
Amber77 0:54f408fdeada 140 float dt; // time for entire loop
Amber77 0:54f408fdeada 141 float dt_sensors; // time only to read sensors
Amber77 0:54f408fdeada 142 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
Amber77 0:54f408fdeada 143 float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
Amber77 0:54f408fdeada 144 float exInt = 0, eyInt = 0, ezInt = 0;
Amber77 0:54f408fdeada 145 float OX = 0, OY = 0, OZ = 0;
Amber77 0:54f408fdeada 146 float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4;
Amber77 0:54f408fdeada 147 float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4;
Amber77 0:54f408fdeada 148 float Mag_x_pre,Mag_y_pre,Mag_z_pre;
Amber77 0:54f408fdeada 149 float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
Amber77 0:54f408fdeada 150 float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
Amber77 0:54f408fdeada 151 float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
Amber77 0:54f408fdeada 152 float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
Amber77 0:54f408fdeada 153 float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
Amber77 0:54f408fdeada 154 float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
Amber77 0:54f408fdeada 155 float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
Amber77 0:54f408fdeada 156 float Global_mag_vector_angle,Yaw_pre;
Amber77 0:54f408fdeada 157 float Global_mag_x_vector_angle,Mag_x_vector_angle;
Amber77 0:54f408fdeada 158 float Global_mag_y_vector_angle,Mag_y_vector_angle;
Amber77 0:54f408fdeada 159 int Count_mag_check=0;
Amber77 0:54f408fdeada 160 float angle[3];
Amber77 0:54f408fdeada 161 float Roll,Pitch,Yaw;
Amber77 0:54f408fdeada 162 float calibrated_values[3],magCalibrationp[3];
Amber77 0:54f408fdeada 163 float v_index[3];
Amber77 0:54f408fdeada 164 float dest1,dest2;
Amber77 0:54f408fdeada 165 int f=0;
Amber77 0:54f408fdeada 166 int j=0;
Amber77 0:54f408fdeada 167 int k=0;
Amber77 0:54f408fdeada 168 int g=0;
Amber77 0:54f408fdeada 169 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;
Amber77 0:54f408fdeada 170 int Rot_index;
Amber77 0:54f408fdeada 171 float mRes = 10.*4912./32760.0;
Amber77 0:54f408fdeada 172
Amber77 0:54f408fdeada 173 int AngleFilter_counter = 0;
Amber77 0:54f408fdeada 174 float Roll_total = 0,Pitch_total = 0;
Amber77 0:54f408fdeada 175 //****************************Motor & SD card & MPU9250 declare************************************//
Amber77 0:54f408fdeada 176 int control_AR=0, control_brake=0,control_stopRun=1, control_X1_CW_CCW=0, control_X2_CW_CCW=0, control_Y1_CW_CCW=0, control_Y2_CW_CCW=0;
Amber77 0:54f408fdeada 177 int Control_stopRun=1, Control_brake=0, Control_AR=0, Control_X1_CW_CCW=0, Control_X2_CW_CCW=0, Control_Y1_CW_CCW=0, Control_Y2_CW_CCW=0;
Amber77 0:54f408fdeada 178 int control_LiftingStopRun=1, control_F_R=1, control_H_F=1;
Amber77 0:54f408fdeada 179 int Control_LiftingStopRun=1, Control_F_R=1, Control_H_F=1;
Amber77 0:54f408fdeada 180 int X_pos=0, Y_pos=0;
Amber77 0:54f408fdeada 181 DigitalOut StopRun(PA_12);
Amber77 0:54f408fdeada 182 DigitalOut Brake(PA_11);
Amber77 0:54f408fdeada 183 DigitalOut AR(PB_12);
Amber77 0:54f408fdeada 184 DigitalOut X1_CW_CCW(PC_11);
Amber77 0:54f408fdeada 185 DigitalOut X2_CW_CCW(PC_10);
Amber77 0:54f408fdeada 186 DigitalOut Y1_CW_CCW(PC_12);
Amber77 0:54f408fdeada 187 DigitalOut Y2_CW_CCW(PD_2);
Amber77 0:54f408fdeada 188 Servo X_PWM(PB_1);
Amber77 0:54f408fdeada 189 Servo Y_PWM(PB_2);
Amber77 0:54f408fdeada 190 DigitalOut myled(LED1);
Amber77 0:54f408fdeada 191
Amber77 0:54f408fdeada 192 //---SD card---//
Amber77 0:54f408fdeada 193 SDFileSystem sd( D4, D5, D3, D6, "sd"); // mosi, miso, sclk, cs
Amber77 0:54f408fdeada 194 DigitalIn mybutton(USER_BUTTON);
Amber77 0:54f408fdeada 195 //---MPU 9250---//
Amber77 0:54f408fdeada 196 SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
Amber77 0:54f408fdeada 197 //SPI_MOSI = PA_7(D11), SPI_MISO = PA_6(D12), SPI_SCK = PA_5(D13), SPI_CS = PB_6(D10)
Amber77 0:54f408fdeada 198 mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object
Amber77 0:54f408fdeada 199 //****************************Lifting mechanism & limit switches declare************************************//
Amber77 0:54f408fdeada 200 DigitalOut LiftingStopRun(PA_15);
Amber77 0:54f408fdeada 201 DigitalOut F_R(PA_14); // CW/CCW
Amber77 0:54f408fdeada 202 DigitalOut H_F(PA_13);
Amber77 0:54f408fdeada 203 //DigitalIn TIM(PB_7);
Amber77 0:54f408fdeada 204 DigitalIn LimitSwitchUp(PB_15,PullUp);
Amber77 0:54f408fdeada 205 DigitalIn LimitSwitchDown(PB_14,PullUp);
Amber77 0:54f408fdeada 206 int limitSwitchUp;
Amber77 0:54f408fdeada 207 int limitSwitchDown;
Amber77 0:54f408fdeada 208 //****************************Timer declaration************************************//
Amber77 0:54f408fdeada 209 Timer NowTime;
Amber77 0:54f408fdeada 210 Timer LiftingDownTime;
Amber77 0:54f408fdeada 211 //---------------------------------------------------------------------------------//
Amber77 0:54f408fdeada 212
Amber77 0:54f408fdeada 213 //**************************** Filter_IMUupdate ************************************//
Amber77 0:54f408fdeada 214 //**************************** Filter_IMUupdate ************************************//
Amber77 0:54f408fdeada 215 void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az)
Amber77 0:54f408fdeada 216 {
Amber77 0:54f408fdeada 217 float norm;
Amber77 0:54f408fdeada 218 float vx, vy, vz;
Amber77 0:54f408fdeada 219 float ex, ey, ez;
Amber77 0:54f408fdeada 220
Amber77 0:54f408fdeada 221 // normalise the measurements
Amber77 0:54f408fdeada 222 norm = sqrt(ax*ax + ay*ay + az*az);
Amber77 0:54f408fdeada 223 if(norm == 0.0f) return;
Amber77 0:54f408fdeada 224 ax /= norm;
Amber77 0:54f408fdeada 225 ay /= norm;
Amber77 0:54f408fdeada 226 az /= norm;
Amber77 0:54f408fdeada 227
Amber77 0:54f408fdeada 228 // estimated direction of gravity
Amber77 0:54f408fdeada 229 vx = 2*(q1*q3 - q0*q2);
Amber77 0:54f408fdeada 230 vy = 2*(q0*q1 + q2*q3);
Amber77 0:54f408fdeada 231 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
Amber77 0:54f408fdeada 232
Amber77 0:54f408fdeada 233 // error is sum of cross product between reference direction of field and direction measured by sensor
Amber77 0:54f408fdeada 234 ex = (ay*vz - az*vy);
Amber77 0:54f408fdeada 235 ey = (az*vx - ax*vz);
Amber77 0:54f408fdeada 236 ez = (ax*vy - ay*vx);
Amber77 0:54f408fdeada 237
Amber77 0:54f408fdeada 238 // integral error scaled integral gain
Amber77 0:54f408fdeada 239 exInt += ex*Ki;
Amber77 0:54f408fdeada 240 eyInt += ey*Ki;
Amber77 0:54f408fdeada 241 ezInt += ez*Ki;
Amber77 0:54f408fdeada 242
Amber77 0:54f408fdeada 243 // adjusted gyroscope measurements
Amber77 0:54f408fdeada 244 gx += Kp*ex + exInt;
Amber77 0:54f408fdeada 245 gy += Kp*ey + eyInt;
Amber77 0:54f408fdeada 246 gz += Kp*ez + ezInt;
Amber77 0:54f408fdeada 247
Amber77 0:54f408fdeada 248 // integrate quaternion rate and normalise
Amber77 0:54f408fdeada 249 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!
Amber77 0:54f408fdeada 250 float q1o = q1;
Amber77 0:54f408fdeada 251 float q2o = q2;
Amber77 0:54f408fdeada 252 float q3o = q3;
Amber77 0:54f408fdeada 253 q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
Amber77 0:54f408fdeada 254 q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
Amber77 0:54f408fdeada 255 q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
Amber77 0:54f408fdeada 256 q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;
Amber77 0:54f408fdeada 257
Amber77 0:54f408fdeada 258 // normalise quaternion
Amber77 0:54f408fdeada 259 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
Amber77 0:54f408fdeada 260 q0 = q0 / norm;
Amber77 0:54f408fdeada 261 q1 = q1 / norm;
Amber77 0:54f408fdeada 262 q2 = q2 / norm;
Amber77 0:54f408fdeada 263 q3 = q3 / norm;
Amber77 0:54f408fdeada 264 }
Amber77 0:54f408fdeada 265
Amber77 0:54f408fdeada 266 //**************************** Filter_compute ************************************//
Amber77 0:54f408fdeada 267 //**************************** Filter_compute ************************************//
Amber77 0:54f408fdeada 268 void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
Amber77 0:54f408fdeada 269 {
Amber77 0:54f408fdeada 270 // IMU/AHRS
Amber77 0:54f408fdeada 271
Amber77 0:54f408fdeada 272 float d_Gyro_angle[3];
Amber77 0:54f408fdeada 273 void get_Acc_angle(const float * Acc_data);
Amber77 0:54f408fdeada 274 // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
Amber77 0:54f408fdeada 275 float radGyro[3],Gyro_cal_data; // Gyro in radians per second
Amber77 0:54f408fdeada 276
Amber77 0:54f408fdeada 277 for(int i=0; i<3; i++)
Amber77 0:54f408fdeada 278 {
Amber77 0:54f408fdeada 279 radGyro[i] = Gyro_data[i] * 3.14159/ 180;
Amber77 0:54f408fdeada 280 }
Amber77 0:54f408fdeada 281
Amber77 0:54f408fdeada 282 Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
Amber77 0:54f408fdeada 283 //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]);
Amber77 0:54f408fdeada 284
Amber77 0:54f408fdeada 285 float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
Amber77 0:54f408fdeada 286
Amber77 0:54f408fdeada 287 rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
Amber77 0:54f408fdeada 288 rangle[1] = asin (2*q0*q2 - 2*q3*q1);
Amber77 0:54f408fdeada 289 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
Amber77 0:54f408fdeada 290
Amber77 0:54f408fdeada 291
Amber77 0:54f408fdeada 292 for(int i=0; i<2; i++)
Amber77 0:54f408fdeada 293 { // angle in degree
Amber77 0:54f408fdeada 294 angle[i] = rangle[i] * 180 / 3.14159;
Amber77 0:54f408fdeada 295 }
Amber77 0:54f408fdeada 296 /*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;
Amber77 0:54f408fdeada 297 Roll_pre_4 = Roll_pre_3;
Amber77 0:54f408fdeada 298 Roll_pre_3 = Roll_pre_2;
Amber77 0:54f408fdeada 299 Roll_pre_2 = Roll_pre_1;
Amber77 0:54f408fdeada 300 Roll_pre_1 = Roll;
Amber77 0:54f408fdeada 301
Amber77 0:54f408fdeada 302 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;
Amber77 0:54f408fdeada 303 Pitch_pre_4 = Pitch_pre_3;
Amber77 0:54f408fdeada 304 Pitch_pre_3 = Pitch_pre_2;
Amber77 0:54f408fdeada 305 Pitch_pre_2 = Pitch_pre_1;
Amber77 0:54f408fdeada 306 Pitch_pre_1 = Pitch;*/
Amber77 0:54f408fdeada 307
Amber77 0:54f408fdeada 308 Roll_total += angle[0];
Amber77 0:54f408fdeada 309 Pitch_total += angle[1];
Amber77 0:54f408fdeada 310 AngleFilter_counter++;
Amber77 0:54f408fdeada 311
Amber77 0:54f408fdeada 312 if( AngleFilter_counter > 1 )
Amber77 0:54f408fdeada 313 { // The average of the attitude angle for 100 times.
Amber77 0:54f408fdeada 314 Roll = Roll_total / AngleFilter_counter;
Amber77 0:54f408fdeada 315 Pitch = Pitch_total / AngleFilter_counter;
Amber77 0:54f408fdeada 316 AngleFilter_counter = 0;
Amber77 0:54f408fdeada 317 Roll_total = 0;
Amber77 0:54f408fdeada 318 Pitch_total = 0;
Amber77 0:54f408fdeada 319 Roll -= Roll_offset;
Amber77 0:54f408fdeada 320 Pitch -= Pitch_offset;
Amber77 0:54f408fdeada 321 }
Amber77 0:54f408fdeada 322
Amber77 0:54f408fdeada 323 //**************************************************Gyro_data[2] filter start
Amber77 0:54f408fdeada 324 float GYRO_z=0;
Amber77 0:54f408fdeada 325
Amber77 0:54f408fdeada 326 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;
Amber77 0:54f408fdeada 327 if( count4==1 )
Amber77 0:54f408fdeada 328 {
Amber77 0:54f408fdeada 329 GYRO_z_pre_L=GYRO_z_pre;
Amber77 0:54f408fdeada 330
Amber77 0:54f408fdeada 331 count4=0;
Amber77 0:54f408fdeada 332 }
Amber77 0:54f408fdeada 333 if( count5==2 )
Amber77 0:54f408fdeada 334 {
Amber77 0:54f408fdeada 335 GYRO_z_pre_LL=GYRO_z_pre_L;
Amber77 0:54f408fdeada 336
Amber77 0:54f408fdeada 337 count5=0;
Amber77 0:54f408fdeada 338 }
Amber77 0:54f408fdeada 339 if( count6==3 )
Amber77 0:54f408fdeada 340 {
Amber77 0:54f408fdeada 341 GYRO_z_pre_LLL=GYRO_z_pre_LL;
Amber77 0:54f408fdeada 342
Amber77 0:54f408fdeada 343 count6=0;
Amber77 0:54f408fdeada 344 }
Amber77 0:54f408fdeada 345 count4++;
Amber77 0:54f408fdeada 346 count5++;
Amber77 0:54f408fdeada 347 count6++;
Amber77 0:54f408fdeada 348 GYRO_z_pre=Gyro_data[2];
Amber77 0:54f408fdeada 349 Global_GYRO_z=GYRO_z;
Amber77 0:54f408fdeada 350 /*printf(" GYRO_z:%10.3f ,count8:%10d \n",
Amber77 0:54f408fdeada 351 GYRO_z,
Amber77 0:54f408fdeada 352 count8
Amber77 0:54f408fdeada 353 );*/
Amber77 0:54f408fdeada 354 if((count8>5)&&(count8<=2005))
Amber77 0:54f408fdeada 355 {
Amber77 0:54f408fdeada 356 GYRO_z_total+=GYRO_z;
Amber77 0:54f408fdeada 357 }
Amber77 0:54f408fdeada 358 if( count8==2005 )
Amber77 0:54f408fdeada 359 {
Amber77 0:54f408fdeada 360 GYRO_z_offset=GYRO_z_total/2000;
Amber77 0:54f408fdeada 361 /* printf(" GYRO_z_offset:%10.5f \n ",
Amber77 0:54f408fdeada 362 GYRO_z_offset
Amber77 0:54f408fdeada 363 );*/
Amber77 0:54f408fdeada 364 GYRO_z_total=0;
Amber77 0:54f408fdeada 365 count8=0;
Amber77 0:54f408fdeada 366 }
Amber77 0:54f408fdeada 367
Amber77 0:54f408fdeada 368 count8++;
Amber77 0:54f408fdeada 369 //**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 0.74956
Amber77 0:54f408fdeada 370 //************************************************** calculate Yaw
Amber77 0:54f408fdeada 371 if( (count11==35) )
Amber77 0:54f408fdeada 372 {
Amber77 0:54f408fdeada 373 if( abs(Yaw_pre-Yaw)<1 )
Amber77 0:54f408fdeada 374 {
Amber77 0:54f408fdeada 375 Yaw_pre=Yaw_pre;
Amber77 0:54f408fdeada 376 }
Amber77 0:54f408fdeada 377 else
Amber77 0:54f408fdeada 378 {
Amber77 0:54f408fdeada 379 Yaw_pre=Yaw;
Amber77 0:54f408fdeada 380 }
Amber77 0:54f408fdeada 381 count11=0;
Amber77 0:54f408fdeada 382 }
Amber77 0:54f408fdeada 383 count11++;
Amber77 0:54f408fdeada 384
Amber77 0:54f408fdeada 385 if( count12>=20 )
Amber77 0:54f408fdeada 386 {
Amber77 0:54f408fdeada 387 Yaw += (Gyro_data[2]-0.74936) *dt;
Amber77 0:54f408fdeada 388 }
Amber77 0:54f408fdeada 389 count12++;
Amber77 0:54f408fdeada 390 //pc.printf(" Yaw:%10.5f ",
Amber77 0:54f408fdeada 391 //Yaw
Amber77 0:54f408fdeada 392 // );
Amber77 0:54f408fdeada 393 }
Amber77 0:54f408fdeada 394
Amber77 0:54f408fdeada 395 //**************************** Mag_Complentary_Filter ************************************//
Amber77 0:54f408fdeada 396 void Mag_Complentary_Filter(float dt, const float * Comp_data)
Amber77 0:54f408fdeada 397 {
Amber77 0:54f408fdeada 398 float Mag_x=0,Mag_y=0,Mag_z=0;
Amber77 0:54f408fdeada 399 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;
Amber77 0:54f408fdeada 400 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;
Amber77 0:54f408fdeada 401 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;
Amber77 0:54f408fdeada 402
Amber77 0:54f408fdeada 403 if( count1==1 )
Amber77 0:54f408fdeada 404 {
Amber77 0:54f408fdeada 405 Mag_x_pre_L=Mag_x_pre;
Amber77 0:54f408fdeada 406 Mag_y_pre_L=Mag_y_pre;
Amber77 0:54f408fdeada 407 Mag_z_pre_L=Mag_z_pre;
Amber77 0:54f408fdeada 408 Cal_Mag_x_pre=Cal_Mag_x;
Amber77 0:54f408fdeada 409
Amber77 0:54f408fdeada 410 count1=0;
Amber77 0:54f408fdeada 411 }
Amber77 0:54f408fdeada 412 if( count2==2 )
Amber77 0:54f408fdeada 413 {
Amber77 0:54f408fdeada 414 Mag_x_pre_LL=Mag_x_pre_L;
Amber77 0:54f408fdeada 415 Mag_y_pre_LL=Mag_y_pre_L;
Amber77 0:54f408fdeada 416 Mag_z_pre_LL=Mag_z_pre_L;
Amber77 0:54f408fdeada 417 Cal_Mag_x_pre_L=Cal_Mag_x_pre;
Amber77 0:54f408fdeada 418
Amber77 0:54f408fdeada 419 count2=0;
Amber77 0:54f408fdeada 420 }
Amber77 0:54f408fdeada 421 if( count7==3 )
Amber77 0:54f408fdeada 422 {
Amber77 0:54f408fdeada 423 Mag_x_pre_LLL=Mag_x_pre_LL;
Amber77 0:54f408fdeada 424 Mag_y_pre_LLL=Mag_y_pre_LL;
Amber77 0:54f408fdeada 425 Mag_z_pre_LLL=Mag_z_pre_LL;
Amber77 0:54f408fdeada 426 Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
Amber77 0:54f408fdeada 427
Amber77 0:54f408fdeada 428 count7=0;
Amber77 0:54f408fdeada 429 }
Amber77 0:54f408fdeada 430 count1++;
Amber77 0:54f408fdeada 431 count2++;
Amber77 0:54f408fdeada 432 count7++;
Amber77 0:54f408fdeada 433 Mag_x_pre=Comp_data[0];
Amber77 0:54f408fdeada 434 Mag_y_pre=Comp_data[1];
Amber77 0:54f408fdeada 435 Mag_z_pre=Comp_data[2];
Amber77 0:54f408fdeada 436 if( count14>4 )
Amber77 0:54f408fdeada 437 {
Amber77 0:54f408fdeada 438 Cal_Mag_x=Mag_x;
Amber77 0:54f408fdeada 439 }
Amber77 0:54f408fdeada 440 count14++;
Amber77 0:54f408fdeada 441
Amber77 0:54f408fdeada 442
Amber77 0:54f408fdeada 443 //*************************************Mag_ave calculate
Amber77 0:54f408fdeada 444 if(count3<=20)
Amber77 0:54f408fdeada 445 {
Amber77 0:54f408fdeada 446 Mag_x_total+=Mag_x;
Amber77 0:54f408fdeada 447 Mag_y_total+=Mag_y;
Amber77 0:54f408fdeada 448 }
Amber77 0:54f408fdeada 449 if( count3==20)
Amber77 0:54f408fdeada 450 {
Amber77 0:54f408fdeada 451 Mag_x_ave=Mag_x_total/21;
Amber77 0:54f408fdeada 452 Mag_y_ave=Mag_y_total/21;
Amber77 0:54f408fdeada 453 /*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ",
Amber77 0:54f408fdeada 454 Mag_x_ave,
Amber77 0:54f408fdeada 455 Mag_y_ave
Amber77 0:54f408fdeada 456 );*/
Amber77 0:54f408fdeada 457 Mag_x_total=0;
Amber77 0:54f408fdeada 458 Mag_y_total=0;
Amber77 0:54f408fdeada 459 count3=0;
Amber77 0:54f408fdeada 460 }
Amber77 0:54f408fdeada 461 count3++;
Amber77 0:54f408fdeada 462
Amber77 0:54f408fdeada 463 //********************************ROT_check start
Amber77 0:54f408fdeada 464
Amber77 0:54f408fdeada 465 float v_length,v_length_ave,MagVector_angle;
Amber77 0:54f408fdeada 466 v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y );
Amber77 0:54f408fdeada 467 v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave );
Amber77 0:54f408fdeada 468
Amber77 0:54f408fdeada 469 MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3;
Amber77 0:54f408fdeada 470
Amber77 0:54f408fdeada 471 if( count9==3 )
Amber77 0:54f408fdeada 472 {
Amber77 0:54f408fdeada 473 Global_mag_vector_angle=MagVector_angle;
Amber77 0:54f408fdeada 474 count9=0;
Amber77 0:54f408fdeada 475 }
Amber77 0:54f408fdeada 476 count9++;
Amber77 0:54f408fdeada 477
Amber77 0:54f408fdeada 478 if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) )
Amber77 0:54f408fdeada 479 {
Amber77 0:54f408fdeada 480 Count_mag_check++;
Amber77 0:54f408fdeada 481
Amber77 0:54f408fdeada 482 }
Amber77 0:54f408fdeada 483 else
Amber77 0:54f408fdeada 484 {
Amber77 0:54f408fdeada 485 Count_mag_check=0;
Amber77 0:54f408fdeada 486 }
Amber77 0:54f408fdeada 487
Amber77 0:54f408fdeada 488 if( Count_mag_check==30 )
Amber77 0:54f408fdeada 489 {
Amber77 0:54f408fdeada 490 Yaw=Yaw_pre;
Amber77 0:54f408fdeada 491 Count_mag_check=0;
Amber77 0:54f408fdeada 492 }
Amber77 0:54f408fdeada 493 float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
Amber77 0:54f408fdeada 494 //********************************Theta_check end
Amber77 0:54f408fdeada 495 /*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 ",
Amber77 0:54f408fdeada 496 ABS_CHECK,
Amber77 0:54f408fdeada 497 Cal_Mag_x_pre_LL,
Amber77 0:54f408fdeada 498 Mag_x,
Amber77 0:54f408fdeada 499 Count_mag_check,
Amber77 0:54f408fdeada 500 Yaw_pre,
Amber77 0:54f408fdeada 501 Yaw
Amber77 0:54f408fdeada 502 );*/
Amber77 0:54f408fdeada 503 }
Amber77 0:54f408fdeada 504 //****************************Motor_run************************************//
Amber77 0:54f408fdeada 505 int Motor_run(double control_value_PWM_X, double control_value_PWM_Y, int control_AR, int control_brake , int control_stopRun, int control_X1_CW_CCW, int control_X2_CW_CCW, int control_Y1_CW_CCW, int control_Y2_CW_CCW)
Amber77 0:54f408fdeada 506 {
Amber77 0:54f408fdeada 507 StopRun.write(control_stopRun);
Amber77 0:54f408fdeada 508 Brake.write(control_brake);
Amber77 0:54f408fdeada 509 AR.write(control_AR);
Amber77 0:54f408fdeada 510 X1_CW_CCW.write(control_X1_CW_CCW);
Amber77 0:54f408fdeada 511 X2_CW_CCW.write(control_X2_CW_CCW);
Amber77 0:54f408fdeada 512 Y1_CW_CCW.write(control_Y1_CW_CCW);
Amber77 0:54f408fdeada 513 Y2_CW_CCW.write(control_Y2_CW_CCW);
Amber77 0:54f408fdeada 514
Amber77 0:54f408fdeada 515
Amber77 0:54f408fdeada 516 X_PWM.write(control_value_PWM_X);
Amber77 0:54f408fdeada 517 Y_PWM.write(control_value_PWM_Y);
Amber77 0:54f408fdeada 518 }
Amber77 0:54f408fdeada 519
Amber77 0:54f408fdeada 520 //****************************quadratureDecoder************************************//
Amber77 0:54f408fdeada 521 void quadratureDecoder( void )
Amber77 0:54f408fdeada 522 {
Amber77 0:54f408fdeada 523 int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read();
Amber77 0:54f408fdeada 524 int currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read();
Amber77 0:54f408fdeada 525 int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read();
Amber77 0:54f408fdeada 526 int currentEncoderState_4 = (phaseB_4.read() << 1) + phaseA_4.read();
Amber77 0:54f408fdeada 527 //**************************** 1 ************************************//
Amber77 0:54f408fdeada 528 if( currentEncoderState_1 == previousEncoderState_1 )
Amber77 0:54f408fdeada 529 {
Amber77 0:54f408fdeada 530 return;
Amber77 0:54f408fdeada 531 }
Amber77 0:54f408fdeada 532
Amber77 0:54f408fdeada 533 switch( previousEncoderState_1 )
Amber77 0:54f408fdeada 534 {
Amber77 0:54f408fdeada 535 case 0:
Amber77 0:54f408fdeada 536 if( currentEncoderState_1 == 2 )
Amber77 0:54f408fdeada 537 {
Amber77 0:54f408fdeada 538 encoderClickCount_1--;
Amber77 0:54f408fdeada 539
Amber77 0:54f408fdeada 540 }
Amber77 0:54f408fdeada 541 else if( currentEncoderState_1 == 1 )
Amber77 0:54f408fdeada 542 {
Amber77 0:54f408fdeada 543 encoderClickCount_1++;
Amber77 0:54f408fdeada 544
Amber77 0:54f408fdeada 545 }
Amber77 0:54f408fdeada 546 break;
Amber77 0:54f408fdeada 547
Amber77 0:54f408fdeada 548 case 1:
Amber77 0:54f408fdeada 549 if( currentEncoderState_1 == 0 )
Amber77 0:54f408fdeada 550 {
Amber77 0:54f408fdeada 551 encoderClickCount_1--;
Amber77 0:54f408fdeada 552
Amber77 0:54f408fdeada 553 }
Amber77 0:54f408fdeada 554 else if( currentEncoderState_1 == 3 )
Amber77 0:54f408fdeada 555 {
Amber77 0:54f408fdeada 556 encoderClickCount_1++;
Amber77 0:54f408fdeada 557
Amber77 0:54f408fdeada 558 }
Amber77 0:54f408fdeada 559 break;
Amber77 0:54f408fdeada 560
Amber77 0:54f408fdeada 561 case 2:
Amber77 0:54f408fdeada 562 if( currentEncoderState_1 == 3 )
Amber77 0:54f408fdeada 563 {
Amber77 0:54f408fdeada 564 encoderClickCount_1--;
Amber77 0:54f408fdeada 565
Amber77 0:54f408fdeada 566 }
Amber77 0:54f408fdeada 567 else if( currentEncoderState_1 == 0 )
Amber77 0:54f408fdeada 568 {
Amber77 0:54f408fdeada 569 encoderClickCount_1++;
Amber77 0:54f408fdeada 570
Amber77 0:54f408fdeada 571 }
Amber77 0:54f408fdeada 572 break;
Amber77 0:54f408fdeada 573
Amber77 0:54f408fdeada 574 case 3:
Amber77 0:54f408fdeada 575 if( currentEncoderState_1 == 1 )
Amber77 0:54f408fdeada 576 {
Amber77 0:54f408fdeada 577 encoderClickCount_1--;
Amber77 0:54f408fdeada 578
Amber77 0:54f408fdeada 579 }
Amber77 0:54f408fdeada 580 else if( currentEncoderState_1 == 2 )
Amber77 0:54f408fdeada 581 {
Amber77 0:54f408fdeada 582 encoderClickCount_1++;
Amber77 0:54f408fdeada 583
Amber77 0:54f408fdeada 584 }
Amber77 0:54f408fdeada 585 break;
Amber77 0:54f408fdeada 586
Amber77 0:54f408fdeada 587 default:
Amber77 0:54f408fdeada 588 break;
Amber77 0:54f408fdeada 589 }
Amber77 0:54f408fdeada 590 previousEncoderState_1 = currentEncoderState_1;
Amber77 0:54f408fdeada 591 //**************************** 2 ************************************//
Amber77 0:54f408fdeada 592 if( currentEncoderState_2 == previousEncoderState_2 )
Amber77 0:54f408fdeada 593 {
Amber77 0:54f408fdeada 594 return;
Amber77 0:54f408fdeada 595 }
Amber77 0:54f408fdeada 596
Amber77 0:54f408fdeada 597 switch( previousEncoderState_2 )
Amber77 0:54f408fdeada 598 {
Amber77 0:54f408fdeada 599 case 0:
Amber77 0:54f408fdeada 600 if( currentEncoderState_2 == 2 )
Amber77 0:54f408fdeada 601 {
Amber77 0:54f408fdeada 602 encoderClickCount_2--;
Amber77 0:54f408fdeada 603
Amber77 0:54f408fdeada 604 }
Amber77 0:54f408fdeada 605 else if( currentEncoderState_2 == 1 )
Amber77 0:54f408fdeada 606 {
Amber77 0:54f408fdeada 607 encoderClickCount_2++;
Amber77 0:54f408fdeada 608
Amber77 0:54f408fdeada 609 }
Amber77 0:54f408fdeada 610 break;
Amber77 0:54f408fdeada 611
Amber77 0:54f408fdeada 612 case 1:
Amber77 0:54f408fdeada 613 if( currentEncoderState_2 == 0 )
Amber77 0:54f408fdeada 614 {
Amber77 0:54f408fdeada 615 encoderClickCount_2--;
Amber77 0:54f408fdeada 616
Amber77 0:54f408fdeada 617 }
Amber77 0:54f408fdeada 618 else if( currentEncoderState_2 == 3 )
Amber77 0:54f408fdeada 619 {
Amber77 0:54f408fdeada 620 encoderClickCount_2++;
Amber77 0:54f408fdeada 621
Amber77 0:54f408fdeada 622 }
Amber77 0:54f408fdeada 623 break;
Amber77 0:54f408fdeada 624
Amber77 0:54f408fdeada 625 case 2:
Amber77 0:54f408fdeada 626 if( currentEncoderState_2 == 3 )
Amber77 0:54f408fdeada 627 {
Amber77 0:54f408fdeada 628 encoderClickCount_2--;
Amber77 0:54f408fdeada 629
Amber77 0:54f408fdeada 630 }
Amber77 0:54f408fdeada 631 else if( currentEncoderState_2 == 0 )
Amber77 0:54f408fdeada 632 {
Amber77 0:54f408fdeada 633 encoderClickCount_2++;
Amber77 0:54f408fdeada 634
Amber77 0:54f408fdeada 635 }
Amber77 0:54f408fdeada 636 break;
Amber77 0:54f408fdeada 637
Amber77 0:54f408fdeada 638 case 3:
Amber77 0:54f408fdeada 639 if( currentEncoderState_2 == 1 )
Amber77 0:54f408fdeada 640 {
Amber77 0:54f408fdeada 641 encoderClickCount_2--;
Amber77 0:54f408fdeada 642
Amber77 0:54f408fdeada 643 }
Amber77 0:54f408fdeada 644 else if( currentEncoderState_2 == 2 )
Amber77 0:54f408fdeada 645 {
Amber77 0:54f408fdeada 646 encoderClickCount_2++;
Amber77 0:54f408fdeada 647
Amber77 0:54f408fdeada 648 }
Amber77 0:54f408fdeada 649 break;
Amber77 0:54f408fdeada 650
Amber77 0:54f408fdeada 651 default:
Amber77 0:54f408fdeada 652 break;
Amber77 0:54f408fdeada 653 }
Amber77 0:54f408fdeada 654 previousEncoderState_2 = currentEncoderState_2;
Amber77 0:54f408fdeada 655 //**************************** 3 ************************************//
Amber77 0:54f408fdeada 656 if( currentEncoderState_3 == previousEncoderState_3 )
Amber77 0:54f408fdeada 657 {
Amber77 0:54f408fdeada 658 return;
Amber77 0:54f408fdeada 659 }
Amber77 0:54f408fdeada 660
Amber77 0:54f408fdeada 661 switch( previousEncoderState_3 )
Amber77 0:54f408fdeada 662 {
Amber77 0:54f408fdeada 663 case 0:
Amber77 0:54f408fdeada 664 if( currentEncoderState_3 == 2 )
Amber77 0:54f408fdeada 665 {
Amber77 0:54f408fdeada 666 encoderClickCount_3--;
Amber77 0:54f408fdeada 667
Amber77 0:54f408fdeada 668 }
Amber77 0:54f408fdeada 669 else if( currentEncoderState_3 == 1 )
Amber77 0:54f408fdeada 670 {
Amber77 0:54f408fdeada 671 encoderClickCount_3++;
Amber77 0:54f408fdeada 672
Amber77 0:54f408fdeada 673 }
Amber77 0:54f408fdeada 674 break;
Amber77 0:54f408fdeada 675
Amber77 0:54f408fdeada 676 case 1:
Amber77 0:54f408fdeada 677 if( currentEncoderState_3 == 0 )
Amber77 0:54f408fdeada 678 {
Amber77 0:54f408fdeada 679 encoderClickCount_3--;
Amber77 0:54f408fdeada 680
Amber77 0:54f408fdeada 681 }
Amber77 0:54f408fdeada 682 else if( currentEncoderState_3 == 3 )
Amber77 0:54f408fdeada 683 {
Amber77 0:54f408fdeada 684 encoderClickCount_3++;
Amber77 0:54f408fdeada 685
Amber77 0:54f408fdeada 686 }
Amber77 0:54f408fdeada 687 break;
Amber77 0:54f408fdeada 688
Amber77 0:54f408fdeada 689 case 2:
Amber77 0:54f408fdeada 690 if( currentEncoderState_3 == 3 )
Amber77 0:54f408fdeada 691 {
Amber77 0:54f408fdeada 692 encoderClickCount_3--;
Amber77 0:54f408fdeada 693
Amber77 0:54f408fdeada 694 }
Amber77 0:54f408fdeada 695 else if( currentEncoderState_3 == 0 )
Amber77 0:54f408fdeada 696 {
Amber77 0:54f408fdeada 697 encoderClickCount_3++;
Amber77 0:54f408fdeada 698
Amber77 0:54f408fdeada 699 }
Amber77 0:54f408fdeada 700 break;
Amber77 0:54f408fdeada 701
Amber77 0:54f408fdeada 702 case 3:
Amber77 0:54f408fdeada 703 if( currentEncoderState_3 == 1 )
Amber77 0:54f408fdeada 704 {
Amber77 0:54f408fdeada 705 encoderClickCount_3--;
Amber77 0:54f408fdeada 706
Amber77 0:54f408fdeada 707 }
Amber77 0:54f408fdeada 708 else if( currentEncoderState_3 == 2 )
Amber77 0:54f408fdeada 709 {
Amber77 0:54f408fdeada 710 encoderClickCount_3++;
Amber77 0:54f408fdeada 711
Amber77 0:54f408fdeada 712 }
Amber77 0:54f408fdeada 713 break;
Amber77 0:54f408fdeada 714
Amber77 0:54f408fdeada 715 default:
Amber77 0:54f408fdeada 716 break;
Amber77 0:54f408fdeada 717 }
Amber77 0:54f408fdeada 718 previousEncoderState_3 = currentEncoderState_3;
Amber77 0:54f408fdeada 719 //**************************** 4 ************************************//
Amber77 0:54f408fdeada 720 if( currentEncoderState_4 == previousEncoderState_4 )
Amber77 0:54f408fdeada 721 {
Amber77 0:54f408fdeada 722 return;
Amber77 0:54f408fdeada 723 }
Amber77 0:54f408fdeada 724
Amber77 0:54f408fdeada 725 switch( previousEncoderState_4 )
Amber77 0:54f408fdeada 726 {
Amber77 0:54f408fdeada 727 case 0:
Amber77 0:54f408fdeada 728 if( currentEncoderState_4 == 2 )
Amber77 0:54f408fdeada 729 {
Amber77 0:54f408fdeada 730 encoderClickCount_4--;
Amber77 0:54f408fdeada 731
Amber77 0:54f408fdeada 732 }
Amber77 0:54f408fdeada 733 else if( currentEncoderState_4 == 1 )
Amber77 0:54f408fdeada 734 {
Amber77 0:54f408fdeada 735 encoderClickCount_4++;
Amber77 0:54f408fdeada 736
Amber77 0:54f408fdeada 737 }
Amber77 0:54f408fdeada 738 break;
Amber77 0:54f408fdeada 739
Amber77 0:54f408fdeada 740 case 1:
Amber77 0:54f408fdeada 741 if( currentEncoderState_4 == 0 )
Amber77 0:54f408fdeada 742 {
Amber77 0:54f408fdeada 743 encoderClickCount_4--;
Amber77 0:54f408fdeada 744
Amber77 0:54f408fdeada 745 }
Amber77 0:54f408fdeada 746 else if( currentEncoderState_4 == 3 )
Amber77 0:54f408fdeada 747 {
Amber77 0:54f408fdeada 748 encoderClickCount_4++;
Amber77 0:54f408fdeada 749
Amber77 0:54f408fdeada 750 }
Amber77 0:54f408fdeada 751 break;
Amber77 0:54f408fdeada 752
Amber77 0:54f408fdeada 753 case 2:
Amber77 0:54f408fdeada 754 if( currentEncoderState_4 == 3 )
Amber77 0:54f408fdeada 755 {
Amber77 0:54f408fdeada 756 encoderClickCount_4--;
Amber77 0:54f408fdeada 757
Amber77 0:54f408fdeada 758 }
Amber77 0:54f408fdeada 759 else if( currentEncoderState_4 == 0 )
Amber77 0:54f408fdeada 760 {
Amber77 0:54f408fdeada 761 encoderClickCount_4++;
Amber77 0:54f408fdeada 762
Amber77 0:54f408fdeada 763 }
Amber77 0:54f408fdeada 764 break;
Amber77 0:54f408fdeada 765
Amber77 0:54f408fdeada 766 case 3:
Amber77 0:54f408fdeada 767 if( currentEncoderState_4 == 1 )
Amber77 0:54f408fdeada 768 {
Amber77 0:54f408fdeada 769 encoderClickCount_4--;
Amber77 0:54f408fdeada 770
Amber77 0:54f408fdeada 771 }
Amber77 0:54f408fdeada 772 else if( currentEncoderState_4 == 2 )
Amber77 0:54f408fdeada 773 {
Amber77 0:54f408fdeada 774 encoderClickCount_4++;
Amber77 0:54f408fdeada 775
Amber77 0:54f408fdeada 776 }
Amber77 0:54f408fdeada 777 break;
Amber77 0:54f408fdeada 778
Amber77 0:54f408fdeada 779 default:
Amber77 0:54f408fdeada 780 break;
Amber77 0:54f408fdeada 781 }
Amber77 0:54f408fdeada 782 previousEncoderState_4 = currentEncoderState_4;
Amber77 0:54f408fdeada 783 }
Amber77 0:54f408fdeada 784
Amber77 0:54f408fdeada 785 //****************************getAngular************************************//
Amber77 0:54f408fdeada 786 //****************************getAngular************************************//
Amber77 0:54f408fdeada 787 void getAngular( void )
Amber77 0:54f408fdeada 788 {
Amber77 0:54f408fdeada 789 // Angle_1 = (encoderClickCount_1*0.1499)/5;
Amber77 0:54f408fdeada 790 // Angle_2 = (encoderClickCount_2*0.1499)/5;
Amber77 0:54f408fdeada 791 // Angle_3 = (encoderClickCount_3*0.1499)/5;
Amber77 0:54f408fdeada 792 // Angle_4 = (encoderClickCount_4*0.1499)/5;
Amber77 0:54f408fdeada 793 Angle_1 = encoderClickCount_1*0.03;
Amber77 0:54f408fdeada 794 Angle_2 = encoderClickCount_2*0.03;
Amber77 0:54f408fdeada 795 Angle_3 = encoderClickCount_3*0.03;
Amber77 0:54f408fdeada 796 Angle_4 = encoderClickCount_4*0.03;
Amber77 0:54f408fdeada 797
Amber77 0:54f408fdeada 798 _1_SectionAngle = Angle_1 - LastAngle_1;
Amber77 0:54f408fdeada 799 _2_SectionAngle = Angle_2 - LastAngle_2;
Amber77 0:54f408fdeada 800 _3_SectionAngle = Angle_3 - LastAngle_3;
Amber77 0:54f408fdeada 801 _4_SectionAngle = Angle_4 - LastAngle_4;
Amber77 0:54f408fdeada 802 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;
Amber77 0:54f408fdeada 803 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;
Amber77 0:54f408fdeada 804 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;
Amber77 0:54f408fdeada 805 Average_SectionAngle_4 = (_4_SectionAngle*0.3 + _4_LastSectionAngle*0.3 + _4_LastSectionAngle_1*0.1 + _4_LastSectionAngle_2*0.1 + _4_LastSectionAngle_3*0.1 + _4_LastSectionAngle_4*0.1)/1;
Amber77 0:54f408fdeada 806 NowTime_measureVelocity = NowTime.read();
Amber77 0:54f408fdeada 807 SectionTime = NowTime_measureVelocity - LastTime_measureVelocity;
Amber77 0:54f408fdeada 808 Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime));
Amber77 0:54f408fdeada 809 Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime));
Amber77 0:54f408fdeada 810 Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime));
Amber77 0:54f408fdeada 811 Now_angularVelocity_4 = abs(Average_SectionAngle_4/(SectionTime));
Amber77 0:54f408fdeada 812 Now_angularVelocity_X = (Now_angularVelocity_2+Now_angularVelocity_4)*0.5;
Amber77 0:54f408fdeada 813 Now_angularVelocity_Y = (Now_angularVelocity_1+Now_angularVelocity_3)*0.5;
Amber77 0:54f408fdeada 814
Amber77 0:54f408fdeada 815
Amber77 0:54f408fdeada 816 LastTime_measureVelocity = NowTime_measureVelocity;
Amber77 0:54f408fdeada 817 LastAngle_1 = Angle_1;
Amber77 0:54f408fdeada 818 _1_LastSectionAngle_4 = _1_LastSectionAngle_3;
Amber77 0:54f408fdeada 819 _1_LastSectionAngle_3 = _1_LastSectionAngle_2;
Amber77 0:54f408fdeada 820 _1_LastSectionAngle_2 = _1_LastSectionAngle_1;
Amber77 0:54f408fdeada 821 _1_LastSectionAngle_1 = _1_LastSectionAngle;
Amber77 0:54f408fdeada 822 _1_LastSectionAngle = _1_SectionAngle;
Amber77 0:54f408fdeada 823 LastAngle_2 = Angle_2;
Amber77 0:54f408fdeada 824 _2_LastSectionAngle_4 = _2_LastSectionAngle_3;
Amber77 0:54f408fdeada 825 _2_LastSectionAngle_3 = _2_LastSectionAngle_2;
Amber77 0:54f408fdeada 826 _2_LastSectionAngle_2 = _2_LastSectionAngle_1;
Amber77 0:54f408fdeada 827 _2_LastSectionAngle_1 = _2_LastSectionAngle;
Amber77 0:54f408fdeada 828 _2_LastSectionAngle = _2_SectionAngle;
Amber77 0:54f408fdeada 829 LastAngle_3 = Angle_3;
Amber77 0:54f408fdeada 830 _3_LastSectionAngle_4 = _3_LastSectionAngle_3;
Amber77 0:54f408fdeada 831 _3_LastSectionAngle_3 = _3_LastSectionAngle_2;
Amber77 0:54f408fdeada 832 _3_LastSectionAngle_2 = _3_LastSectionAngle_1;
Amber77 0:54f408fdeada 833 _3_LastSectionAngle_1 = _3_LastSectionAngle;
Amber77 0:54f408fdeada 834 _3_LastSectionAngle = _3_SectionAngle;
Amber77 0:54f408fdeada 835 LastAngle_4 = Angle_4;
Amber77 0:54f408fdeada 836 _4_LastSectionAngle_4 = _4_LastSectionAngle_3;
Amber77 0:54f408fdeada 837 _4_LastSectionAngle_3 = _4_LastSectionAngle_2;
Amber77 0:54f408fdeada 838 _4_LastSectionAngle_2 = _4_LastSectionAngle_1;
Amber77 0:54f408fdeada 839 _4_LastSectionAngle_1 = _4_LastSectionAngle;
Amber77 0:54f408fdeada 840 _4_LastSectionAngle = _4_SectionAngle;
Amber77 0:54f408fdeada 841
Amber77 0:54f408fdeada 842 if (Angle_1>=0 && Angle_3>=0)
Amber77 0:54f408fdeada 843 {
Amber77 0:54f408fdeada 844 Angle_Y = (Angle_1 + Angle_3)*0.5;
Amber77 0:54f408fdeada 845 }
Amber77 0:54f408fdeada 846 else if(Angle_1>=0 && Angle_3<=0)
Amber77 0:54f408fdeada 847 {
Amber77 0:54f408fdeada 848 Angle_Y = (Angle_1 + abs(Angle_3))*0.5;
Amber77 0:54f408fdeada 849 }
Amber77 0:54f408fdeada 850 else if(Angle_1<=0 && Angle_3>=0)
Amber77 0:54f408fdeada 851 {
Amber77 0:54f408fdeada 852 Angle_Y = -(abs(Angle_1) + Angle_3)*0.5;
Amber77 0:54f408fdeada 853 }
Amber77 0:54f408fdeada 854 else if(Angle_1<=0 && Angle_3<=0)
Amber77 0:54f408fdeada 855 {
Amber77 0:54f408fdeada 856 Angle_Y = (Angle_1 + Angle_3)*0.5;
Amber77 0:54f408fdeada 857 }
Amber77 0:54f408fdeada 858
Amber77 0:54f408fdeada 859 if (Angle_2>=0 && Angle_4>=0)
Amber77 0:54f408fdeada 860 {
Amber77 0:54f408fdeada 861 Angle_X = (Angle_2 + Angle_4)*0.5;
Amber77 0:54f408fdeada 862 }
Amber77 0:54f408fdeada 863 else if (Angle_2>=0 && Angle_4<=0)
Amber77 0:54f408fdeada 864 {
Amber77 0:54f408fdeada 865 Angle_X = (Angle_2 + abs(Angle_4))*0.5;
Amber77 0:54f408fdeada 866 }
Amber77 0:54f408fdeada 867 else if (Angle_2<=0 && Angle_4>=0)
Amber77 0:54f408fdeada 868 {
Amber77 0:54f408fdeada 869 Angle_X = -(abs(Angle_2) + Angle_4)*0.5;
Amber77 0:54f408fdeada 870 }
Amber77 0:54f408fdeada 871 else if (Angle_2<=0 && Angle_4<=0)
Amber77 0:54f408fdeada 872 {
Amber77 0:54f408fdeada 873 Angle_X = (Angle_2 + Angle_4)*0.5;
Amber77 0:54f408fdeada 874 }
Amber77 0:54f408fdeada 875
Amber77 0:54f408fdeada 876 }
Amber77 0:54f408fdeada 877
Amber77 0:54f408fdeada 878 //****************************PWM_commmand_transformation************************************//
Amber77 0:54f408fdeada 879 //****************************PWM_commmand_transformation************************************//
Amber77 0:54f408fdeada 880 double PWM_commmand_transformation( double Control_AngVel_Value )
Amber77 0:54f408fdeada 881 {
Amber77 0:54f408fdeada 882 double Control_PWM_Value = 0;
Amber77 0:54f408fdeada 883 if( Control_AngVel_Value > 0 )
Amber77 0:54f408fdeada 884 {
Amber77 0:54f408fdeada 885 Control_AngVel_Value = Control_AngVel_Value;
Amber77 0:54f408fdeada 886 }
Amber77 0:54f408fdeada 887 else if( Control_AngVel_Value < 0 )
Amber77 0:54f408fdeada 888 {
Amber77 0:54f408fdeada 889 Control_AngVel_Value = -Control_AngVel_Value;
Amber77 0:54f408fdeada 890 }
Amber77 0:54f408fdeada 891 if( Control_AngVel_Value > 325)
Amber77 0:54f408fdeada 892 {
Amber77 0:54f408fdeada 893 if( Control_AngVel_Value < 466 )
Amber77 0:54f408fdeada 894 {
Amber77 0:54f408fdeada 895 if( Control_AngVel_Value < 393 )
Amber77 0:54f408fdeada 896 {
Amber77 0:54f408fdeada 897 Control_PWM_Value = Control_AngVel_Value/651.6 ; //0.5~0.6
Amber77 0:54f408fdeada 898 }
Amber77 0:54f408fdeada 899 else
Amber77 0:54f408fdeada 900 {
Amber77 0:54f408fdeada 901 Control_PWM_Value = Control_AngVel_Value/658.8; //0.6~0.7
Amber77 0:54f408fdeada 902 }
Amber77 0:54f408fdeada 903 }
Amber77 0:54f408fdeada 904 else
Amber77 0:54f408fdeada 905 {
Amber77 0:54f408fdeada 906 if( Control_AngVel_Value < 523 )
Amber77 0:54f408fdeada 907 {
Amber77 0:54f408fdeada 908 Control_PWM_Value = Control_AngVel_Value/658.39; //0.7~0.8
Amber77 0:54f408fdeada 909 }
Amber77 0:54f408fdeada 910 else
Amber77 0:54f408fdeada 911 {
Amber77 0:54f408fdeada 912 if( Control_AngVel_Value < 588 )
Amber77 0:54f408fdeada 913 {
Amber77 0:54f408fdeada 914 Control_PWM_Value = Control_AngVel_Value/652.36; //0.8~0.9
Amber77 0:54f408fdeada 915 }
Amber77 0:54f408fdeada 916 else
Amber77 0:54f408fdeada 917 {
Amber77 0:54f408fdeada 918 Control_PWM_Value = Control_AngVel_Value/655.11; //0.9~1
Amber77 0:54f408fdeada 919 }
Amber77 0:54f408fdeada 920 }
Amber77 0:54f408fdeada 921 }
Amber77 0:54f408fdeada 922 }
Amber77 0:54f408fdeada 923 else if( Control_AngVel_Value < 325)
Amber77 0:54f408fdeada 924 {
Amber77 0:54f408fdeada 925 if( Control_AngVel_Value < 40 )
Amber77 0:54f408fdeada 926 {
Amber77 0:54f408fdeada 927 Control_PWM_Value = Control_AngVel_Value/533.3; //0~0.075
Amber77 0:54f408fdeada 928 }
Amber77 0:54f408fdeada 929 else
Amber77 0:54f408fdeada 930 {
Amber77 0:54f408fdeada 931 if( Control_AngVel_Value < 59 )
Amber77 0:54f408fdeada 932 {
Amber77 0:54f408fdeada 933 Control_PWM_Value = Control_AngVel_Value/560.65; //0.1~0.15
Amber77 0:54f408fdeada 934 }
Amber77 0:54f408fdeada 935 else
Amber77 0:54f408fdeada 936 {
Amber77 0:54f408fdeada 937 if( Control_AngVel_Value < 131 )
Amber77 0:54f408fdeada 938 {
Amber77 0:54f408fdeada 939 Control_PWM_Value = Control_AngVel_Value/638.3; //0.15~0.2
Amber77 0:54f408fdeada 940 }
Amber77 0:54f408fdeada 941 else
Amber77 0:54f408fdeada 942 {
Amber77 0:54f408fdeada 943 if( Control_AngVel_Value < 197 )
Amber77 0:54f408fdeada 944 {
Amber77 0:54f408fdeada 945 Control_PWM_Value = Control_AngVel_Value/651.6; //0.2~0.3
Amber77 0:54f408fdeada 946 }
Amber77 0:54f408fdeada 947 else
Amber77 0:54f408fdeada 948 {
Amber77 0:54f408fdeada 949 if( Control_AngVel_Value < 263 )
Amber77 0:54f408fdeada 950 {
Amber77 0:54f408fdeada 951 Control_PWM_Value = Control_AngVel_Value/654.16; //0.3~0.4
Amber77 0:54f408fdeada 952 }
Amber77 0:54f408fdeada 953 else
Amber77 0:54f408fdeada 954 {
Amber77 0:54f408fdeada 955 Control_PWM_Value = Control_AngVel_Value/652.5; //0.4~0.5
Amber77 0:54f408fdeada 956 }
Amber77 0:54f408fdeada 957 }
Amber77 0:54f408fdeada 958 }
Amber77 0:54f408fdeada 959 }
Amber77 0:54f408fdeada 960 }
Amber77 0:54f408fdeada 961 }
Amber77 0:54f408fdeada 962 return Control_PWM_Value;
Amber77 0:54f408fdeada 963 }
Amber77 0:54f408fdeada 964
Amber77 0:54f408fdeada 965 //****************************PIDcontrol_compute_velocity************************************//
Amber77 0:54f408fdeada 966 //****************************PIDcontrol_compute_velocity************************************//
Amber77 0:54f408fdeada 967 void PIDcontrol_compute_velocity(void)
Amber77 0:54f408fdeada 968 {
Amber77 0:54f408fdeada 969 if(command_AngularVel_X >= 0)
Amber77 0:54f408fdeada 970 {
Amber77 0:54f408fdeada 971 Control_X1_CW_CCW = 1;
Amber77 0:54f408fdeada 972 Control_X2_CW_CCW = 0;
Amber77 0:54f408fdeada 973 Command_AngularVel_X = command_AngularVel_X;
Amber77 0:54f408fdeada 974 }
Amber77 0:54f408fdeada 975 else
Amber77 0:54f408fdeada 976 {
Amber77 0:54f408fdeada 977 Control_X1_CW_CCW = 0;
Amber77 0:54f408fdeada 978 Control_X2_CW_CCW = 1;
Amber77 0:54f408fdeada 979 Command_AngularVel_X = -command_AngularVel_X;
Amber77 0:54f408fdeada 980 }
Amber77 0:54f408fdeada 981 if(command_AngularVel_Y >= 0)
Amber77 0:54f408fdeada 982 {
Amber77 0:54f408fdeada 983 Control_Y1_CW_CCW = 1;
Amber77 0:54f408fdeada 984 Control_Y2_CW_CCW = 0;
Amber77 0:54f408fdeada 985 Command_AngularVel_Y = command_AngularVel_Y;
Amber77 0:54f408fdeada 986 }
Amber77 0:54f408fdeada 987 else
Amber77 0:54f408fdeada 988 {
Amber77 0:54f408fdeada 989 Control_Y1_CW_CCW = 0;
Amber77 0:54f408fdeada 990 Control_Y2_CW_CCW = 1;
Amber77 0:54f408fdeada 991 Command_AngularVel_Y = -command_AngularVel_Y;
Amber77 0:54f408fdeada 992 }
Amber77 0:54f408fdeada 993 Now_time_PID=NowTime.read();
Amber77 0:54f408fdeada 994 Motor_X.Compute(&Now_time_PID);
Amber77 0:54f408fdeada 995 Motor_Y.Compute(&Now_time_PID);
Amber77 0:54f408fdeada 996 // Control_Motor_PWM_X = PWM_commmand_transformation(Control_motor_X);
Amber77 0:54f408fdeada 997 // Control_Motor_PWM_Y = PWM_commmand_transformation(Control_motor_Y);
Amber77 0:54f408fdeada 998 Control_Motor_PWM_X = PWM_commmand_transformation(command_AngularVel_X);
Amber77 0:54f408fdeada 999 Control_Motor_PWM_Y = PWM_commmand_transformation(command_AngularVel_Y);
Amber77 0:54f408fdeada 1000 }
Amber77 0:54f408fdeada 1001
Amber77 0:54f408fdeada 1002 //****************************Compute_point************************************//
Amber77 0:54f408fdeada 1003 //****************************Compute_point************************************//
Amber77 0:54f408fdeada 1004 void LQR_control_compute(void)
Amber77 0:54f408fdeada 1005 {
Amber77 0:54f408fdeada 1006 Diff_Roll = (Roll - Roll_last);
Amber77 0:54f408fdeada 1007 Diff_Pitch = (Pitch - Pitch_last);
Amber77 0:54f408fdeada 1008 Integ_Roll += Roll;
Amber77 0:54f408fdeada 1009 //printf("Diff_Roll:%.3f\n",Diff_Roll);
Amber77 0:54f408fdeada 1010 Integ_Pitch += Pitch;
Amber77 0:54f408fdeada 1011 Roll_last = Roll;
Amber77 0:54f408fdeada 1012 Pitch_last = Pitch;
Amber77 0:54f408fdeada 1013
Amber77 0:54f408fdeada 1014 //Diff_Roll =0;
Amber77 0:54f408fdeada 1015 //Roll -= 2.45;
Amber77 0:54f408fdeada 1016 //Pitch -=2.5;
Amber77 0:54f408fdeada 1017 Diff_x = x_now - x_trajectory;
Amber77 0:54f408fdeada 1018 Diff_y = y_now - y_trajectory;
Amber77 0:54f408fdeada 1019 dot_diff_x = Diff_x - Diff_x_pre;
Amber77 0:54f408fdeada 1020 dot_diff_y = Diff_y - Diff_y_pre;
Amber77 0:54f408fdeada 1021 Integ_x += Diff_x;
Amber77 0:54f408fdeada 1022 Integ_y += Diff_y;
Amber77 0:54f408fdeada 1023
Amber77 0:54f408fdeada 1024 //x_pre_1 = x_now;
Amber77 0:54f408fdeada 1025 //y_pre_1 = y_now;
Amber77 0:54f408fdeada 1026 Diff_x_pre = Diff_x;
Amber77 0:54f408fdeada 1027 Diff_y_pre = Diff_y;
Amber77 0:54f408fdeada 1028
Amber77 0:54f408fdeada 1029 // u_x = Ka*(Roll)+ Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x;
Amber77 0:54f408fdeada 1030 // u_y = Ka*(Pitch)+Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y;
Amber77 0:54f408fdeada 1031 u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x;
Amber77 0:54f408fdeada 1032 // u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll;
Amber77 0:54f408fdeada 1033 //u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll - Kt*(10) - Kv*0;
Amber77 0:54f408fdeada 1034 // u_x = Ka*(Roll);
Amber77 0:54f408fdeada 1035 u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y;
Amber77 0:54f408fdeada 1036 // u_y = Ka*(Pitch)+(Integ_Roll*Kii) + Kav*Diff_Roll;
Amber77 0:54f408fdeada 1037 //u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch - Kt*0 - Kv*0;
Amber77 0:54f408fdeada 1038 // u_y = Ka*(Pitch);
Amber77 0:54f408fdeada 1039 if(u_x > 580)
Amber77 0:54f408fdeada 1040 {
Amber77 0:54f408fdeada 1041 u_x = 580;
Amber77 0:54f408fdeada 1042 }
Amber77 0:54f408fdeada 1043 else if(u_x < -580)
Amber77 0:54f408fdeada 1044 {
Amber77 0:54f408fdeada 1045 u_x = -580;
Amber77 0:54f408fdeada 1046 }
Amber77 0:54f408fdeada 1047 if(u_y > 580 )
Amber77 0:54f408fdeada 1048 {
Amber77 0:54f408fdeada 1049 u_y = 580;
Amber77 0:54f408fdeada 1050 }
Amber77 0:54f408fdeada 1051 else if(u_y < -580)
Amber77 0:54f408fdeada 1052 {
Amber77 0:54f408fdeada 1053 u_y = -580;
Amber77 0:54f408fdeada 1054 }
Amber77 0:54f408fdeada 1055 Vx = u_x;
Amber77 0:54f408fdeada 1056 Vy = u_y;
Amber77 0:54f408fdeada 1057 Wz = Yaw;
Amber77 0:54f408fdeada 1058
Amber77 0:54f408fdeada 1059 // Vx = 100;
Amber77 0:54f408fdeada 1060 // Vy = 100;
Amber77 0:54f408fdeada 1061 // Wz =0;
Amber77 0:54f408fdeada 1062 command_AngularVel_X = Vx+Kz*Wz;
Amber77 0:54f408fdeada 1063 command_AngularVel_Y = Vy+Kz*Wz;
Amber77 0:54f408fdeada 1064 }
Amber77 0:54f408fdeada 1065
Amber77 0:54f408fdeada 1066 //***********************************************************//
Amber77 0:54f408fdeada 1067 //
Amber77 0:54f408fdeada 1068 ////若要前進，則在x或y加減p_set。根據底盤的正負標識，來決定加或減。
Amber77 0:54f408fdeada 1069 //
Amber77 0:54f408fdeada 1070 //
Amber77 0:54f408fdeada 1071 // /*v_dx_new=0.4*(1*Kp3*(err_tiltX)+(1.9*Kd3*1)*(err_gyroX) +(x)*Kp3*19 +vel_x*Kd3*15-0*v_dx);
Amber77 0:54f408fdeada 1072 // v_dy_new=0.4*(1*Kp3*(err_tiltY)+(1.9*Kd3*1)*(err_gyroY) +(y)*Kp3*19 +(vel_y)*Kd3*15-0*v_dy);*/
Amber77 0:54f408fdeada 1073 // // x_u=12.5*1.6666*v_d_gain*(1*Kp3*(err_tiltX)+(1.9*Kd3*1)*(err_gyroX) +(x)*Kp3*0 +vel_x*Kd3*0-0*x_iu);
Amber77 0:54f408fdeada 1074 // // y_u=12.5*1.6666*v_d_gain*(1*Kp3*(err_tiltY)+(1.9*Kd3*1)*(err_gyroY) +(y)*Kp3*0 +vel_y*Kd3*0-0*y_iu);
Amber77 0:54f408fdeada 1075 // /*x_u=6.25*v_d_gain*( -Kp3*err_tiltX+(-1.9*Kd3)*err_gyroX +(-0.001*0*Kp3)*x +(-0.018*0*Kp3)*vel_x-0*x_iu);
Amber77 0:54f408fdeada 1076 // y_u=6.25*v_d_gain*( -Kp3*err_tiltY+(-1.9*Kd3)*err_gyroY +(-0.001*0*Kp3)*y +(-0.018*0*Kp3)*vel_y-0*y_iu);
Amber77 0:54f408fdeada 1077 //*/
Amber77 0:54f408fdeada 1078 // // integral U for PI close loop
Amber77 0:54f408fdeada 1079 // x_iu=x_u*T2+x_iu_old; //integral value of v_dx
Amber77 0:54f408fdeada 1080 // y_iu=y_u*T2+y_iu_old;
Amber77 0:54f408fdeada 1081 //
Amber77 0:54f408fdeada 1082 // x_iu_old=x_iu;
Amber77 0:54f408fdeada 1083 // y_iu_old=y_iu;
Amber77 0:54f408fdeada 1084 //
Amber77 0:54f408fdeada 1085 // // dV cmd(U cmd) PI close loop (like low-pass filter multip a gain value)
Amber77 0:54f408fdeada 1086 // C_theta1_d=(8)*((x_u-vel_x*0.3*C_theta_gain1)+(C_theta_gain2)*(x_iu*0.24- x*0.3 )+fc*vel_x);
Amber77 0:54f408fdeada 1087 // C_theta2_d=(8)*((y_u-vel_y*0.3*C_theta_gain1)+(C_theta_gain2)*(y_iu*0.24- y*0.3 )+fc*vel_y);
Amber77 0:54f408fdeada 1088 //
Amber77 0:54f408fdeada 1089 // // Vx = integral dVx
Amber77 0:54f408fdeada 1090 //
Amber77 0:54f408fdeada 1091 // C_theta1=C_theta1_d*T2+C_theta1_old;
Amber77 0:54f408fdeada 1092 // // limit
Amber77 0:54f408fdeada 1093 // C_theta1= C_theta1> (5000) ? (5000): C_theta1;
Amber77 0:54f408fdeada 1094 // C_theta1= C_theta1< (-5000) ? (-5000): C_theta1;
Amber77 0:54f408fdeada 1095 //
Amber77 0:54f408fdeada 1096 // C_theta1_old=C_theta1;
Amber77 0:54f408fdeada 1097 // // integral Vx
Amber77 0:54f408fdeada 1098 // C_theta1_i=C_theta1*T2+C_theta1_i;
Amber77 0:54f408fdeada 1099 //
Amber77 0:54f408fdeada 1100 // // Vy = integral dVy
Amber77 0:54f408fdeada 1101 // C_theta2=C_theta2_d*T2+C_theta2_old;
Amber77 0:54f408fdeada 1102 // // limit
Amber77 0:54f408fdeada 1103 // C_theta2= C_theta2>(5000) ? (5000): C_theta2;
Amber77 0:54f408fdeada 1104 // C_theta2= C_theta2<(-5000) ? (-5000):C_theta2;
Amber77 0:54f408fdeada 1105 //
Amber77 0:54f408fdeada 1106 // C_theta2_old=C_theta2;
Amber77 0:54f408fdeada 1107 // // integral Vy
Amber77 0:54f408fdeada 1108 // C_theta2_i=C_theta2*T2+C_theta2_i;
Amber77 0:54f408fdeada 1109 //
Amber77 0:54f408fdeada 1110 // // speed PI LOOP control
Amber77 0:54f408fdeada 1111 //
Amber77 0:54f408fdeada 1112 // C_thetaX=Kcp*(C_theta1-vel_x*Cgain1)+Kci*(C_theta1_i-x*Cgain2); //X axis
Amber77 0:54f408fdeada 1113 // C_thetaY=Kcp*(C_theta2-vel_y*Cgain1)+Kci*(C_theta2_i-y*Cgain2); //Y axis
Amber77 0:54f408fdeada 1114 //
Amber77 0:54f408fdeada 1115 // #endif
Amber77 0:54f408fdeada 1116 // #if Testmode == 1 // nonlinear
Amber77 0:54f408fdeada 1117 //
Amber77 0:54f408fdeada 1118 ////########### x 方向之控制器#############
Amber77 0:54f408fdeada 1119 // r1=1;
Amber77 0:54f408fdeada 1120 // r2=1;
Amber77 0:54f408fdeada 1121 // err_tilt= 0+tilt2*0.0006086;/*tilt_out;傾斜儀資料 */
Amber77 0:54f408fdeada 1122 // err_gyro= 0+gyro2;/*陀螺儀資料*/
Amber77 0:54f408fdeada 1123 // sin_t=sin(err_tilt);
Amber77 0:54f408fdeada 1124 // cos_t=cos(err_tilt);
Amber77 0:54f408fdeada 1125 // err_tilt_i=err_tilt_i+err_tilt*0.1;
Amber77 0:54f408fdeada 1126 // x_i=x_i+x*0.001;
Amber77 0:54f408fdeada 1127 // //err_tilt_i+=err_tilt;
Amber77 0:54f408fdeada 1128 // //x_i+=x;
Amber77 0:54f408fdeada 1129 // delta=187.5-125.3*cos_t;
Amber77 0:54f408fdeada 1130 // A=(5.51+33.75*cos_t)/delta;
Amber77 0:54f408fdeada 1131 // B=-((125.3*sin_t*cos_t)/delta)*err_gyro*err_gyro+182.2*sin_t/delta;
Amber77 0:54f408fdeada 1132 //// B=-((125.3*sin_t*cos_t)/delta)*err_gyro*err_gyro+182.2*sin_t/delta-5.5091*((err_gyro+vel_x/0.11))/187.5-125.3*cos_t*cos_t;
Amber77 0:54f408fdeada 1133 // C=Kp3*(eta1-Kp3*err_tilt-Ki3*err_tilt_i)+Ki3*err_tilt;
Amber77 0:54f408fdeada 1134 // D=-(5.51+33.75*cos_t)/delta;
Amber77 0:54f408fdeada 1135 // E=126.4/delta*err_gyro*err_gyro*sin_t-1637/delta*sin_t*cos_t;
Amber77 0:54f408fdeada 1136 // F=Kp3*(eta2-Kp3*x-Ki3*x_i)+Ki3*x;
Amber77 0:54f408fdeada 1137 ////F=Kp3*(eta2-Kp3*err_tilt-Ki3*0)+Ki3*0;
Amber77 0:54f408fdeada 1138 // eta1=err_gyro+Kp3*err_tilt+Ki3*err_tilt_i;
Amber77 0:54f408fdeada 1139 // eta2=vel_x+Kp3*x+Ki3*x_i;
Amber77 0:54f408fdeada 1140 ////eta2=0;
Amber77 0:54f408fdeada 1141 // s=r1*eta1+r2*eta2;
Amber77 0:54f408fdeada 1142 // if(s>=1) sgns=1;
Amber77 0:54f408fdeada 1143 // else if(s<=-1) sgns=-1;
Amber77 0:54f408fdeada 1144 // else sgns=s;
Amber77 0:54f408fdeada 1145 //
Amber77 0:54f408fdeada 1146 // if (s*abs(vel_x)>=1) sgns_c=1;
Amber77 0:54f408fdeada 1147 // else if(s*abs(vel_x)<=-1) sgns_c=-1;
Amber77 0:54f408fdeada 1148 // else sgns_c=s*abs(vel_x);
Amber77 0:54f408fdeada 1149 ////########### y 方向之控制器#############
Amber77 0:54f408fdeada 1150 // err_tilt2= 0+tilt5*0.0006086;/*tilt_out;傾斜儀資料 */
Amber77 0:54f408fdeada 1151 // err_gyro2= 0+gyro5;/*陀螺儀資料*/
Amber77 0:54f408fdeada 1152 // sin_t2=sin(err_tilt2);
Amber77 0:54f408fdeada 1153 // cos_t2=cos(err_tilt2);
Amber77 0:54f408fdeada 1154 // err_tilt2_i=err_tilt2_i+err_tilt2*0.1;
Amber77 0:54f408fdeada 1155 // y_i=y_i+y*0.001;
Amber77 0:54f408fdeada 1156 ////err_tilt2_i+=err_tilt2;
Amber77 0:54f408fdeada 1157 ////y_i+=y;
Amber77 0:54f408fdeada 1158 // delta2=187.5-125.3*cos_t2;
Amber77 0:54f408fdeada 1159 // A2=(5.51+33.75*cos_t2)/delta2;
Amber77 0:54f408fdeada 1160 // B2=-((125.3*sin_t2*cos_t2)/delta2)*err_gyro2*err_gyro2+182.2*sin_t2/delta2;
Amber77 0:54f408fdeada 1161 // C2=Kp3*(eta3-Kp3*err_tilt2-Ki3*err_tilt2_i)+Ki3*err_tilt2;
Amber77 0:54f408fdeada 1162 // D2=-(5.51+33.75*cos_t2)/delta2;
Amber77 0:54f408fdeada 1163 // E2=126.4/delta2*err_gyro2*err_gyro2*sin_t2-1637/delta2*sin_t2*cos_t2;
Amber77 0:54f408fdeada 1164 // F2=Kp3*(eta4-Kp3*y-Ki3*y_i)+Ki3*y;
Amber77 0:54f408fdeada 1165 ////F2=Kp3*(eta4-Kp3*err_tilt2-Ki3*0)+Ki3*0;
Amber77 0:54f408fdeada 1166 // eta3=err_gyro2+Kp3*err_tilt2+Ki3*err_tilt2_i;
Amber77 0:54f408fdeada 1167 // eta4=vel_y+Kp3*y+Ki3*y_i;
Amber77 0:54f408fdeada 1168 ////eta4=0;
Amber77 0:54f408fdeada 1169 // s2=r1*eta3+r2*eta4;
Amber77 0:54f408fdeada 1170 // if(s2>=1) sgns2=1;
Amber77 0:54f408fdeada 1171 // else if(s2<=-1) sgns2=-1;
Amber77 0:54f408fdeada 1172 // else sgns2=s2;
Amber77 0:54f408fdeada 1173 //
Amber77 0:54f408fdeada 1174 // if(s2*abs(vel_y)>=1) sgns2_c=1;
Amber77 0:54f408fdeada 1175 // else if(s2*abs(vel_y)<=-1) sgns2_c=-1;
Amber77 0:54f408fdeada 1176 // else sgns2_c=s2*abs(vel_y);
Amber77 0:54f408fdeada 1177 //
Amber77 0:54f408fdeada 1178 ////若要前進，則在x或y加減p_set。根據底盤的正負標識，來決定加或減。
Amber77 0:54f408fdeada 1179 // C_theta1_new=((r1*B+r1*C+r2*E+r2*F)+s+sgns+sgns_c)/(r1*A+r2*D);
Amber77 0:54f408fdeada 1180 // C_theta2_new=((r1*B2+r1*C2+r2*E2+r2*F2)+s2+sgns2+sgns2_c)/(r1*A2+r2*D2);
Amber77 0:54f408fdeada 1181 // }
Amber77 0:54f408fdeada 1182 // i++;
Amber77 0:54f408fdeada 1183 //
Amber77 0:54f408fdeada 1184 // C_theta1=C_theta1_new*0.00050+C_theta1_old;
Amber77 0:54f408fdeada 1185 // C_theta1_old=C_theta1;
Amber77 0:54f408fdeada 1186 //
Amber77 0:54f408fdeada 1187 //
Amber77 0:54f408fdeada 1188 // C_theta2=C_theta2_new*0.00050+C_theta2_old;
Amber77 0:54f408fdeada 1189 // C_theta2_old=C_theta2;
Amber77 0:54f408fdeada 1190 //
Amber77 0:54f408fdeada 1191 // C_theta1= C_theta1>(42143)?(42143): C_theta1;
Amber77 0:54f408fdeada 1192 // C_theta1= C_theta1<-1*(42143)?-1*(42143): C_theta1;
Amber77 0:54f408fdeada 1193 // C_theta2= C_theta2>(42143)?(42143): C_theta2;
Amber77 0:54f408fdeada 1194 // C_theta2=C_theta2<-1*(42143)?-1*(42143):C_theta2;
Amber77 0:54f408fdeada 1195 //
Amber77 0:54f408fdeada 1196 // #endif
Amber77 0:54f408fdeada 1197
Amber77 0:54f408fdeada 1198 //***********************************************************//
Amber77 0:54f408fdeada 1199
Amber77 0:54f408fdeada 1200
Amber77 0:54f408fdeada 1201 void MeasureRobotAttitudeAngle(void)
Amber77 0:54f408fdeada 1202 {
Amber77 0:54f408fdeada 1203 dt = t_MeasureRobotAttitudeAngle;
Amber77 0:54f408fdeada 1204 imu.read_all();
Amber77 0:54f408fdeada 1205 Mag_Complentary_Filter(dt,imu.Magnetometer);
Amber77 0:54f408fdeada 1206 Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
Amber77 0:54f408fdeada 1207 /*x_now = x_pre_1 + (1/(dt*dt))*(ax_now-2*ax_pre_1+ax_pre_2);
Amber77 0:54f408fdeada 1208 y_now = y_pre_1 + (1/(dt*dt))*(ay_now-2*ay_pre_1+ay_pre_2);
Amber77 0:54f408fdeada 1209 x_pre_1 = x_now;
Amber77 0:54f408fdeada 1210 y_pre_1 = y_now;*/
Amber77 0:54f408fdeada 1211 x_now = r_grounder*pi*(Angle_X/180);
Amber77 0:54f408fdeada 1212 y_now = r_grounder*pi*(Angle_Y/180);
Amber77 0:54f408fdeada 1213 // x_now = r_ball*pi*(Angle_X/180);
Amber77 0:54f408fdeada 1214 // y_now = r_ball*pi*(Angle_Y/180);
Amber77 0:54f408fdeada 1215 do_measure_index++;
Amber77 0:54f408fdeada 1216 }
Amber77 0:54f408fdeada 1217 //void Trajectory_tracking(void)
Amber77 0:54f408fdeada 1218 //{
Amber77 0:54f408fdeada 1219 // double t_trajectory = NowTime.read();
Amber77 0:54f408fdeada 1220 // if( RunTime>=0 &&( RunTime<=10))
Amber77 0:54f408fdeada 1221 // {
Amber77 0:54f408fdeada 1222 // //x_trajectory = t_trajectory * (0.5);
Amber77 0:54f408fdeada 1223 // x_trajectory = 0;
Amber77 0:54f408fdeada 1224 // y_trajectory = 0;
Amber77 0:54f408fdeada 1225 // }
Amber77 0:54f408fdeada 1226 // else if( RunTime>10 )
Amber77 0:54f408fdeada 1227 // {
Amber77 0:54f408fdeada 1228 // x_trajectory = 0;
Amber77 0:54f408fdeada 1229 // y_trajectory = 0;
Amber77 0:54f408fdeada 1230 // }
Amber77 0:54f408fdeada 1231 //}
Amber77 0:54f408fdeada 1232 //****************************main function************************************//
Amber77 0:54f408fdeada 1233 int main()
Amber77 0:54f408fdeada 1234 {
Amber77 0:54f408fdeada 1235 // pc.baud(115200);
Amber77 0:54f408fdeada 1236 pc.baud(230400);
Amber77 0:54f408fdeada 1237
Amber77 0:54f408fdeada 1238 //****************************Motor driver declare************************************//
Amber77 0:54f408fdeada 1239 //ExtInt = 0; //number high, voltage high.
Amber77 0:54f408fdeada 1240 //Control_stopRun=0;
Amber77 0:54f408fdeada 1241 // Control_brake=1;
Amber77 0:54f408fdeada 1242 Control_AR=1;
Amber77 0:54f408fdeada 1243 Control_H_F=1;
Amber77 0:54f408fdeada 1244 H_F.write(Control_H_F); // control_H_F must equal to 1
Amber77 0:54f408fdeada 1245 // StopRun.write(Control_stopRun);
Amber77 0:54f408fdeada 1246 // Brake.write(Control_brake);
Amber77 0:54f408fdeada 1247 // AR.write(Control_AR);
Amber77 0:54f408fdeada 1248 //****************************Angle Sensor initialization************************************//
Amber77 0:54f408fdeada 1249 if(imu.init(1,BITS_DLPF_CFG_188HZ))
Amber77 0:54f408fdeada 1250 { //INIT the mpu9250
Amber77 0:54f408fdeada 1251 // pc.printf("\nCouldn't initialize MPU9250 via SPI!");
Amber77 0:54f408fdeada 1252 }
Amber77 0:54f408fdeada 1253 imu.whoami();
Amber77 0:54f408fdeada 1254 // wait(0.1);
Amber77 0:54f408fdeada 1255 imu.set_gyro_scale(BITS_FS_2000DPS);
Amber77 0:54f408fdeada 1256 // wait(0.1);
Amber77 0:54f408fdeada 1257 imu.set_acc_scale(BITS_FS_16G);
Amber77 0:54f408fdeada 1258 // wait(0.1);
Amber77 0:54f408fdeada 1259 imu.AK8963_whoami();
Amber77 0:54f408fdeada 1260 // wait(0.1);
Amber77 0:54f408fdeada 1261 imu.AK8963_calib_Magnetometer();
Amber77 0:54f408fdeada 1262 ////****************************SD card************************************//
Amber77 0:54f408fdeada 1263 mkdir("/sd/Amber20170822a", 0777); //SD裡面的資料夾叫Amber77,在此做宣告
Amber77 0:54f408fdeada 1264 FILE *fp = fopen("/sd/Amber20170822a/RollPitchYaw_y1.csv", "a");
Amber77 0:54f408fdeada 1265 //將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv
Amber77 0:54f408fdeada 1266 fprintf(fp,"RunTime,Roll,Pitch,Yaw,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory\n");
Amber77 0:54f408fdeada 1267
Amber77 0:54f408fdeada 1268 //**************************** PWM ************************************//
Amber77 0:54f408fdeada 1269 // X_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
Amber77 0:54f408fdeada 1270 // Y_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
Amber77 0:54f408fdeada 1271 // X_PWM.calibrate(0.02, 0*0.02, 1*0.02);
Amber77 0:54f408fdeada 1272 // Y_PWM.calibrate(0.02, 0*0.02, 1*0.02);
Amber77 0:54f408fdeada 1273 X_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ
Amber77 0:54f408fdeada 1274 Y_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ
Amber77 0:54f408fdeada 1275 //**************************** Encoder ************************************//
Amber77 0:54f408fdeada 1276 phaseA_1.mode( PullUp );
Amber77 0:54f408fdeada 1277 phaseB_1.mode( PullUp );
Amber77 0:54f408fdeada 1278 phaseA_2.mode( PullUp );
Amber77 0:54f408fdeada 1279 phaseB_2.mode( PullUp );
Amber77 0:54f408fdeada 1280 phaseA_3.mode( PullUp );
Amber77 0:54f408fdeada 1281 phaseB_3.mode( PullUp );
Amber77 0:54f408fdeada 1282 phaseA_4.mode( PullUp );
Amber77 0:54f408fdeada 1283 phaseB_4.mode( PullUp );
Amber77 0:54f408fdeada 1284 //**************************** Create the motor encoder sampler. ************************************//
Amber77 0:54f408fdeada 1285 Sample_Motor_encoder.attach_us( &quadratureDecoder, t_quadratureDecoder );
Amber77 0:54f408fdeada 1286 //**************************** Create the motor encoder sampler. ************************************//
Amber77 0:54f408fdeada 1287 Sample_robotAngleSensor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle );
Amber77 0:54f408fdeada 1288 //**************************** Create the motor angular velocity measurement. ************************************//
Amber77 0:54f408fdeada 1289 MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity );
Amber77 0:54f408fdeada 1290 //**************************** Create the motor angular velocity PID control. ************************************//
Amber77 0:54f408fdeada 1291 PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity );
Amber77 0:54f408fdeada 1292 //**************************** Create the robot LQR control. ************************************//
Amber77 0:54f408fdeada 1293 LQR_control.attach( &LQR_control_compute, t_LQR_control );
Amber77 0:54f408fdeada 1294 //**************************** Trajectory tracking control ************************************//
Amber77 0:54f408fdeada 1295 // TrajectoryTracking_control.attach( &Trajectory_tracking, t_trajectory );
Amber77 0:54f408fdeada 1296 //**************************** Motor settlement ************************************//
Amber77 0:54f408fdeada 1297 Motor_X.SetMode(AUTOMATIC);
Amber77 0:54f408fdeada 1298 Motor_Y.SetMode(AUTOMATIC);
Amber77 0:54f408fdeada 1299 // Command_AngularVel_X = 0;
Amber77 0:54f408fdeada 1300 // Command_AngularVel_Y = 0;
Amber77 0:54f408fdeada 1301 //**************************** Timers start ************************************//
Amber77 0:54f408fdeada 1302 NowTime.start();
Amber77 0:54f408fdeada 1303 while( 1 )
Amber77 0:54f408fdeada 1304 {
Amber77 0:54f408fdeada 1305 RunTime = NowTime.read();
Amber77 0:54f408fdeada 1306 t_trajectory = NowTime.read();
Amber77 0:54f408fdeada 1307 limitSwitchUp = LimitSwitchUp.read();
Amber77 0:54f408fdeada 1308 limitSwitchDown = LimitSwitchDown.read();
Amber77 0:54f408fdeada 1309 //pc.printf("%d \n",Control_F_R);
Amber77 0:54f408fdeada 1310 if (mybutton == 0)
Amber77 0:54f408fdeada 1311 {
Amber77 0:54f408fdeada 1312 if( Control_F_R == 1)
Amber77 0:54f408fdeada 1313 {
Amber77 0:54f408fdeada 1314 Control_LiftingStopRun=0;
Amber77 0:54f408fdeada 1315 LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop
Amber77 0:54f408fdeada 1316 F_R.write(Control_F_R); // 0:turn down 1:turn up
Amber77 0:54f408fdeada 1317 Control_stopRun=0;
Amber77 0:54f408fdeada 1318 Control_brake=1;
Amber77 0:54f408fdeada 1319 Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
Amber77 0:54f408fdeada 1320 wait(3);
Amber77 0:54f408fdeada 1321 }
Amber77 0:54f408fdeada 1322 else if ( Control_F_R == 0)
Amber77 0:54f408fdeada 1323 {
Amber77 0:54f408fdeada 1324 Control_LiftingStopRun=0;
Amber77 0:54f408fdeada 1325 LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop
Amber77 0:54f408fdeada 1326 F_R.write(Control_F_R); // 0:turn down 1:turn up
Amber77 0:54f408fdeada 1327 Control_stopRun=0;
Amber77 0:54f408fdeada 1328 Control_brake=1;
Amber77 0:54f408fdeada 1329 Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
Amber77 0:54f408fdeada 1330 wait(2);
Amber77 0:54f408fdeada 1331 Control_stopRun=1;
Amber77 0:54f408fdeada 1332 Control_brake=1;
Amber77 0:54f408fdeada 1333 Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
Amber77 0:54f408fdeada 1334 wait(1);
Amber77 0:54f408fdeada 1335 }
Amber77 0:54f408fdeada 1336 Control_F_R=1-Control_F_R;
Amber77 0:54f408fdeada 1337 wait(0.2);
Amber77 0:54f408fdeada 1338 }
Amber77 0:54f408fdeada 1339 if(limitSwitchUp == 0)
Amber77 0:54f408fdeada 1340 {
Amber77 0:54f408fdeada 1341 LiftingStopRun.write(1); // 0:Run 1:Stop
Amber77 0:54f408fdeada 1342 F_R.write(1); // 0:turn down 1:turn up
Amber77 0:54f408fdeada 1343 }
Amber77 0:54f408fdeada 1344 if(limitSwitchDown == 0)
Amber77 0:54f408fdeada 1345 {
Amber77 0:54f408fdeada 1346 LiftingStopRun.write(1); // 0:Run 1:Stop
Amber77 0:54f408fdeada 1347 F_R.write(0); // 0:turn down 1:turn up
Amber77 0:54f408fdeada 1348 }
Amber77 0:54f408fdeada 1349 if( RunTime>=0 &&( RunTime<=10))
Amber77 0:54f408fdeada 1350 {
Amber77 0:54f408fdeada 1351 //x_trajectory = t_trajectory * (0.5);
Amber77 0:54f408fdeada 1352 //Control_stopRun=0;
Amber77 0:54f408fdeada 1353 // Control_brake=1;
Amber77 0:54f408fdeada 1354 x_trajectory = 0;
Amber77 0:54f408fdeada 1355 y_trajectory = 0;
Amber77 0:54f408fdeada 1356 }
Amber77 0:54f408fdeada 1357 else if( RunTime>10 && (RunTime <=20 ))
Amber77 0:54f408fdeada 1358 {
Amber77 0:54f408fdeada 1359 //Control_stopRun=0;
Amber77 0:54f408fdeada 1360 // Control_brake=1;
Amber77 0:54f408fdeada 1361 x_trajectory = 0;
Amber77 0:54f408fdeada 1362 y_trajectory = 0;
Amber77 0:54f408fdeada 1363 }
Amber77 0:54f408fdeada 1364 else if( RunTime>=20 )
Amber77 0:54f408fdeada 1365 {
Amber77 0:54f408fdeada 1366 //Control_stopRun=0;
Amber77 0:54f408fdeada 1367 // Control_brake=1;
Amber77 0:54f408fdeada 1368 x_trajectory = 0;
Amber77 0:54f408fdeada 1369 y_trajectory = 0;
Amber77 0:54f408fdeada 1370 }
Amber77 0:54f408fdeada 1371
Amber77 0:54f408fdeada 1372
Amber77 0:54f408fdeada 1373 if( (RunTime-lastTime) > 0.1 )
Amber77 0:54f408fdeada 1374 {
Amber77 0:54f408fdeada 1375 index_times++;
Amber77 0:54f408fdeada 1376 lastTime = RunTime;
Amber77 0:54f408fdeada 1377 }
Amber77 0:54f408fdeada 1378
Amber77 0:54f408fdeada 1379 if( index_times >= 0.1 )
Amber77 0:54f408fdeada 1380 {
Amber77 0:54f408fdeada 1381 /*pc.printf("x_now: %.3f , y_now: %.3f , x_trajectory: %f , u_y: %f ",x_now,y_now,x_trajectory,u_y);
Amber77 0:54f408fdeada 1382 pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f \n",
Amber77 0:54f408fdeada 1383 Roll,
Amber77 0:54f408fdeada 1384 Pitch,
Amber77 0:54f408fdeada 1385 Yaw
Amber77 0:54f408fdeada 1386 );*/
Amber77 0:54f408fdeada 1387 //pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory);
Amber77 0:54f408fdeada 1388 //pc.printf("RunTime=%10.3f || Roll=%10.3f || Pitch=%10.3f || Yaw=%10.3f \n",RunTime, Roll, Pitch, Yaw);
Amber77 0:54f408fdeada 1389 // pc.printf("Vx=%10.3f || Vy=%10.3f || PWM_X=%10.3f || PWM_Y=%10.3f || x_now=%10.3f || y_now=%10.3f || Angle_X=%10.3f || Angle_Y=%10.3f\n",
Amber77 0:54f408fdeada 1390 // Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_now,y_now,Angle_X,Angle_Y);
Amber77 0:54f408fdeada 1391 pc.printf("RunTime=%.3f || Roll=%.3f || Pitch=%.3f || Yaw=%.3f \n",RunTime, Roll, Pitch, Yaw);
Amber77 0:54f408fdeada 1392 pc.printf("Vx=%.3f || Vy=%.3f || PWM_X=%.3f || PWM_Y=%.3f || x_now=%.3f || y_now=%.3f || Angle_X=%.3f || Angle_Y=%.3f\n\n\n",
Amber77 0:54f408fdeada 1393 Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory,Angle_X,Angle_Y);
Amber77 0:54f408fdeada 1394 //pc.printf(" Now_angularVelocity : %.3f ",Now_angularVelocity);
Amber77 0:54f408fdeada 1395 // pc.printf(",RunTime : %.3f ", RunTime );
Amber77 0:54f408fdeada 1396 // pc.printf(",control_PWM_Value : %.3f \n", control_PWM_Value);
Amber77 0:54f408fdeada 1397 index_times = 0;
Amber77 0:54f408fdeada 1398 //if(fp == NULL)
Amber77 0:54f408fdeada 1399 // {
Amber77 0:54f408fdeada 1400 // error("Could not open file for write\n");
Amber77 0:54f408fdeada 1401 // }
Amber77 0:54f408fdeada 1402 // //fprintf(fp,"%.3f,%.3f,%.3f\n", RunTime,Now_angularVelocity,control_PWM_Value);
Amber77 0:54f408fdeada 1403 fprintf(fp,"%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f \n", RunTime,Roll,Pitch,Yaw,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y, x_trajectory,y_trajectory);
Amber77 0:54f408fdeada 1404 }
Amber77 0:54f408fdeada 1405 // Lifting(Control_LiftingStopRun, Control_F_R , Control_H_F);
Amber77 0:54f408fdeada 1406 Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
Amber77 0:54f408fdeada 1407 //wait(0.1);
Amber77 0:54f408fdeada 1408 // if(!mybutton)
Amber77 0:54f408fdeada 1409 // {
Amber77 0:54f408fdeada 1410 // StopRun.write(1);
Amber77 0:54f408fdeada 1411 // Brake.write(1);
Amber77 0:54f408fdeada 1412 // X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:54f408fdeada 1413 // X2_CW_CCW.write(0);
Amber77 0:54f408fdeada 1414 // Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:54f408fdeada 1415 // Y2_CW_CCW.write(0);
Amber77 0:54f408fdeada 1416 // X_PWM.write(0); // clockwise:0 counterclockwise:1
Amber77 0:54f408fdeada 1417 // Y_PWM.write(0);
Amber77 0:54f408fdeada 1418 // break;
Amber77 0:54f408fdeada 1419 // }
Amber77 0:54f408fdeada 1420 }
Amber77 0:54f408fdeada 1421 fclose(fp);
Amber77 0:54f408fdeada 1422 }
Amber77 0:54f408fdeada 1423