Revision 23:def04f2e894f, committed 2019-11-22

Comitter:

GGU

Date:

Fri Nov 22 10:52:35 2019 +0000

Parent:

22:b40f7d0c0f12

Commit message:

test;

Changed in this revision

diff -r b40f7d0c0f12 -r def04f2e894f main.cpp
--- a/main.cpp	Fri Nov 22 04:20:20 2019 +0000
+++ b/main.cpp	Fri Nov 22 10:52:35 2019 +0000
@@ -31,6 +31,14 @@
 //ラインセンサ各種制御成分
 #define     S_KP            1.0f    //ラインセンサ比例成分。大きいほど曲がりやすい
 #define     S_KD            0.5f    //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+
+#define     S_KP1           0.5f
+#define     S_KP2           0.8f
+#define     S_KP3           1.2f
+
+#define     S_KD1           0.2f
+#define     S_KD2           0.4f
+#define     S_KD3           0.6f
 //////////☆★☆★☆★☆★☆★//////////////
 
 
@@ -104,7 +112,10 @@
 char    MemoryA_Str[5];     //LCD閾値表示用文字列
 char    MemoryB_Str[5];
 long int Distance_A=0,Distance_B=0;          //タイヤ移動距離を格納[mm]
-long int Distance_memory_A=0, Distance_memory_B=0;   
+long int Distance_memory_A=0, Distance_memory_B=0;
+
+long int SSKP=0;
+long int SSKD=0;
 
 long int Marker_Run_Distance=0; 
 long int Speed_A=0,  Speed_B=0;              //現在速度
@@ -137,7 +148,6 @@
 
 int Row=0;    //行変数
 float course_data[100][3];  //記憶走行用配列
-float curvature=0;
 
 void sensor_analog_read(){
     S1_Data=s1.read();
@@ -215,15 +225,23 @@
                 course_data[Row][1]=(float)Distance_memory_B;
                 //キョクリツ演算
                 if((Distance_memory_A>(Distance_memory_B*1.8)) || (Distance_memory_A<(Distance_memory_B*0.2))){//左が右より80%以上早いか20%以上遅いとき
-                    Imaginary_Speed=Low_Speed;         
+                    Imaginary_Speed=Low_Speed;
+                    SSKP=S_KP1;
+                    SSKD=S_KD1;        
                 }else if((Distance_memory_A>(Distance_memory_B*1.5)) || (Distance_memory_A<(Distance_memory_B*0.5))){//左が右より50%以上早いか50%以上遅いとき
                     Imaginary_Speed=Medium_Speed;
+                    SSKP=S_KP2;
+                    SSKD=S_KD2;
                 }else if((Distance_memory_A>(Distance_memory_B*1.2)) || (Distance_memory_A<(Distance_memory_A*0.8))){//左が右より20%以上早いか20%以上遅いとき⇒直線
                     Imaginary_Speed=High_Speed;
+                    SSKP=S_KP3;
+                    SSKD=S_KD3;
                     }
                 else{
                     Imaginary_Speed=High_Speed;
-            }                              
+                    SSKP=S_KP;
+                    SSKD=S_KD;
+                }                              
                 course_data[Row][2]=Imaginary_Speed; //仮想の演算速度ヲ格納                              
                 PC.printf("left:%.2f\t",course_data[Row][0]);        
                 PC.printf("right:%.2f\t",course_data[Row][1]);            
@@ -239,36 +257,7 @@
             else{
                 Distance_memory_A=(Enc_Count_A*PULSE_TO_UM);
                 Distance_memory_B=(Enc_Count_B*PULSE_TO_UM);                                
-                }
-                
- /*               
-            if((motor_a>(motor_b*1.8)) || (motor_a<(motor_b*0.2))){//左が右より80%以上早いか20%以上遅いとき
-                    Imaginary_Speed=Low_Speed;
-                    Target_Speed_A=Imaginary_Speed;
-                    Target_Speed_B=Imaginary_Speed;
-          
-                }else if((motor_a>(motor_b*1.5)) || (motor_a<(motor_b*0.5))){//左が右より50%以上早いか50%以上遅いとき
-                    Imaginary_Speed=Medium_Speed;
-                    Target_Speed_A=Imaginary_Speed;
-                    Target_Speed_B=Imaginary_Speed;
-                }else if((motor_a>(motor_b*1.2)) || (motor_a<(motor_b*0.8))){//左が右より20%以上早いか20%以上遅いとき⇒直線
-                    Imaginary_Speed=High_Speed;
-                    Target_Speed_A=Imaginary_Speed;
-                    Target_Speed_B=Imaginary_Speed;
-                    }
-                else{
-                    Imaginary_Speed=High_Speed;
-                    Target_Speed_A=Imaginary_Speed;                    
-                    Target_Speed_B=Imaginary_Speed;
-            }
-*/            
-            /*course_data[Row][0]=(float)Distance_memory_A;
-            course_data[Row][1]=(float)Distance_memory_B;
-            course_data[Row][2]=Imaginary_Speed;
-            PC.printf("left:%.2f\t",course_data[Row][0]);        
-            PC.printf("right:%.2f\t",course_data[Row][1]);            
-            PC.printf("speed:%.2f\n\r",course_data[Row][2]);*/
-                    
+                }                    
 }
 
 //タイマ割り込み1[ms]周期
@@ -323,8 +312,8 @@
     All_Sensor_Data=-(S2_Data*S_K3+S3_Data*S_K2+S4_Data*S_K1)+(S5_Data*S_K1+S6_Data*S_K2+S7_Data*S_K3);
     Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避
     Sensor_Diff[0]=All_Sensor_Data;
-    Sensor_P=All_Sensor_Data*S_KP;                  //ラインセンサ比例成分の演算       
-    Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD;  //ラインセンサ微分成分の演算
+    Sensor_P=All_Sensor_Data*SSKP;                  //ラインセンサ比例成分の演算       
+    Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*SSKD;  //ラインセンサ微分成分の演算
     Sensor_PD=Sensor_P+Sensor_D; 
           
     ////モータ現在速度の取得