Roger Weng / InPipeRobot

pipeRobot ver1

Dependencies:   LSM9DS0 MX28 mbed PID QEI Matrix

Files at this revision

API Documentation at this revision

Comitter:
roger5641
Date:
Mon Jul 17 02:19:28 2017 +0000
Parent:
6:0778ff3e79a6
Commit message:
add slope mode

Changed in this revision

MX28.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- a/MX28.lib	Fri Apr 28 06:34:09 2017 +0000
+++ b/MX28.lib	Mon Jul 17 02:19:28 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/roger5641/code/MX28/#2e32ee9f0e51
+https://developer.mbed.org/users/roger5641/code/MX28/#e221394e585b
--- a/main.cpp	Fri Apr 28 06:34:09 2017 +0000
+++ b/main.cpp	Mon Jul 17 02:19:28 2017 +0000
@@ -29,7 +29,7 @@
 
 Timer t;
 
-DynamixelClass mx106(115200,PA_13, PC_10, PC_11);
+DynamixelClass mx106(1000000, PC_12, PC_10, PC_11); //(control,Tx,Rx)
 LSM9DS0 sensor(SPI_MODE, D9, D6);
 QEI wheel_L (A2, A1, NC, 1560, 10, 0.01, QEI::X4_ENCODING); //390*4 = 1560 //chang array 50 to 10
 QEI wheel_R (D12, D13, NC, 1560, 10, 0.01, QEI::X4_ENCODING);//390*4 = 1560 //chang array 50 to 10
@@ -65,24 +65,26 @@
 
 char Idle_Mode = 'a';
 char Normal_Mode = 'b';
-char Climbing_Mode   = 'c';
+char Climbing_Mode = 'c';
 char Idle2Turn_Mode = 'd';
 char Turn2Idle_Mode = 'e';
 char Arm_up_Mode = 'f';
-char Arm_up_small_Mode = 's';
+char Arm_up_strong_Mode = 's';
 char Arm_down_Mode = 'g';
 char Set_large_phi_Mode = 'l';
 char Set_phi_Mode = 'k';
 char Set_middle_phi_Mode = 'j';
 char Set_small_phi_Mode = 'h';
+char Set_zero_phi_Mode = 'o';
+char Set_still_Mode = 'm';
 char TurnHalf_Mode = 'w';
 char Roll_Mode = 't';
 char Fusion1_Mode = 'y';
 char OpenLoop_Mode = 'u';
 char Kalman_test_Mode = 'q';
-char reset_Mode = 'r';
 char robot_Mode = 'a';
 
+/***************************counter & flag***********************************/
 int theta_flag = 1;
 int normal_counter = 0;
 int rls_counter = 0;
@@ -140,6 +142,10 @@
 float w3_hpf = 0.0;
 float w3_hpf_old = 0.0;
 
+float gyro1 = 0.0;
+float gyro2 = 0.0;
+float gyro3 = 0.0;
+
 // estimated state variables
 float gs1_hat = 0.0;
 float gs1_hat_old = 0.0;
@@ -156,6 +162,10 @@
 float gs1_hat_g = 0.0;
 float gs2_hat_g = 0.0;
 float gs3_hat_g = 0.0;
+// slope coordinates
+float gs1_hat_s = 0.0;
+float gs2_hat_s = 0.0;
+float gs3_hat_s = 0.0;
 
 // bandwidth
 float alpha = 6.28;     // 1Hz
@@ -164,8 +174,6 @@
 //****************theta Angle from encoder****************************//
 float theta = 0.0;        // theta angle
 float theta_old = 0.0;
-float Itheta = 0.0;
-float Itheta_old = 0.0;
 float dtheta = 0.0;
 float dtheta_old = 0.0;
 float dtheta_f = 0.0;
@@ -175,21 +183,24 @@
 float theta_gyro = 0.0;
 float theta_gyro_f = 0.0;
 float theta_gyro_f_old = 0.0;
-float Itheta_gyro = 0.0;
-float Itheta_gyro_f = 0.0;
-float Itheta_gyro_f_old = 0.0;
 float dtheta_gyro = 0.0;
 float dtheta_gyro_old = 0.0;
 float dtheta_gyro_f = 0.0;
 float dtheta_gyro_f_old = 0.0;
 
+//****************theta Angle from gyro with slope****************************//
+float theta_gyro_slope = 0.0;
+float theta_gyro_slope_f = 0.0;
+float theta_gyro_slope_f_old = 0.0;
+float dtheta_gyro_slope = 0.0;
+float dtheta_gyro_slope_old = 0.0;
+float dtheta_gyro_slope_f = 0.0;
+float dtheta_gyro_slope_f_old = 0.0;
+
 //****************theta Angle from gyro roll fusion 1****************************//
 float gyro_roll_fusion1 = 0.0;
 float gyro_roll_fusion1_f = 0.0;
 float gyro_roll_fusion1_f_old = 0.0;
-float Igyro_roll_fusion1 = 0.0;
-float Igyro_roll_fusion1_f = 0.0;
-float Igyro_roll_fusion1_f_old = 0.0;
 float dgyro_roll_fusion1 = 0.0;
 float dgyro_roll_fusion1_old = 0.0;
 float dgyro_roll_fusion1_f = 0.0;
@@ -199,9 +210,6 @@
 float gyro_roll_fusion2 = 0.0;
 float gyro_roll_fusion2_f = 0.0;
 float gyro_roll_fusion2_f_old = 0.0;
-float Igyro_roll_fusion2 = 0.0;
-float Igyro_roll_fusion2_f = 0.0;
-float Igyro_roll_fusion2_f_old = 0.0;
 float dgyro_roll_fusion2 = 0.0;
 float dgyro_roll_fusion2_old = 0.0;
 float dgyro_roll_fusion2_f = 0.0;
@@ -214,9 +222,6 @@
 float phi_old = 0.0;
 float phi_f = 0.0;
 float phi_f_old = 0.0;
-float Iphi = 0.0;
-float Iphi_f = 0.0;
-float Iphi_f_old = 0.0;
 float dphi = 0.0;
 float dphi_old = 0.0;
 float dphi_f = 0.0;
@@ -229,39 +234,31 @@
 float roll_old = 0.0;
 float roll_f = 0.0;
 float roll_f_old = 0.0;
-float Iroll = 0.0;
-float Iroll_f = 0.0;
-float Iroll_f_old = 0.0;
 float droll = 0.0;
 float droll_old = 0.0;
 float droll_f = 0.0;
 float droll_f_old = 0.0;
+float initial_roll = 0.0;
 
 //**************** roll Angle for traveling ****************************//
 float gama = 0.0;      //Roll Angle
 float gama_old = 0.0;
 float gama_f = 0.0;
 float gama_f_old = 0.0;
-float Igama  = 0.0;
-float Igama_f = 0.0;
-float Igama_f_old = 0.0;
 float dgama  = 0.0;
 float dgama_old = 0.0;
 float dgama_f = 0.0;
 float dgama_f_old = 0.0;
 
 //**************** pitch Angle not use ****************************//
-float pitch = 0.0;      //pitch Angle
-float pitch_old = 0.0;
-float pitch_f = 0.0;
-float pitch_f_old = 0.0;
-float Ipitch = 0.0;      //
-float Ipitch_f = 0.0;
-float Ipitch_f_old = 0.0;
-float dpitch = 0.0;      //pitch Angle
-float dpitch_old = 0.0;
-float dpitch_f = 0.0;
-float dpitch_f_old = 0.0;
+float theta_roll = 0.0;      //pitch Angle
+float theta_roll_old = 0.0;
+float theta_roll_f = 0.0;
+float theta_roll_f_old = 0.0;
+float dtheta_roll = 0.0;      //pitch Angle
+float dtheta_roll_old = 0.0;
+float dtheta_roll_f = 0.0;
+float dtheta_roll_f_old = 0.0;
 
 //*********************************** kalman filter variables **************************//
 float theta_kalman = 0.0;
@@ -284,18 +281,22 @@
 int roll_update_flag = 0;
 
 //*********************************** other variables **************************//
-float angle_set = 0.0; // for roll bias
+float angle_set = 0.0; 
+float slope_angle_set = 0.0; // for roll bias
+float slope_gain_roll = 1.0;
+float slope_gain_gyro = 1.0;
+float torque = 0.0;
 
 float r_wheel = 0.0333; //車輪半徑
 float b = 0.1665;       //輪距長
 float r_pipe = 0.2;     //圓管半徑
 
 // direction pwm
-int servopulsewidth = 1330; //700us ~ 2300us (0 - 180)
+int servopulsewidth = 1265; //700us ~ 2300us (0 - 180)
 
 // L&R DC motor
 float v = 0.0;      //Velocity
-float sample_speed = 5.7;
+float sample_speed = 6.5;//6.5
 float pwm1_duty = 0.5f;
 float pwm2_duty = 0.5f;
 float e_1 = 0.0;
@@ -332,8 +333,6 @@
     v = ( -wheel_L.getAngularSpeed() + wheel_R.getAngularSpeed() )/2.0; //*r_wheel;
     theta = ( wheel_L.getPulses() + wheel_R.getPulses() )/3850.0*pi;//*2.0; //1560/2.0*r_wheel/b
 
-//    Itheta = Itheta_old + theta*T;
-//    Itheta_old = Itheta;
     dtheta = (theta - dtheta_old)/T;
     dtheta_old = theta;
     dtheta_f = lpf(dtheta, dtheta_f_old, 1.0);
@@ -349,29 +348,33 @@
     if(robot_Mode == Idle_Mode) 
     {
         //bluetooth.printf("Idle_Mode\n");
-        theta_PID.error[0] = 0.0;
-        theta_PID.error[1] = 0.0;
-        theta_PID.error[2] = 0.0;
-        theta_PID.output = 0.0;
+        theta_PID.error[0] = 0.0f;
+        theta_PID.error[1] = 0.0f;
+        theta_PID.error[2] = 0.0f;
+        theta_PID.output = 0.0f;
 
-        speed_PID.error[0] = 0.0;
-        speed_PID.error[1] = 0.0;
-        speed_PID.error[2] = 0.0;
-        speed_PID.output = 0.0;
+        speed_PID.error[0] = 0.0f;
+        speed_PID.error[1] = 0.0f;
+        speed_PID.error[2] = 0.0f;
+        speed_PID.output = 0.0f;
 
-        unknown = 0.0;
-        unknown_old = 0.0;
+        unknown = 0.0f;
+        unknown_old = 0.0f;
 
         theta_flag = 1;
 
-        theta = 0.0;
-        angle_set = 0.0;
+        theta = 0.0f;
+        angle_set = 0.0f;
+        slope_angle_set = 0.0f;
+        slope_gain_roll = 1.0f;
+        slope_gain_gyro = 1.0f;
+        initial_roll = 0.0f;
         
         kalman_flag = 0;
-        angle_desired = 0.0;
+        angle_desired = 0.0f;
         K_kalman = 0.0f;
-        sigma = 5.0f*pi/180.0f; // initial
-        theta_kalman = -4.0f*pi/180.0f;
+        sigma = (3.0f*pi/180.0f)*(3.0f*pi/180.0f); // initial
+        theta_kalman = 0.0f;
         
         pwm1_duty = 0.5f;
         pwm2_duty = 0.5f;
@@ -380,8 +383,8 @@
         e_2 = 0.0;
         e_3 = 0.0;
 
-        mx106.servo(SERVO_ID,0x430,0x100);//speed 0x3ff
-        arm_pwm.pulsewidth_us(1330);
+        mx106.servo(SERVO_ID,0x500,0x100);//speed 0x3ff
+        arm_pwm.pulsewidth_us(1265);
         normal_counter = 0;
         angle_counter = 0;
     }
@@ -393,8 +396,8 @@
         e_2 = -1.0f*(0.7f*dtheta_f + 18.0f*gama_f + 25.0f*dgama_f);   //有一點震盪,收斂速度快
 
         //Idle Mode 和 Normal Mode 切換 仍有機會爆衝 改為 Climb Stop Mode 則無 //暴衝是因為counter問題
-        pwm1_duty = 0.5f - (e_1 - 1.0f*e_2/12.0f)*1.1f; // 除以12是因為電池電壓
-        pwm2_duty = 0.5f + (e_1 + 1.0f*e_2/12.0f)*1.1f;
+        pwm1_duty = 0.5f - (e_1 - 1.0f*e_2/12.0f)*1.05f; // 除以12是因為電池電壓
+        pwm2_duty = 0.5f + (e_1 + 1.0f*e_2/12.0f)*1.05f;
     }
 
     else if(robot_Mode == Idle2Turn_Mode) 
@@ -433,7 +436,8 @@
 
     else if(robot_Mode == TurnHalf_Mode) 
     {
-        if(theta_flag == 1) {
+        if(theta_flag == 1) 
+        {
             wheel_L.reset();
             wheel_R.reset();
             theta = 0;
@@ -453,12 +457,13 @@
         speed_PID.Compute(sample_speed, v);
 
         if(normal_counter == 250) {
-            e_3 = (6.5f*theta_gyro_f + 0.6f*dtheta_gyro_f);
+            e_3 = (6.5f*theta_gyro_slope_f + 0.6f*dtheta_gyro_slope_f);
         } else {
-            e_3 = 6.5f*(-angle_set);
+//            e_3 = (10.0f*theta_roll_f + 2.52f*dtheta_roll_f);
+            e_3 = 6.5f*slope_gain_gyro*(-angle_set);
             normal_counter = normal_counter+1;
-
         }
+        
         pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f); // 除以12是因為電池電壓
         pwm2_duty = 0.5f + (speed_PID.output + 1.0f*e_3/12.0f);
 
@@ -467,44 +472,41 @@
     else if(robot_Mode == Roll_Mode) 
     {
         speed_PID.Compute(sample_speed, v);
-        e_3 = (10.0f*roll_f + 2.52f*droll_f);;
-
-        pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f); // 除以12是因為電池電壓
+        e_3 = (10.0f*theta_roll_f + 2.52f*dtheta_roll_f);
+        pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f);
         pwm2_duty = 0.5f + (speed_PID.output + 1.0f*e_3/12.0f);
-
     }
 
-    else if(robot_Mode == Fusion1_Mode) 
-    {
-        kalman_filter();
-        speed_PID.Compute(sample_speed, v);
-
-        if(normal_counter == 200) 
-        {
-            e_3 = (6.5f*gyro_roll_fusion1_f + 0.6f*dgyro_roll_fusion1_f);
-        } 
-        else 
-        {
-            e_3 = 6.5f*(-angle_set);
-            normal_counter = normal_counter+1;
-        }
-
-        //e_3 = -1.0f*(-1.2155f*dtheta_f+ -4.0099f*roll_f + -6.0179f*droll_f);
-        //-0.7091  -16.4003  -16.0558 可以爬但震盪很嚴重
-        //-1.7155   -4.0099   -4.0179 可以爬 震盪可接受
-        //-1.7155   -4.0099   -6.0179 OK
-        //-1.7155   -4.0099   -4.0179
-
-        pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f); // 除以12是因為電池電壓
-        pwm2_duty = 0.5f + (speed_PID.output + 1.0f*e_3/12.0f);
-
-    } 
+//    else if(robot_Mode == Fusion1_Mode) 
+//    {
+//        kalman_filter();
+//        speed_PID.Compute(sample_speed, v);
+//
+//        if(normal_counter == 200) 
+//        {
+//            e_3 = (6.5f*gyro_roll_fusion1_f + 0.6f*dgyro_roll_fusion1_f);
+//        } 
+//        else 
+//        {
+//            e_3 = 6.5f*(-angle_set);
+//            normal_counter = normal_counter+1;
+//        }
+//
+//        //e_3 = -1.0f*(-1.2155f*dtheta_f+ -4.0099f*roll_f + -6.0179f*droll_f);
+//        //-0.7091  -16.4003  -16.0558 可以爬但震盪很嚴重
+//        //-1.7155   -4.0099   -4.0179 可以爬 震盪可接受
+//        //-1.7155   -4.0099   -6.0179 OK
+//        //-1.7155   -4.0099   -4.0179
+//
+//        pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f); // 除以12是因為電池電壓
+//        pwm2_duty = 0.5f + (speed_PID.output + 1.0f*e_3/12.0f);
+//    } 
     
     else if(robot_Mode == OpenLoop_Mode) 
     {   
         speed_PID.Compute(sample_speed, v);
         
-        e_3 = 6.5f*(-angle_set);
+        e_3 = 6.5f*slope_gain_gyro*(-angle_set);
 
         pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f); // 除以12是因為電池電壓
         pwm2_duty = 0.5f + (speed_PID.output + 1.0f*e_3/12.0f);       
@@ -513,13 +515,23 @@
     else if(robot_Mode == Kalman_test_Mode) 
     {   
         speed_PID.Compute(sample_speed, v); 
-        
-        if(normal_counter >= 65)
+          
+        if(angle_set < 0.1f)
+        {    
+            if(normal_counter >= 35)
+            {
+                kalman_filter();       
+            }
+        }
+        else if(angle_set >= 0.1f)
         {
-            kalman_filter();        
-        }
-        
-        if(normal_counter == 150)
+            if(normal_counter >= 50)
+            {
+                kalman_filter();     
+            }
+        }       
+
+        if(normal_counter == 100)
         {
             e_3 = (6.5f*theta_kalman + 0.6f*dtheta_kalman);
         } 
@@ -528,7 +540,6 @@
             e_3 = 6.5f*(-angle_set);
             normal_counter = normal_counter+1;          
         }
-        
         pwm1_duty = 0.5f - (speed_PID.output - 1.0f*e_3/12.0f); // 除以12是因為電池電壓
         pwm2_duty = 0.5f + (speed_PID.output + 1.0f*e_3/12.0f);
     }
@@ -538,64 +549,97 @@
         //bluetooth.printf("Arm_up_Mode\n");
         pwm1_duty = 0.5f;
         pwm2_duty = 0.5f;
-
-        mx106.torqueMode(SERVO_ID, 0x01);
-        mx106.torque(SERVO_ID, 0x50); //55
+        initial_roll = roll;
+        
+        slope_gain_gyro = 1.0f;
+        slope_gain_roll = 1.0f;
+        
+        mx106.torqueMode(SERVO_ID, 1);
+        mx106.torque(SERVO_ID, 95); 
+//        torque = mx106.readLoad(SERVO_ID);
     }
 
-    else if(robot_Mode == Arm_up_small_Mode) 
+    else if(robot_Mode == Arm_up_strong_Mode) 
     {
         //bluetooth.printf("Arm_up_Mode\n");
         pwm1_duty = 0.5f;
         pwm2_duty = 0.5f;
-
+        initial_roll = roll;
         mx106.torqueMode(SERVO_ID, 0x01);
-        mx106.torque(SERVO_ID, 0x45);  //50
+        
+        if(initial_roll >= -0.314f) // 18 degree
+        {
+            slope_gain_roll = 1.35f;
+            slope_gain_gyro = 1.7f; //1.4 for large angle, 1.65 for 4-5 degree
+            mx106.torque(SERVO_ID, 120);  
+        }
+        else if ((initial_roll < -0.314f)&&(initial_roll >= -0.384)) // 18~22 degree
+        {
+            slope_gain_roll = 1.42f;
+            slope_gain_gyro = 2.8f;
+            mx106.torque(SERVO_ID, 155);  
+        }
+        else if ((initial_roll < -0.384f)&&(initial_roll >= -0.524)) // 22~30 degree
+        {
+            slope_gain_roll = 1.5f;
+            slope_gain_gyro = 3.5f;
+            mx106.torque(SERVO_ID, 170);  
+        }    
     }
 
-    else if(robot_Mode == Arm_down_Mode) {
+    else if(robot_Mode == Arm_down_Mode) 
+    {
         //bluetooth.printf("ArmDown_Mode\n");
         pwm1_duty = 0.5f;
         pwm2_duty = 0.5f;
 
         mx106.torqueMode(SERVO_ID, 0x00);
-        mx106.servo(SERVO_ID,0x430,0x100);
+        mx106.servo(SERVO_ID,0x510,0x100);
     }
 
     else if(robot_Mode == Set_large_phi_Mode) 
-    {
-        arm_pwm.pulsewidth_us(1130); //
-        angle_set = 0.122f;//12 degree -> 6 degree    
-        angle_desired = -7.0f*pi/180;    
+    {// 8 degree 
+        arm_pwm.pulsewidth_us(1090); //1260 -> 0 16.7=1deg
+        angle_set = 0.140f;   
+//        slope_angle_set = 0.140f*slope_gain_roll; 
+        theta_kalman = -angle_set;    
     }
     else if(robot_Mode == Set_phi_Mode) 
-    {
-        arm_pwm.pulsewidth_us(1160); //原點1330  16.7=1deg
-        //angle_set = 0.15; //10 degree -> 5 degree
-        angle_set = 0.088f; //刻意給的穩態誤差~theta command
-        //1330 0.0
-        angle_desired = -6.0f*pi/180;
+    {// 7 degree
+        arm_pwm.pulsewidth_us(1110); //1330 -> 0  16.7=1deg
+        angle_set = 0.120f; //刻意給的穩態誤差~theta command     
+//        slope_angle_set = 0.120f*slope_gain_roll;
+        theta_kalman = -angle_set;
     } 
     else if(robot_Mode == Set_middle_phi_Mode) 
-    {
-        arm_pwm.pulsewidth_us(1210); //1200
-        angle_set = 0.072f; // 8 degree -> 4 degree
-        angle_desired = -4.5f*pi/180;
-        //arm_pwm.pulsewidth_us(1280); //1200
-        //angle_set = 0.047; // 8 degree -> 4 degree
+    {// 5(5.5) degree
+        arm_pwm.pulsewidth_us(1140); //1200
+        angle_set = 0.087f; 
+//        slope_angle_set = 0.087f*slope_gain_roll;
+        theta_kalman = -angle_set;
     }
     else if(robot_Mode == Set_small_phi_Mode) 
+    {// 4 degree
+        arm_pwm.pulsewidth_us(1165);//1165
+        angle_set = 0.073f; //0.07
+//        slope_angle_set = 0.07f*slope_gain_roll;
+        theta_kalman = -angle_set;
+    }
+    else if(robot_Mode == Set_zero_phi_Mode) 
     {
-        arm_pwm.pulsewidth_us(1250);
-        angle_set = 0.047f; // 5 degree -> 2.5 degree
-        angle_desired = -3.0f*pi/180;
-    } 
+        arm_pwm.pulsewidth_us(1265);
+        angle_set = 0.0175f;
+//        slope_angle_set = 0.0175f*slope_gain_roll;
+        theta_kalman = -angle_set;
+    }
+    else if(robot_Mode == Set_still_Mode) 
+    {
+        arm_pwm.pulsewidth_us(1245);
+        angle_set = 0.019f;
+//        slope_angle_set = 0.019f*slope_gain_roll;
+        theta_kalman = -angle_set;
+    }
     
-    else if(robot_Mode == reset_Mode) 
-    {
-        //reset_sensor();
-    }
-
     else 
     {
         //bluetooth.printf("%c\n", robot_Mode);
@@ -621,8 +665,8 @@
         e_2 = 0.0;
         e_3 = 0.0;
 
-        mx106.servo(SERVO_ID,0x430,0x100);//speed 0x3ff
-        arm_pwm.pulsewidth_us(1330);
+        mx106.servo(SERVO_ID,0x500,0x100);//speed 0x3ff
+        arm_pwm.pulsewidth_us(1265);
         normal_counter = 0;
     }
 
@@ -633,7 +677,7 @@
     TIM1->CCER |= 64; //enable ch2 complimentary output
 
     uart_send();
-    
+     
 }
 //******************************************* main *************************************//
 
@@ -658,10 +702,6 @@
 
     theta_PID.SetOutputLimits(0.5, -0.5);
     speed_PID.SetOutputLimits(0.5, -0.5);
-
-    mx106.setMaxTorque(SERVO_ID,1024);
-    mx106.setHoldingTorque(SERVO_ID,0x01);
-
 }
 
 /**************************** sensor *********************************/
@@ -721,6 +761,10 @@
     w1 = u1;
     w2 = u2;
     w3 = u3;
+    
+    gyro1 = w1; //initial_roll<0
+    gyro2 = cos(-initial_roll)*w2 - sin(-initial_roll)*w3;
+    gyro3 = sin(-initial_roll)*w2 + cos(-initial_roll)*w3;
 
 }//real_sensor_value(void)end
 
@@ -741,33 +785,30 @@
     gs1_hat_g = cos(theta_sf)*gs1_hat_n - sin(theta_sf)*gs2_hat_n;
     gs2_hat_g = sin(theta_sf)*gs1_hat_n + cos(theta_sf)*gs2_hat_n;
     gs3_hat_g = gs3_hat_n;
-
+    
+    gs1_hat_s = gs1_hat_g;
+    gs2_hat_s = cos(-initial_roll)*gs2_hat_g - sin(-initial_roll)*gs3_hat_g;
+    gs3_hat_s = sin(-initial_roll)*gs2_hat_g + cos(-initial_roll)*gs3_hat_g;
 }
 
 /******************************* angle_measure ***************************/
 void angle_measure(void)
 {    
 //****************************** phi angle 前傾角 (position in pipe)**********************//
-    phi = atan(gs2_hat_g / gs3_hat_g);
-    if((gs2_hat_g <= 0)&&(gs3_hat_g <= 0)) {
+    phi = atan(gs2_hat_s / gs3_hat_s);
+    if((gs2_hat_s <= 0)&&(gs3_hat_s <= 0)) {
         phi = phi;
-    } else if((gs2_hat_g <= 0)&&(gs3_hat_g > 0)) {
+    } else if((gs2_hat_s <= 0)&&(gs3_hat_s > 0)) {
         phi = pi + phi;
-    } else if((gs2_hat_g > 0)&&(gs3_hat_g > 0)) {
+    } else if((gs2_hat_s > 0)&&(gs3_hat_s > 0)) {
         phi = pi + phi;
-    } else if((gs2_hat_g > 0)&&(gs3_hat_g <= 0)) {
+    } else if((gs2_hat_s > 0)&&(gs3_hat_s <= 0)) {
         phi = 2.0f*pi + phi;
     }
-//    phi_f = lpf(phi, phi_f_old, 1.0);
-//    phi_f_old = phi_f;
-//    dphi = (phi - dphi_old)/T;
-//    dphi_old = phi;
-//    dphi_f = lpf(dphi, dphi_f_old, 1.0);
-//    dphi_f_old = dphi_f;
-
 
 //********************** theta from gyro ****************************************//
     theta_gyro = atan(w1/w2);
+//    theta_gyro = atan(w1/w2);
     theta_gyro_f = lpf(theta_gyro, theta_gyro_f_old, 1.0); //2.0會有點不穩定
     theta_gyro_f_old = theta_gyro_f;
     dtheta_gyro = (theta_gyro - dtheta_gyro_old)/T;
@@ -775,8 +816,17 @@
     dtheta_gyro_f = lpf(dtheta_gyro, dtheta_gyro_f_old, 1.0);
     dtheta_gyro_f_old = dtheta_gyro_f;
 
+//********************** theta from gyro with slope****************************************//
+    theta_gyro_slope = atan(gyro1/gyro2);
+    theta_gyro_slope_f = lpf(theta_gyro_slope, theta_gyro_slope_f_old, 1.0); //2.0會有點不穩定
+    theta_gyro_slope_f_old = theta_gyro_slope_f;
+    dtheta_gyro_slope = (theta_gyro_slope - dtheta_gyro_slope_old)/T;
+    dtheta_gyro_slope_old = theta_gyro_slope;
+    dtheta_gyro_slope_f = lpf(dtheta_gyro_slope, dtheta_gyro_slope_f_old, 1.0);
+    dtheta_gyro_slope_f_old = dtheta_gyro_slope_f;
+
 //********************** roll ************************************//
-    roll = atan(gs1_hat_g/g) + 0.025f - angle_set; // robot roll     //+0.0264 initial error
+    roll = atan(gs1_hat_g/g);// + 0.025f - angle_set; // robot roll     //+0.0264 initial error
     roll_f = lpf(roll, roll_f_old, 1.0);
     roll_f_old = roll_f;
     droll = (roll - droll_old)/T;
@@ -800,13 +850,13 @@
 //    Igama_f_old = Igama_f;
 
 //**************************** gyro_roll_fusion1 *******************************//
-    gyro_roll_fusion1 = (roll-0.8f*angle_set + (1.0f+sin(phi))*theta_gyro)*0.3846f; //*0.4f
-    gyro_roll_fusion1_f = lpf(gyro_roll_fusion1, gyro_roll_fusion1_f_old, 1.0);
-    gyro_roll_fusion1_f_old = gyro_roll_fusion1_f;
-    dgyro_roll_fusion1 = (gyro_roll_fusion1 - dgyro_roll_fusion1_old)/T;
-    dgyro_roll_fusion1_old = gyro_roll_fusion1;
-    dgyro_roll_fusion1_f = lpf(dgyro_roll_fusion1, dgyro_roll_fusion1_f_old, 1.0);
-    dgyro_roll_fusion1_f_old = dgyro_roll_fusion1_f;
+//    gyro_roll_fusion1 = (roll-0.8f*angle_set + (1.0f+sin(phi))*theta_gyro)*0.3846f; //*0.4f
+//    gyro_roll_fusion1_f = lpf(gyro_roll_fusion1, gyro_roll_fusion1_f_old, 1.0);
+//    gyro_roll_fusion1_f_old = gyro_roll_fusion1_f;
+//    dgyro_roll_fusion1 = (gyro_roll_fusion1 - dgyro_roll_fusion1_old)/T;
+//    dgyro_roll_fusion1_old = gyro_roll_fusion1;
+//    dgyro_roll_fusion1_f = lpf(dgyro_roll_fusion1, dgyro_roll_fusion1_f_old, 1.0);
+//    dgyro_roll_fusion1_f_old = dgyro_roll_fusion1_f;
 
 //**************************** gyro_roll_fusion2 *******************************//
 //    gyro_roll_fusion2 = -1.0f*sin(phi)*(roll+angle_set) + (cos(phi)*cos(phi))*theta_gyro; // 原地、小角度ok;大角度失敗;有震盪 
@@ -825,19 +875,18 @@
     dgyro_roll_fusion2_f = lpf(dgyro_roll_fusion2, dgyro_roll_fusion2_f_old, 1.0);
     dgyro_roll_fusion2_f_old = dgyro_roll_fusion2_f;
 
-//********************** pitch ************************************//
+//********************** theta_roll ************************************//
     
-//    pitch = 0.5f*sin(phi)*(roll+angle_set) + (1.5f-0.5f*cos(phi)*cos(phi))*theta_gyro;;
-//    pitch_f = lpf(pitch, pitch_f_old, 1.0);
-//    pitch_f_old = pitch_f;
-//    Ipitch = Ipitch + pitch_f*T;
-//    Ipitch_f = lpf(Ipitch, Ipitch_f_old, 18.0);
-//    Ipitch_f_old = Ipitch_f;
-//    dpitch = (pitch - dpitch_old)/T;
-//    dpitch_old = pitch;
-//    dpitch_f = lpf(dpitch, dpitch_f_old, 1.0);
-//    dpitch_f_old = dpitch_f;
-    
+    theta_roll = atan(gs1_hat_g/g) - initial_roll - slope_angle_set;
+    theta_roll_f = lpf(theta_roll, theta_roll_f_old, 1.0);
+    theta_roll_f_old = theta_roll_f;
+//    Itheta_roll = Itheta_roll + theta_roll_f*T;
+//    Itheta_roll_f = lpf(Itheta_roll, Itheta_roll_f_old, 18.0);
+//    Itheta_roll_f_old = Itheta_roll_f;
+    dtheta_roll = (theta_roll - dtheta_roll_old)/T;
+    dtheta_roll_old = theta_roll;
+    dtheta_roll_f = lpf(dtheta_roll, dtheta_roll_f_old, 1.0);
+    dtheta_roll_f_old = dtheta_roll_f;   
 }
 
 /******************************* uart_send ***************************/
@@ -852,19 +901,19 @@
 //    display[4] = 100*sin(phi);//pwm1_duty;//pwm1_duty;
 //    display[5] = 100*normal_counter; //*gama_try*180/pi;
     
-    display[0] = 100*gyro_roll_fusion1_f*180/pi;
-    display[1] = 100*theta_kalman*180/pi;
-    display[2] = 100*theta_gyro_f*180/pi;
-    display[3] = 1000*K_kalman;
-    display[4] = 100*sin(phi);
-    display[5] = 100*theta_gyro*180/pi;
+    display[0] = 100*theta_roll_f*180/pi;
+    display[1] = 100*theta_gyro_f*180/pi;
+    display[2] = 100*theta_gyro*180/pi;
+    display[3] = 100*theta_gyro_slope_f*180/pi;
+    display[4] = 100*initial_roll*180/pi; //e_3
+    display[5] = 100*sin(phi);
     
 //    display[0] = 100*ax;
 //    display[1] = 100*ay;
 //    display[2] = 100*az;
-//    display[3] = 100*w1;
-//    display[4] = 100*w2;
-//    display[5] = 100*w3;
+//    display[3] = 100*gyro1;
+//    display[4] = 100*gyro2;
+//    display[5] = 100*gyro3;
     
     separate();
 
@@ -904,22 +953,9 @@
 
 /**************** kalman filter ****************/
 void kalman_filter(void)
-{
-    roll_theta = -(roll + angle_set)/sin(phi);
-    
-    gain = 5.0f;
-    if(angle_set == 0.0f)
-        Q_gyro = 0.8125f*pi/180.0f*gain;
-    else if(angle_set == 0.047f)
-        Q_gyro = 1.32f*pi/180.0f*gain;
-    else if(angle_set == 0.072f)
-        Q_gyro = 1.7804f*pi/180.0f*gain;
-    else if(angle_set == 0.088f)
-        Q_gyro = 1.7154f*pi/180.0f*gain;
-    else if(angle_set == 0.122f)
-        Q_gyro = 1.8021f*pi/180.0f*gain;
-    
-    R = 0.000001;
+{    
+    Q_gyro = 0.15f;  
+    R = 0.0000001;
     
     // prediction
     theta_kalman_bar = theta_kalman;
@@ -942,7 +978,6 @@
          
     dtheta_kalman = (theta_kalman - dtheta_kalman_old)/T;
     dtheta_kalman_old = theta_kalman;
-
 }
 
 /**************** recursive_least_squares ****************/
--- a/mbed.bld	Fri Apr 28 06:34:09 2017 +0000
+++ b/mbed.bld	Mon Jul 17 02:19:28 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/ef9c61f8c49f
\ No newline at end of file
+https://mbed.org/users/mbed_official/code/mbed/builds/22da6e220af6
\ No newline at end of file