yusaku aoki
LCD表示系の整理。現状の問題としては、配列への左右移動距離の記憶ができていない様子。2走目で常にHIGH_SPEEDとなってしまうので、エンコーダパルス関係の蓄積がうまくできているか?左右同じ情報が演算されていないか？といった部分を疑ってデバッグする必要がある。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 23:def04f2e894f
- Parent:
- 22:b40f7d0c0f12
- Child:
- 24:5fd9d128e587
--- 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;
////モータ現在速度の取得