![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20170830
Dependencies: MPU9250_SPI PID02 SDFileSystem02 Servo mbed
Revision 0:67bd1a7efc59, committed 2017-08-30
- Comitter:
- Amber77
- Date:
- Wed Aug 30 02:10:02 2017 +0000
- Commit message:
- 20170830
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250.lib Wed Aug 30 02:10:02 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/kylongmu/code/MPU9250_SPI/#084e8ba240c1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Wed Aug 30 02:10:02 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/Amber77/code/PID02/#32c9f196607d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Wed Aug 30 02:10:02 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/Amber77/code/SDFileSystem02/#ab75665fac6f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed Aug 30 02:10:02 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/andrewrussell/code/Servo/#4c315bcd91b4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Aug 30 02:10:02 2017 +0000 @@ -0,0 +1,1423 @@ +#include "mbed.h" +#include "PID.h" +#include "Servo.h" +#include "MPU9250.h" +#include "SDFileSystem.h" +/* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */ +/* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */ +// SDFileSystem---1. https://developer.mbed.org/users/mbed_official/code/SDFileSystem/ +// 2. https://developer.mbed.org/users/simon/code/SDCardTest/ +// SD care---3.3v +// Ctrl+? =>註解 +#define pi 3.14159265359 +//**************************** PID control para declaration ************************************// +#define RATE 0.005 +#define Kp_1 1.1 //1.1 +#define Ki_1 0.18 //0.2 +#define Kd_1 0.001 +//**************************** Angle Sonsor para declaration ************************************// +#define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer +#define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases +//**************************** pragram debug declaration ************************************// +int index_times = 0; +double RunTime =0,lastTime =0; +double t_trajectory=0; +int do_measure_index=0; + +double t_MeasureAngularVelocity=0.03; +double t_PIDcontrol_velocity =0.03; +double t_LQR_control = 0.05; +double t_MeasureRobotAttitudeAngle = 0.05; +double t_quadratureDecoder = 15; +//double t_TrajectoryTracking = 0.1; +//**************************** Motor para declaration ************************************// +double r_ball = 0.1; // 球的半徑 +double r_grounder = 0.018; //滾球的半徑 +//**************************** Ticker ************************************// +Serial pc(USBTX, USBRX); +Ticker Sample_Motor_encoder; // create a timer to sample the encoder. +Ticker Sample_robotAngleSensor; // create a timer to sample the robot attitude. +Ticker MeasureAngularVelocity; // create a timer to measure the motor angular velocity. +Ticker PIDcontrol_velocity; // create a timer to do the motor angular velocity PID control. +Ticker LQR_control; // create a timer to do the position control. +//Ticker TrajectoryTracking_control; +//**************************** Measuremant of angular velocity declaration ************************************// +double Angle_1 =0,Angle_2 =0,Angle_3 =0,Angle_4 =0; +double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0,LastAngle_4 =0; +double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4; +double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4; +double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4; +double _4_SectionAngle=0,_4_LastSectionAngle,_4_LastSectionAngle_1,_4_LastSectionAngle_2,_4_LastSectionAngle_3,_4_LastSectionAngle_4; +double Average_SectionAngle_1,Now_angularVelocity_1=0; +double Average_SectionAngle_2,Now_angularVelocity_2=0; +double Average_SectionAngle_3,Now_angularVelocity_3=0; +double Average_SectionAngle_4,Now_angularVelocity_4=0; +double Now_angularVelocity_X=0,Now_angularVelocity_Y=0; +double Angle_X=0, Angle_Y=0; +double SectionTime =0; +double NowTime_measureVelocity=0, LastTime_measureVelocity = 0; +//**************************** PID control declaration ************************************// +float Now_time_PID,Last_Time_PID; + //Command +//double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1,Command_AngularVel_4=1; +//double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1,command_AngularVel_4=1; +double Command_AngularVel_X=1,Command_AngularVel_Y=1; +double command_AngularVel_X=1,command_AngularVel_Y=1; + //Control +double Control_motor_X,Control_motor_Y; + //Control PWM value +double Control_Motor_PWM_X,Control_Motor_PWM_Y; + + +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); +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); +//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); +//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); + +//****************************LQR_control declaration************************************// +double Ka = 60; //16.9573 25.0 +double Kav = 10; //10.4423 10.7423 + +double Kt = 4.0 ; //3.0 3.1 +double Kt_y = 4.0; //4.0 3.98 + +double Kv = 1.0; //1.0 +double Kv_y = 1.1; //1.1 + +double Kz = 0.05; //0.05 +double Kii = 0.2; //0.2 + +double KI_xy = 0.9; //0.02 +double KI_xy_y = 0.9; //0.025 + +double Roll_offset = -5; //2.88 4.0 3.5 +double Pitch_offset = -4; //-0.05 2.1 2.1 +double Diff_Roll,Diff_Pitch,Diff_Yaw; +double Integ_Roll,Integ_Pitch; +double Integ_x,Integ_y; +double Roll_last,Pitch_last; +double Vx=0,Vy=0,Wz=0; +double d_x=0,d_y=0; +double u_x=0,u_y=0; +double Point_x; +double x_now,y_now; +double x_pre_1,y_pre_1; +double ax_now,ay_now; +double ax_pre_1,ay_pre_1; +double ax_pre_2,ay_pre_2; +double Vx_pre_1,Vy_pre_1; +double Diff_x,Diff_y; +double Diff_x_pre,Diff_y_pre; +double dot_diff_x,dot_diff_y; +//****************************Trajectory Tracking declaration************************************// +double x_trajectory=0,y_trajectory=0; +//****************************Encoder declaration************************************// +// phase a of the quadrature encoder +// phase b of the quadrature encoder +DigitalIn phaseA_1( PA_0 ); +DigitalIn phaseB_1( PA_1 ); +DigitalIn phaseA_2( PA_8 ); +DigitalIn phaseB_2( PA_9 ); +DigitalIn phaseA_3( PC_6 ); +DigitalIn phaseB_3( PC_7 ); +DigitalIn phaseA_4( PB_8 ); +DigitalIn phaseB_4( PB_9 ); +// hold the signed value corresponding to the number of clicks left or right since last sample +// keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt +int encoderClickCount_1 = 0; +int previousEncoderState_1 = 0; +int encoderClickCount_2 = 0; +int previousEncoderState_2 = 0; +int encoderClickCount_3 = 0; +int previousEncoderState_3 = 0; +int encoderClickCount_4 = 0; +int previousEncoderState_4 = 0; +//****************************Angle Sensor declaration************************************// +float Times[10] = {0,0,0,0,0,0,0,0,0,0}; +float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz +int counter = 0; +int divider = 20; +float dt; // time for entire loop +float dt_sensors; // time only to read sensors +float q0 = 1, q1 = 0, q2 = 0, q3 = 0; +float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0; +float exInt = 0, eyInt = 0, ezInt = 0; +float OX = 0, OY = 0, OZ = 0; +float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4; +float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4; +float Mag_x_pre,Mag_y_pre,Mag_z_pre; +float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L; +float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL; +float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL; +float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total; +float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x; +float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL; +float GYRO_z_total,GYRO_z_offset,Global_GYRO_z; +float Global_mag_vector_angle,Yaw_pre; +float Global_mag_x_vector_angle,Mag_x_vector_angle; +float Global_mag_y_vector_angle,Mag_y_vector_angle; +int Count_mag_check=0; +float angle[3]; +float Roll,Pitch,Yaw; +float calibrated_values[3],magCalibrationp[3]; +float v_index[3]; +float dest1,dest2; +int f=0; +int j=0; +int k=0; +int g=0; +int count1=0,count2=0,count3=0,count4=0,count5=0,count6=0,count7=0,count8=0,count9=0,count11=0,count12=0,count14=0; +int Rot_index; +float mRes = 10.*4912./32760.0; + +int AngleFilter_counter = 0; +float Roll_total = 0,Pitch_total = 0; +//****************************Motor & SD card & MPU9250 declare************************************// +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; +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; +int control_LiftingStopRun=1, control_F_R=1, control_H_F=1; +int Control_LiftingStopRun=1, Control_F_R=1, Control_H_F=1; +int X_pos=0, Y_pos=0; +DigitalOut StopRun(PA_12); +DigitalOut Brake(PA_11); +DigitalOut AR(PB_12); +DigitalOut X1_CW_CCW(PC_11); +DigitalOut X2_CW_CCW(PC_10); +DigitalOut Y1_CW_CCW(PC_12); +DigitalOut Y2_CW_CCW(PD_2); +Servo X_PWM(PB_1); +Servo Y_PWM(PB_2); +DigitalOut myled(LED1); + +//---SD card---// +SDFileSystem sd( D4, D5, D3, D6, "sd"); // mosi, miso, sclk, cs +DigitalIn mybutton(USER_BUTTON); +//---MPU 9250---// +SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK); +//SPI_MOSI = PA_7(D11), SPI_MISO = PA_6(D12), SPI_SCK = PA_5(D13), SPI_CS = PB_6(D10) +mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object +//****************************Lifting mechanism & limit switches declare************************************// +DigitalOut LiftingStopRun(PA_15); +DigitalOut F_R(PA_14); // CW/CCW +DigitalOut H_F(PA_13); +//DigitalIn TIM(PB_7); +DigitalIn LimitSwitchUp(PB_15,PullUp); +DigitalIn LimitSwitchDown(PB_14,PullUp); +int limitSwitchUp; +int limitSwitchDown; +//****************************Timer declaration************************************// +Timer NowTime; +Timer LiftingDownTime; +//---------------------------------------------------------------------------------// + +//**************************** Filter_IMUupdate ************************************// +//**************************** Filter_IMUupdate ************************************// +void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) +{ + float norm; + float vx, vy, vz; + float ex, ey, ez; + + // normalise the measurements + norm = sqrt(ax*ax + ay*ay + az*az); + if(norm == 0.0f) return; + ax /= norm; + ay /= norm; + az /= norm; + + // estimated direction of gravity + vx = 2*(q1*q3 - q0*q2); + vy = 2*(q0*q1 + q2*q3); + vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; + + // error is sum of cross product between reference direction of field and direction measured by sensor + ex = (ay*vz - az*vy); + ey = (az*vx - ax*vz); + ez = (ax*vy - ay*vx); + + // integral error scaled integral gain + exInt += ex*Ki; + eyInt += ey*Ki; + ezInt += ez*Ki; + + // adjusted gyroscope measurements + gx += Kp*ex + exInt; + gy += Kp*ey + eyInt; + gz += Kp*ez + ezInt; + + // integrate quaternion rate and normalise + float q0o = q0; // he did the MATLAB to C error by not thinking of the beginning vector elements already being changed for the calculation of the rest! + float q1o = q1; + float q2o = q2; + float q3o = q3; + q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT; + q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT; + q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT; + q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT; + + // normalise quaternion + norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); + q0 = q0 / norm; + q1 = q1 / norm; + q2 = q2 / norm; + q3 = q3 / norm; +} + +//**************************** Filter_compute ************************************// +//**************************** Filter_compute ************************************// +void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data) +{ + // IMU/AHRS + + float d_Gyro_angle[3]; + void get_Acc_angle(const float * Acc_data); + // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/) + float radGyro[3],Gyro_cal_data; // Gyro in radians per second + + for(int i=0; i<3; i++) + { + radGyro[i] = Gyro_data[i] * 3.14159/ 180; + } + + Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]); + //IMU_AHRSupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2], Comp_data[0], Comp_data[1], Comp_data[2]); + + float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) + + rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2)); + rangle[1] = asin (2*q0*q2 - 2*q3*q1); + rangle[2] = atan2(2*q0_A*q3_A + 2*q1_A*q2_A, 1 - 2*(q2_A*q2_A + q3_A*q3_A)); // Z-axis + + + for(int i=0; i<2; i++) + { // angle in degree + angle[i] = rangle[i] * 180 / 3.14159; + } + /*Roll=angle[0]*0.2 + Roll_pre_1*0.2 + Roll_pre_2*0.2 +Roll_pre_3*0.2 + Roll_pre_4*0.2; + Roll_pre_4 = Roll_pre_3; + Roll_pre_3 = Roll_pre_2; + Roll_pre_2 = Roll_pre_1; + Roll_pre_1 = Roll; + + Pitch=angle[1]*0.2 + Pitch_pre_1*0.2 + Pitch_pre_2*0.2 + Pitch_pre_3*0.2 + Pitch_pre_4*0.2; + Pitch_pre_4 = Pitch_pre_3; + Pitch_pre_3 = Pitch_pre_2; + Pitch_pre_2 = Pitch_pre_1; + Pitch_pre_1 = Pitch;*/ + + Roll_total += angle[0]; + Pitch_total += angle[1]; + AngleFilter_counter++; + + if( AngleFilter_counter > 1 ) + { // The average of the attitude angle for 100 times. + Roll = Roll_total / AngleFilter_counter; + Pitch = Pitch_total / AngleFilter_counter; + AngleFilter_counter = 0; + Roll_total = 0; + Pitch_total = 0; + Roll -= Roll_offset; + Pitch -= Pitch_offset; + } + +//**************************************************Gyro_data[2] filter start + float GYRO_z=0; + + GYRO_z=Gyro_data[2]*0.15 + GYRO_z_pre*0.20 + GYRO_z_pre_L*0.20 + GYRO_z_pre_LL*0.25 + GYRO_z_pre_LLL*0.20; + if( count4==1 ) + { + GYRO_z_pre_L=GYRO_z_pre; + + count4=0; + } + if( count5==2 ) + { + GYRO_z_pre_LL=GYRO_z_pre_L; + + count5=0; + } + if( count6==3 ) + { + GYRO_z_pre_LLL=GYRO_z_pre_LL; + + count6=0; + } + count4++; + count5++; + count6++; + GYRO_z_pre=Gyro_data[2]; + Global_GYRO_z=GYRO_z; + /*printf(" GYRO_z:%10.3f ,count8:%10d \n", + GYRO_z, + count8 + );*/ + if((count8>5)&&(count8<=2005)) + { + GYRO_z_total+=GYRO_z; + } + if( count8==2005 ) + { + GYRO_z_offset=GYRO_z_total/2000; + /* printf(" GYRO_z_offset:%10.5f \n ", + GYRO_z_offset + );*/ + GYRO_z_total=0; + count8=0; + } + + count8++; +//**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 0.74956 +//************************************************** calculate Yaw + if( (count11==35) ) + { + if( abs(Yaw_pre-Yaw)<1 ) + { + Yaw_pre=Yaw_pre; + } + else + { + Yaw_pre=Yaw; + } + count11=0; + } + count11++; + + if( count12>=20 ) + { + Yaw += (Gyro_data[2]-0.74936) *dt; + } + count12++; + //pc.printf(" Yaw:%10.5f ", + //Yaw + // ); +} + +//**************************** Mag_Complentary_Filter ************************************// +void Mag_Complentary_Filter(float dt, const float * Comp_data) +{ + float Mag_x=0,Mag_y=0,Mag_z=0; + Mag_x=Comp_data[0]*0.15 + Mag_x_pre*0.20 + Mag_x_pre_L*0.20 + Mag_x_pre_LL*0.25 + Mag_x_pre_LLL*0.20; + Mag_y=Comp_data[1]*0.15 + Mag_y_pre*0.20 + Mag_y_pre_L*0.20 + Mag_y_pre_LL*0.25 + Mag_y_pre_LLL*0.20; + Mag_z=Comp_data[2]*0.15 + Mag_z_pre*0.20 + Mag_z_pre_L*0.20 + Mag_z_pre_LL*0.25 + Mag_z_pre_LLL*0.20; + + if( count1==1 ) + { + Mag_x_pre_L=Mag_x_pre; + Mag_y_pre_L=Mag_y_pre; + Mag_z_pre_L=Mag_z_pre; + Cal_Mag_x_pre=Cal_Mag_x; + + count1=0; + } + if( count2==2 ) + { + Mag_x_pre_LL=Mag_x_pre_L; + Mag_y_pre_LL=Mag_y_pre_L; + Mag_z_pre_LL=Mag_z_pre_L; + Cal_Mag_x_pre_L=Cal_Mag_x_pre; + + count2=0; + } + if( count7==3 ) + { + Mag_x_pre_LLL=Mag_x_pre_LL; + Mag_y_pre_LLL=Mag_y_pre_LL; + Mag_z_pre_LLL=Mag_z_pre_LL; + Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L; + + count7=0; + } + count1++; + count2++; + count7++; + Mag_x_pre=Comp_data[0]; + Mag_y_pre=Comp_data[1]; + Mag_z_pre=Comp_data[2]; + if( count14>4 ) + { + Cal_Mag_x=Mag_x; + } + count14++; + + +//*************************************Mag_ave calculate + if(count3<=20) + { + Mag_x_total+=Mag_x; + Mag_y_total+=Mag_y; + } + if( count3==20) + { + Mag_x_ave=Mag_x_total/21; + Mag_y_ave=Mag_y_total/21; + /*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ", + Mag_x_ave, + Mag_y_ave + );*/ + Mag_x_total=0; + Mag_y_total=0; + count3=0; + } + count3++; + +//********************************ROT_check start + + float v_length,v_length_ave,MagVector_angle; + v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y ); + v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave ); + + MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3; + + if( count9==3 ) + { + Global_mag_vector_angle=MagVector_angle; + count9=0; + } + count9++; + + if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) ) + { + Count_mag_check++; + + } + else + { + Count_mag_check=0; + } + + if( Count_mag_check==30 ) + { + Yaw=Yaw_pre; + Count_mag_check=0; + } + float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle); +//********************************Theta_check end + /*pc.printf("ABS_CHECK:%10.3f,Cal_Mag_x_pre_LL:%10.3f,Mag_x:%10.3f,Count_mag_check:%10d ,Yaw_pre:%10.3f,Yaw_filter:%10.3f ", + ABS_CHECK, + Cal_Mag_x_pre_LL, + Mag_x, + Count_mag_check, + Yaw_pre, + Yaw + );*/ +} +//****************************Motor_run************************************// +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) +{ + StopRun.write(control_stopRun); + Brake.write(control_brake); + AR.write(control_AR); + X1_CW_CCW.write(control_X1_CW_CCW); + X2_CW_CCW.write(control_X2_CW_CCW); + Y1_CW_CCW.write(control_Y1_CW_CCW); + Y2_CW_CCW.write(control_Y2_CW_CCW); + + + X_PWM.write(control_value_PWM_X); + Y_PWM.write(control_value_PWM_Y); +} + +//****************************quadratureDecoder************************************// +void quadratureDecoder( void ) +{ + int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read(); + int currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read(); + int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read(); + int currentEncoderState_4 = (phaseB_4.read() << 1) + phaseA_4.read(); +//**************************** 1 ************************************// + if( currentEncoderState_1 == previousEncoderState_1 ) + { + return; + } + + switch( previousEncoderState_1 ) + { + case 0: + if( currentEncoderState_1 == 2 ) + { + encoderClickCount_1--; + + } + else if( currentEncoderState_1 == 1 ) + { + encoderClickCount_1++; + + } + break; + + case 1: + if( currentEncoderState_1 == 0 ) + { + encoderClickCount_1--; + + } + else if( currentEncoderState_1 == 3 ) + { + encoderClickCount_1++; + + } + break; + + case 2: + if( currentEncoderState_1 == 3 ) + { + encoderClickCount_1--; + + } + else if( currentEncoderState_1 == 0 ) + { + encoderClickCount_1++; + + } + break; + + case 3: + if( currentEncoderState_1 == 1 ) + { + encoderClickCount_1--; + + } + else if( currentEncoderState_1 == 2 ) + { + encoderClickCount_1++; + + } + break; + + default: + break; + } + previousEncoderState_1 = currentEncoderState_1; +//**************************** 2 ************************************// + if( currentEncoderState_2 == previousEncoderState_2 ) + { + return; + } + + switch( previousEncoderState_2 ) + { + case 0: + if( currentEncoderState_2 == 2 ) + { + encoderClickCount_2--; + + } + else if( currentEncoderState_2 == 1 ) + { + encoderClickCount_2++; + + } + break; + + case 1: + if( currentEncoderState_2 == 0 ) + { + encoderClickCount_2--; + + } + else if( currentEncoderState_2 == 3 ) + { + encoderClickCount_2++; + + } + break; + + case 2: + if( currentEncoderState_2 == 3 ) + { + encoderClickCount_2--; + + } + else if( currentEncoderState_2 == 0 ) + { + encoderClickCount_2++; + + } + break; + + case 3: + if( currentEncoderState_2 == 1 ) + { + encoderClickCount_2--; + + } + else if( currentEncoderState_2 == 2 ) + { + encoderClickCount_2++; + + } + break; + + default: + break; + } + previousEncoderState_2 = currentEncoderState_2; +//**************************** 3 ************************************// + if( currentEncoderState_3 == previousEncoderState_3 ) + { + return; + } + + switch( previousEncoderState_3 ) + { + case 0: + if( currentEncoderState_3 == 2 ) + { + encoderClickCount_3--; + + } + else if( currentEncoderState_3 == 1 ) + { + encoderClickCount_3++; + + } + break; + + case 1: + if( currentEncoderState_3 == 0 ) + { + encoderClickCount_3--; + + } + else if( currentEncoderState_3 == 3 ) + { + encoderClickCount_3++; + + } + break; + + case 2: + if( currentEncoderState_3 == 3 ) + { + encoderClickCount_3--; + + } + else if( currentEncoderState_3 == 0 ) + { + encoderClickCount_3++; + + } + break; + + case 3: + if( currentEncoderState_3 == 1 ) + { + encoderClickCount_3--; + + } + else if( currentEncoderState_3 == 2 ) + { + encoderClickCount_3++; + + } + break; + + default: + break; + } + previousEncoderState_3 = currentEncoderState_3; +//**************************** 4 ************************************// + if( currentEncoderState_4 == previousEncoderState_4 ) + { + return; + } + + switch( previousEncoderState_4 ) + { + case 0: + if( currentEncoderState_4 == 2 ) + { + encoderClickCount_4--; + + } + else if( currentEncoderState_4 == 1 ) + { + encoderClickCount_4++; + + } + break; + + case 1: + if( currentEncoderState_4 == 0 ) + { + encoderClickCount_4--; + + } + else if( currentEncoderState_4 == 3 ) + { + encoderClickCount_4++; + + } + break; + + case 2: + if( currentEncoderState_4 == 3 ) + { + encoderClickCount_4--; + + } + else if( currentEncoderState_4 == 0 ) + { + encoderClickCount_4++; + + } + break; + + case 3: + if( currentEncoderState_4 == 1 ) + { + encoderClickCount_4--; + + } + else if( currentEncoderState_4 == 2 ) + { + encoderClickCount_4++; + + } + break; + + default: + break; + } + previousEncoderState_4 = currentEncoderState_4; +} + +//****************************getAngular************************************// +//****************************getAngular************************************// +void getAngular( void ) +{ +// Angle_1 = (encoderClickCount_1*0.1499)/5; +// Angle_2 = (encoderClickCount_2*0.1499)/5; +// Angle_3 = (encoderClickCount_3*0.1499)/5; +// Angle_4 = (encoderClickCount_4*0.1499)/5; + Angle_1 = encoderClickCount_1*0.03; + Angle_2 = encoderClickCount_2*0.03; + Angle_3 = encoderClickCount_3*0.03; + Angle_4 = encoderClickCount_4*0.03; + + _1_SectionAngle = Angle_1 - LastAngle_1; + _2_SectionAngle = Angle_2 - LastAngle_2; + _3_SectionAngle = Angle_3 - LastAngle_3; + _4_SectionAngle = Angle_4 - LastAngle_4; + Average_SectionAngle_1 = (_1_SectionAngle*0.3 + _1_LastSectionAngle*0.3 + _1_LastSectionAngle_1*0.1 + _1_LastSectionAngle_2*0.1 + _1_LastSectionAngle_3*0.1 + _1_LastSectionAngle_4*0.1)/1; + Average_SectionAngle_2 = (_2_SectionAngle*0.3 + _2_LastSectionAngle*0.3 + _2_LastSectionAngle_1*0.1 + _2_LastSectionAngle_2*0.1 + _2_LastSectionAngle_3*0.1 + _2_LastSectionAngle_4*0.1)/1; + Average_SectionAngle_3 = (_3_SectionAngle*0.3 + _3_LastSectionAngle*0.3 + _3_LastSectionAngle_1*0.1 + _3_LastSectionAngle_2*0.1 + _3_LastSectionAngle_3*0.1 + _3_LastSectionAngle_4*0.1)/1; + 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; + NowTime_measureVelocity = NowTime.read(); + SectionTime = NowTime_measureVelocity - LastTime_measureVelocity; + Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime)); + Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime)); + Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime)); + Now_angularVelocity_4 = abs(Average_SectionAngle_4/(SectionTime)); + Now_angularVelocity_X = (Now_angularVelocity_2+Now_angularVelocity_4)*0.5; + Now_angularVelocity_Y = (Now_angularVelocity_1+Now_angularVelocity_3)*0.5; + + + LastTime_measureVelocity = NowTime_measureVelocity; + LastAngle_1 = Angle_1; + _1_LastSectionAngle_4 = _1_LastSectionAngle_3; + _1_LastSectionAngle_3 = _1_LastSectionAngle_2; + _1_LastSectionAngle_2 = _1_LastSectionAngle_1; + _1_LastSectionAngle_1 = _1_LastSectionAngle; + _1_LastSectionAngle = _1_SectionAngle; + LastAngle_2 = Angle_2; + _2_LastSectionAngle_4 = _2_LastSectionAngle_3; + _2_LastSectionAngle_3 = _2_LastSectionAngle_2; + _2_LastSectionAngle_2 = _2_LastSectionAngle_1; + _2_LastSectionAngle_1 = _2_LastSectionAngle; + _2_LastSectionAngle = _2_SectionAngle; + LastAngle_3 = Angle_3; + _3_LastSectionAngle_4 = _3_LastSectionAngle_3; + _3_LastSectionAngle_3 = _3_LastSectionAngle_2; + _3_LastSectionAngle_2 = _3_LastSectionAngle_1; + _3_LastSectionAngle_1 = _3_LastSectionAngle; + _3_LastSectionAngle = _3_SectionAngle; + LastAngle_4 = Angle_4; + _4_LastSectionAngle_4 = _4_LastSectionAngle_3; + _4_LastSectionAngle_3 = _4_LastSectionAngle_2; + _4_LastSectionAngle_2 = _4_LastSectionAngle_1; + _4_LastSectionAngle_1 = _4_LastSectionAngle; + _4_LastSectionAngle = _4_SectionAngle; + + if (Angle_1>=0 && Angle_3>=0) + { + Angle_Y = (Angle_1 + Angle_3)*0.5; + } + else if(Angle_1>=0 && Angle_3<=0) + { + Angle_Y = (Angle_1 + abs(Angle_3))*0.5; + } + else if(Angle_1<=0 && Angle_3>=0) + { + Angle_Y = -(abs(Angle_1) + Angle_3)*0.5; + } + else if(Angle_1<=0 && Angle_3<=0) + { + Angle_Y = (Angle_1 + Angle_3)*0.5; + } + + if (Angle_2>=0 && Angle_4>=0) + { + Angle_X = (Angle_2 + Angle_4)*0.5; + } + else if (Angle_2>=0 && Angle_4<=0) + { + Angle_X = (Angle_2 + abs(Angle_4))*0.5; + } + else if (Angle_2<=0 && Angle_4>=0) + { + Angle_X = -(abs(Angle_2) + Angle_4)*0.5; + } + else if (Angle_2<=0 && Angle_4<=0) + { + Angle_X = (Angle_2 + Angle_4)*0.5; + } + +} + +//****************************PWM_commmand_transformation************************************// +//****************************PWM_commmand_transformation************************************// +double PWM_commmand_transformation( double Control_AngVel_Value ) +{ + double Control_PWM_Value = 0; + if( Control_AngVel_Value > 0 ) + { + Control_AngVel_Value = Control_AngVel_Value; + } + else if( Control_AngVel_Value < 0 ) + { + Control_AngVel_Value = -Control_AngVel_Value; + } + if( Control_AngVel_Value > 325) + { + if( Control_AngVel_Value < 466 ) + { + if( Control_AngVel_Value < 393 ) + { + Control_PWM_Value = Control_AngVel_Value/651.6 ; //0.5~0.6 + } + else + { + Control_PWM_Value = Control_AngVel_Value/658.8; //0.6~0.7 + } + } + else + { + if( Control_AngVel_Value < 523 ) + { + Control_PWM_Value = Control_AngVel_Value/658.39; //0.7~0.8 + } + else + { + if( Control_AngVel_Value < 588 ) + { + Control_PWM_Value = Control_AngVel_Value/652.36; //0.8~0.9 + } + else + { + Control_PWM_Value = Control_AngVel_Value/655.11; //0.9~1 + } + } + } + } + else if( Control_AngVel_Value < 325) + { + if( Control_AngVel_Value < 40 ) + { + Control_PWM_Value = Control_AngVel_Value/533.3; //0~0.075 + } + else + { + if( Control_AngVel_Value < 59 ) + { + Control_PWM_Value = Control_AngVel_Value/560.65; //0.1~0.15 + } + else + { + if( Control_AngVel_Value < 131 ) + { + Control_PWM_Value = Control_AngVel_Value/638.3; //0.15~0.2 + } + else + { + if( Control_AngVel_Value < 197 ) + { + Control_PWM_Value = Control_AngVel_Value/651.6; //0.2~0.3 + } + else + { + if( Control_AngVel_Value < 263 ) + { + Control_PWM_Value = Control_AngVel_Value/654.16; //0.3~0.4 + } + else + { + Control_PWM_Value = Control_AngVel_Value/652.5; //0.4~0.5 + } + } + } + } + } + } + return Control_PWM_Value; +} + +//****************************PIDcontrol_compute_velocity************************************// +//****************************PIDcontrol_compute_velocity************************************// +void PIDcontrol_compute_velocity(void) +{ + if(command_AngularVel_X >= 0) + { + Control_X1_CW_CCW = 1; + Control_X2_CW_CCW = 0; + Command_AngularVel_X = command_AngularVel_X; + } + else + { + Control_X1_CW_CCW = 0; + Control_X2_CW_CCW = 1; + Command_AngularVel_X = -command_AngularVel_X; + } + if(command_AngularVel_Y >= 0) + { + Control_Y1_CW_CCW = 1; + Control_Y2_CW_CCW = 0; + Command_AngularVel_Y = command_AngularVel_Y; + } + else + { + Control_Y1_CW_CCW = 0; + Control_Y2_CW_CCW = 1; + Command_AngularVel_Y = -command_AngularVel_Y; + } + Now_time_PID=NowTime.read(); + Motor_X.Compute(&Now_time_PID); + Motor_Y.Compute(&Now_time_PID); +// Control_Motor_PWM_X = PWM_commmand_transformation(Control_motor_X); +// Control_Motor_PWM_Y = PWM_commmand_transformation(Control_motor_Y); + Control_Motor_PWM_X = PWM_commmand_transformation(command_AngularVel_X); + Control_Motor_PWM_Y = PWM_commmand_transformation(command_AngularVel_Y); +} + +//****************************Compute_point************************************// +//****************************Compute_point************************************// +void LQR_control_compute(void) +{ + Diff_Roll = (Roll - Roll_last); + Diff_Pitch = (Pitch - Pitch_last); + Integ_Roll += Roll; + //printf("Diff_Roll:%.3f\n",Diff_Roll); + Integ_Pitch += Pitch; + Roll_last = Roll; + Pitch_last = Pitch; + + //Diff_Roll =0; + //Roll -= 2.45; + //Pitch -=2.5; + Diff_x = x_now - x_trajectory; + Diff_y = y_now - y_trajectory; + dot_diff_x = Diff_x - Diff_x_pre; + dot_diff_y = Diff_y - Diff_y_pre; + Integ_x += Diff_x; + Integ_y += Diff_y; + + //x_pre_1 = x_now; + //y_pre_1 = y_now; + Diff_x_pre = Diff_x; + Diff_y_pre = Diff_y; + +// u_x = Ka*(Roll)+ Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x; +// u_y = Ka*(Pitch)+Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y; + u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x; +// u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll; + //u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll - Kt*(10) - Kv*0; +// u_x = Ka*(Roll); + u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y; +// u_y = Ka*(Pitch)+(Integ_Roll*Kii) + Kav*Diff_Roll; + //u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch - Kt*0 - Kv*0; +// u_y = Ka*(Pitch); + if(u_x > 580) + { + u_x = 580; + } + else if(u_x < -580) + { + u_x = -580; + } + if(u_y > 580 ) + { + u_y = 580; + } + else if(u_y < -580) + { + u_y = -580; + } + Vx = u_x; + Vy = u_y; + Wz = Yaw; + +// Vx = 100; +// Vy = 100; +// Wz =0; + command_AngularVel_X = Vx+Kz*Wz; + command_AngularVel_Y = Vy+Kz*Wz; +} + +//***********************************************************// +// +////若要前進,則在x或y加減p_set。根據底盤的正負標識,來決定加或減。 +// +// +// /*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); +// 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);*/ +// // 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); +// // 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); +// /*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); +// 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); +//*/ +// // integral U for PI close loop +// x_iu=x_u*T2+x_iu_old; //integral value of v_dx +// y_iu=y_u*T2+y_iu_old; +// +// x_iu_old=x_iu; +// y_iu_old=y_iu; +// +// // dV cmd(U cmd) PI close loop (like low-pass filter multip a gain value) +// 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); +// 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); +// +// // Vx = integral dVx +// +// C_theta1=C_theta1_d*T2+C_theta1_old; +// // limit +// C_theta1= C_theta1> (5000) ? (5000): C_theta1; +// C_theta1= C_theta1< (-5000) ? (-5000): C_theta1; +// +// C_theta1_old=C_theta1; +// // integral Vx +// C_theta1_i=C_theta1*T2+C_theta1_i; +// +// // Vy = integral dVy +// C_theta2=C_theta2_d*T2+C_theta2_old; +// // limit +// C_theta2= C_theta2>(5000) ? (5000): C_theta2; +// C_theta2= C_theta2<(-5000) ? (-5000):C_theta2; +// +// C_theta2_old=C_theta2; +// // integral Vy +// C_theta2_i=C_theta2*T2+C_theta2_i; +// +// // speed PI LOOP control +// +// C_thetaX=Kcp*(C_theta1-vel_x*Cgain1)+Kci*(C_theta1_i-x*Cgain2); //X axis +// C_thetaY=Kcp*(C_theta2-vel_y*Cgain1)+Kci*(C_theta2_i-y*Cgain2); //Y axis +// +// #endif +// #if Testmode == 1 // nonlinear +// +////########### x 方向之控制器############# +// r1=1; +// r2=1; +// err_tilt= 0+tilt2*0.0006086;/*tilt_out;傾斜儀資料 */ +// err_gyro= 0+gyro2;/*陀螺儀資料*/ +// sin_t=sin(err_tilt); +// cos_t=cos(err_tilt); +// err_tilt_i=err_tilt_i+err_tilt*0.1; +// x_i=x_i+x*0.001; +// //err_tilt_i+=err_tilt; +// //x_i+=x; +// delta=187.5-125.3*cos_t; +// A=(5.51+33.75*cos_t)/delta; +// B=-((125.3*sin_t*cos_t)/delta)*err_gyro*err_gyro+182.2*sin_t/delta; +//// 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; +// C=Kp3*(eta1-Kp3*err_tilt-Ki3*err_tilt_i)+Ki3*err_tilt; +// D=-(5.51+33.75*cos_t)/delta; +// E=126.4/delta*err_gyro*err_gyro*sin_t-1637/delta*sin_t*cos_t; +// F=Kp3*(eta2-Kp3*x-Ki3*x_i)+Ki3*x; +////F=Kp3*(eta2-Kp3*err_tilt-Ki3*0)+Ki3*0; +// eta1=err_gyro+Kp3*err_tilt+Ki3*err_tilt_i; +// eta2=vel_x+Kp3*x+Ki3*x_i; +////eta2=0; +// s=r1*eta1+r2*eta2; +// if(s>=1) sgns=1; +// else if(s<=-1) sgns=-1; +// else sgns=s; +// +// if (s*abs(vel_x)>=1) sgns_c=1; +// else if(s*abs(vel_x)<=-1) sgns_c=-1; +// else sgns_c=s*abs(vel_x); +////########### y 方向之控制器############# +// err_tilt2= 0+tilt5*0.0006086;/*tilt_out;傾斜儀資料 */ +// err_gyro2= 0+gyro5;/*陀螺儀資料*/ +// sin_t2=sin(err_tilt2); +// cos_t2=cos(err_tilt2); +// err_tilt2_i=err_tilt2_i+err_tilt2*0.1; +// y_i=y_i+y*0.001; +////err_tilt2_i+=err_tilt2; +////y_i+=y; +// delta2=187.5-125.3*cos_t2; +// A2=(5.51+33.75*cos_t2)/delta2; +// B2=-((125.3*sin_t2*cos_t2)/delta2)*err_gyro2*err_gyro2+182.2*sin_t2/delta2; +// C2=Kp3*(eta3-Kp3*err_tilt2-Ki3*err_tilt2_i)+Ki3*err_tilt2; +// D2=-(5.51+33.75*cos_t2)/delta2; +// E2=126.4/delta2*err_gyro2*err_gyro2*sin_t2-1637/delta2*sin_t2*cos_t2; +// F2=Kp3*(eta4-Kp3*y-Ki3*y_i)+Ki3*y; +////F2=Kp3*(eta4-Kp3*err_tilt2-Ki3*0)+Ki3*0; +// eta3=err_gyro2+Kp3*err_tilt2+Ki3*err_tilt2_i; +// eta4=vel_y+Kp3*y+Ki3*y_i; +////eta4=0; +// s2=r1*eta3+r2*eta4; +// if(s2>=1) sgns2=1; +// else if(s2<=-1) sgns2=-1; +// else sgns2=s2; +// +// if(s2*abs(vel_y)>=1) sgns2_c=1; +// else if(s2*abs(vel_y)<=-1) sgns2_c=-1; +// else sgns2_c=s2*abs(vel_y); +// +////若要前進,則在x或y加減p_set。根據底盤的正負標識,來決定加或減。 +// C_theta1_new=((r1*B+r1*C+r2*E+r2*F)+s+sgns+sgns_c)/(r1*A+r2*D); +// C_theta2_new=((r1*B2+r1*C2+r2*E2+r2*F2)+s2+sgns2+sgns2_c)/(r1*A2+r2*D2); +// } +// i++; +// +// C_theta1=C_theta1_new*0.00050+C_theta1_old; +// C_theta1_old=C_theta1; +// +// +// C_theta2=C_theta2_new*0.00050+C_theta2_old; +// C_theta2_old=C_theta2; +// +// C_theta1= C_theta1>(42143)?(42143): C_theta1; +// C_theta1= C_theta1<-1*(42143)?-1*(42143): C_theta1; +// C_theta2= C_theta2>(42143)?(42143): C_theta2; +// C_theta2=C_theta2<-1*(42143)?-1*(42143):C_theta2; +// +// #endif + +//***********************************************************// + + +void MeasureRobotAttitudeAngle(void) +{ + dt = t_MeasureRobotAttitudeAngle; + imu.read_all(); + Mag_Complentary_Filter(dt,imu.Magnetometer); + Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer); + /*x_now = x_pre_1 + (1/(dt*dt))*(ax_now-2*ax_pre_1+ax_pre_2); + y_now = y_pre_1 + (1/(dt*dt))*(ay_now-2*ay_pre_1+ay_pre_2); + x_pre_1 = x_now; + y_pre_1 = y_now;*/ + x_now = r_grounder*pi*(Angle_X/180); + y_now = r_grounder*pi*(Angle_Y/180); +// x_now = r_ball*pi*(Angle_X/180); +// y_now = r_ball*pi*(Angle_Y/180); + do_measure_index++; +} +//void Trajectory_tracking(void) +//{ +// double t_trajectory = NowTime.read(); +// if( RunTime>=0 &&( RunTime<=10)) +// { +// //x_trajectory = t_trajectory * (0.5); +// x_trajectory = 0; +// y_trajectory = 0; +// } +// else if( RunTime>10 ) +// { +// x_trajectory = 0; +// y_trajectory = 0; +// } +//} +//****************************main function************************************// +int main() +{ +// pc.baud(115200); + pc.baud(230400); + +//****************************Motor driver declare************************************// + //ExtInt = 0; //number high, voltage high. + //Control_stopRun=0; +// Control_brake=1; + Control_AR=1; + Control_H_F=1; + H_F.write(Control_H_F); // control_H_F must equal to 1 +// StopRun.write(Control_stopRun); +// Brake.write(Control_brake); +// AR.write(Control_AR); +//****************************Angle Sensor initialization************************************// + if(imu.init(1,BITS_DLPF_CFG_188HZ)) + { //INIT the mpu9250 +// pc.printf("\nCouldn't initialize MPU9250 via SPI!"); + } + imu.whoami(); +// wait(0.1); + imu.set_gyro_scale(BITS_FS_2000DPS); +// wait(0.1); + imu.set_acc_scale(BITS_FS_16G); +// wait(0.1); + imu.AK8963_whoami(); +// wait(0.1); + imu.AK8963_calib_Magnetometer(); +////****************************SD card************************************// + mkdir("/sd/Amber20170822a", 0777); //SD裡面的資料夾叫Amber77,在此做宣告 + FILE *fp = fopen("/sd/Amber20170822a/RollPitchYaw_y1.csv", "a"); + //將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv + fprintf(fp,"RunTime,Roll,Pitch,Yaw,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory\n"); + + //**************************** PWM ************************************// +// X_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ +// Y_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ +// X_PWM.calibrate(0.02, 0*0.02, 1*0.02); +// Y_PWM.calibrate(0.02, 0*0.02, 1*0.02); + X_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ + Y_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ +//**************************** Encoder ************************************// + phaseA_1.mode( PullUp ); + phaseB_1.mode( PullUp ); + phaseA_2.mode( PullUp ); + phaseB_2.mode( PullUp ); + phaseA_3.mode( PullUp ); + phaseB_3.mode( PullUp ); + phaseA_4.mode( PullUp ); + phaseB_4.mode( PullUp ); +//**************************** Create the motor encoder sampler. ************************************// + Sample_Motor_encoder.attach_us( &quadratureDecoder, t_quadratureDecoder ); +//**************************** Create the motor encoder sampler. ************************************// + Sample_robotAngleSensor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle ); +//**************************** Create the motor angular velocity measurement. ************************************// + MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity ); +//**************************** Create the motor angular velocity PID control. ************************************// + PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity ); +//**************************** Create the robot LQR control. ************************************// + LQR_control.attach( &LQR_control_compute, t_LQR_control ); +//**************************** Trajectory tracking control ************************************// +// TrajectoryTracking_control.attach( &Trajectory_tracking, t_trajectory ); +//**************************** Motor settlement ************************************// + Motor_X.SetMode(AUTOMATIC); + Motor_Y.SetMode(AUTOMATIC); +// Command_AngularVel_X = 0; +// Command_AngularVel_Y = 0; +//**************************** Timers start ************************************// + NowTime.start(); + while( 1 ) + { + RunTime = NowTime.read(); + t_trajectory = NowTime.read(); + limitSwitchUp = LimitSwitchUp.read(); + limitSwitchDown = LimitSwitchDown.read(); + //pc.printf("%d \n",Control_F_R); + if (mybutton == 0) + { + if( Control_F_R == 1) + { + Control_LiftingStopRun=0; + LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop + F_R.write(Control_F_R); // 0:turn down 1:turn up + Control_stopRun=0; + Control_brake=1; + 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); + wait(3); + } + else if ( Control_F_R == 0) + { + Control_LiftingStopRun=0; + LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop + F_R.write(Control_F_R); // 0:turn down 1:turn up + Control_stopRun=0; + Control_brake=1; + 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); + wait(2); + Control_stopRun=1; + Control_brake=1; + 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); + wait(1); + } + Control_F_R=1-Control_F_R; + wait(0.2); + } + if(limitSwitchUp == 0) + { + LiftingStopRun.write(1); // 0:Run 1:Stop + F_R.write(1); // 0:turn down 1:turn up + } + if(limitSwitchDown == 0) + { + LiftingStopRun.write(1); // 0:Run 1:Stop + F_R.write(0); // 0:turn down 1:turn up + } + if( RunTime>=0 &&( RunTime<=10)) + { + //x_trajectory = t_trajectory * (0.5); + //Control_stopRun=0; +// Control_brake=1; + x_trajectory = 0; + y_trajectory = 0; + } + else if( RunTime>10 && (RunTime <=20 )) + { + //Control_stopRun=0; +// Control_brake=1; + x_trajectory = (t_trajectory-10) * (0.05); + y_trajectory = 0; + } + else if( RunTime>=20 ) + { + //Control_stopRun=0; +// Control_brake=1; + x_trajectory = 0; + y_trajectory = 0; + } + + + if( (RunTime-lastTime) > 0.1 ) + { + index_times++; + lastTime = RunTime; + } + + if( index_times >= 0.1 ) + { + /*pc.printf("x_now: %.3f , y_now: %.3f , x_trajectory: %f , u_y: %f ",x_now,y_now,x_trajectory,u_y); + pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f \n", + Roll, + Pitch, + Yaw + );*/ + //pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory); + //pc.printf("RunTime=%10.3f || Roll=%10.3f || Pitch=%10.3f || Yaw=%10.3f \n",RunTime, Roll, Pitch, Yaw); +// 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", +// Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_now,y_now,Angle_X,Angle_Y); + pc.printf("RunTime=%.3f || Roll=%.3f || Pitch=%.3f || Yaw=%.3f \n",RunTime, Roll, Pitch, Yaw); + 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", + Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory,Angle_X,Angle_Y); + //pc.printf(" Now_angularVelocity : %.3f ",Now_angularVelocity); +// pc.printf(",RunTime : %.3f ", RunTime ); +// pc.printf(",control_PWM_Value : %.3f \n", control_PWM_Value); + index_times = 0; + //if(fp == NULL) +// { +// error("Could not open file for write\n"); +// } +// //fprintf(fp,"%.3f,%.3f,%.3f\n", RunTime,Now_angularVelocity,control_PWM_Value); + 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); + } +// Lifting(Control_LiftingStopRun, Control_F_R , Control_H_F); + 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); + //wait(0.1); + // if(!mybutton) +// { +// StopRun.write(1); +// Brake.write(1); +// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1 +// X2_CW_CCW.write(0); +// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1 +// Y2_CW_CCW.write(0); +// X_PWM.write(0); // clockwise:0 counterclockwise:1 +// Y_PWM.write(0); +// break; +// } + } + fclose(fp); +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Aug 30 02:10:02 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/176b8275d35d \ No newline at end of file