Amber Tang
20170830a
Dependencies: MPU9250_SPI PID SDFileSystem01 Servo mbed
Fork of
testSSWMR_StationKeeping_20170830
by 
Amber Tang
Diff: main.cpp
- Revision:
- 0:038acacdae04
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Aug 30 01:59:06 2017 +0000
@@ -0,0 +1,1409 @@
+#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 2 //1.1
+#define Ki_1 0.2 //0.2
+#define Kd_1 0.01
+//**************************** 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_motor_X1,Control_motor_Y1,Control_motor_X2,Control_motor_Y2;
+ //Control PWM value
+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;
+
+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);
+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);
+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);
+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);
+//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 = 20; //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.05; //0.2
+
+double KI_xy = 0.02; //0.02
+double KI_xy_y = 0.02; //0.025
+
+double Roll_offset = -5; //2.88 4.0 3.5
+double Pitch_offset = -3; //-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_X1.Compute(&Now_time_PID);
+ Motor_X2.Compute(&Now_time_PID);
+ Motor_Y1.Compute(&Now_time_PID);
+ Motor_Y2.Compute(&Now_time_PID);
+// Motor_X.Compute(&Now_time_PID);
+// Motor_Y.Compute(&Now_time_PID);
+ Control_Motor_PWM_X1 = PWM_commmand_transformation(Control_motor_X1);
+ Control_Motor_PWM_X2 = PWM_commmand_transformation(Control_motor_X2);
+ Control_Motor_PWM_Y1 = PWM_commmand_transformation(Control_motor_Y1);
+ Control_Motor_PWM_Y2 = PWM_commmand_transformation(Control_motor_Y2);
+ Control_Motor_PWM_X = (Control_Motor_PWM_X1+Control_Motor_PWM_X2)*0.5;
+ Control_Motor_PWM_Y = (Control_Motor_PWM_Y1+Control_Motor_PWM_Y2)*0.5;
+// 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_Roll= Roll*0.0006086;
+//// err_tilt= 0+tilt2*0.0006086;/*tilt_out;傾斜儀資料 */
+// err_Diff_Roll= Diff_Roll;
+//// err_gyro= 0+gyro2;/*陀螺儀資料*/
+// sin_t=sin(err_Roll);
+// cos_t=cos(err_Roll);
+// err_Roll_i=err_Roll_i+err_Roll*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/Amber20170822", 0777); //SD裡面的資料夾叫Amber77,在此做宣告
+ FILE *fp = fopen("/sd/Amber20170822/RollPitchYaw_a.csv", "a");
+ //將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv
+ fprintf(fp,"Roll,Pitch,Yaw,RunTime,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_now,y_now\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_X1.SetMode(AUTOMATIC);
+ Motor_X2.SetMode(AUTOMATIC);
+ Motor_Y1.SetMode(AUTOMATIC);
+ Motor_Y2.SetMode(AUTOMATIC);
+
+ // 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);
+ x_trajectory = 0;
+ y_trajectory = 0;
+ }
+ else if( RunTime>10 )
+ {
+ x_trajectory = 0;
+ y_trajectory = 0;
+ }
+
+
+ if( (RunTime-lastTime) > 0.1 )
+ {
+ index_times++;
+ lastTime = RunTime;
+ }
+
+ if( index_times >= 0.1 )
+ {
+ //pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory);
+ 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_now,y_now,Angle_X,Angle_Y);
+ //pc.printf(" Now_angularVelocity : %.3f ",Now_angularVelocity);
+ 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", Roll,Pitch,Yaw,RunTime,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y, x_now, y_now);
+ }
+// 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);
+ }
+ fclose(fp);
+}
+