Amber Tang / Mbed 2 deprecated testSSWMR_StationKeeping_200170830_OK

20170830a

Dependencies:   MPU9250_SPI PID SDFileSystem01 Servo mbed

Fork of testSSWMR_StationKeeping_20170830 by Amber Tang

main.cpp@0:038acacdae04, 2017-08-30 (annotated)

Committer:
Amber77
Date:
Wed Aug 30 01:59:06 2017 +0000
Revision:
0:038acacdae04
20170830

Who changed what in which revision?

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