島根短大 ライントレーサ制作チーム
2019/11/23 AMに作成。 New_Generationの加減速走行修正プログラム 機体実証をしていないので、うまく動作する確証はないが、 テンプレとして使用すること。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Revision 25:0dbd9ab9fd86, committed 2019-11-23
- Comitter:
- yusaku0125
- Date:
- Sat Nov 23 02:41:50 2019 +0000
- Parent:
- 24:5fd9d128e587
- Commit message:
- 2019/11/23 11:41
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 5fd9d128e587 -r 0dbd9ab9fd86 main.cpp
--- a/main.cpp Sat Nov 23 01:59:23 2019 +0000
+++ b/main.cpp Sat Nov 23 02:41:50 2019 +0000
@@ -1,3 +1,8 @@
+//簡易コースを使って走行テストすること。
+//加減速走行時は1msごとの割込み処理の精度が重要になるので
+//printf等の時間のかかる処理は控えるようにする。
+
+
////ライントレースサンプル
#include "mbed.h"
#include "CRotaryEncoder.h"
@@ -29,17 +34,19 @@
#define S_K2 2.4f //2倍
#define S_K3 4.7f //4倍
-//ラインセンサ各種制御成分
-#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_KP_DEFAULT 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい
+#define S_KP_HIGH 0.5f
+#define S_KP_MEDIUM 0.8f
+#define S_KP_LOW 1.2f
+#define S_KP_DEFAULT_HIGH 1.0f
-#define S_KD1 0.2f
-#define S_KD2 0.4f
-#define S_KD3 0.6f
+#define S_KD_DEFAULT 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define S_KD_HIGH 0.2f
+#define S_KD_MEDIUM 0.4f
+#define S_KD_LOW 0.6f
+#define S_KD_DEFAULT_HIGH 0.5f
//////////☆★☆★☆★☆★☆★//////////////
@@ -116,12 +123,12 @@
////↓左右タイヤの走行距離格納。1走目のコース記憶処理に使用する。マーカ通過ごとにリセット。
long int Distance_memory_A=0, Distance_memory_B=0;
-long int SSKP=0;////???
-long int SSKD=0;////???
+long int S_Kp=S_KP_DEFAULT;////センサP成分
+long int S_Kd=S_KD_DEFAULT;////センサD成分
long int Marker_Run_Distance=0;
long int Speed_A=0, Speed_B=0; //機体の現在速度を格納
-long int default_speed = DEFAULT_SPEED; //1走目の標準速度
+long int Default_Speed = DEFAULT_SPEED; //1走目の標準速度
long int Low_Speed = LOW_SPEED; //2走目以降の低速
long int Medium_Speed = MEDIUM_SPEED; //2走目以降の中速
long int High_Speed = HIGH_SPEED; //2走目以降の高速
@@ -231,37 +238,38 @@
//左右タイヤの走行距離の差分を用いた曲率演算
if(((course_data[Row][0])>(course_data[Row][1]*1.8f)) || ((course_data[Row][1])>(course_data[Row][0]*1.8f))){//左が右より80%以上早いか20%以上遅いとき
- Imaginary_Speed=Low_Speed;//急カーブは低速とする。
- SSKP=S_KP1;//ゲインを高めに設定
- SSKD=S_KD1;
+ Imaginary_Speed=Low_Speed;//急カーブは低速とする。
}else if((course_data[Row][0]>(course_data[Row][1]*1.5f)) || (course_data[Row][1]>(course_data[Row][0]*1.5f))){//左が右より50%以上早いか50%以上遅いとき
Imaginary_Speed=Medium_Speed;//中カーブは中速とする。
- SSKP=S_KP2;//ゲインを中に設定
- SSKD=S_KD2;
}else if((course_data[Row][0]>(course_data[Row][1]*1.2f)) || (course_data[Row][1]>(course_data[Row][0]*1.2f))){//左が右より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]);
-// PC.printf("speed:%.2f\n\r",course_data[Row][2]);
+
}
//2走目の加減速走行に使用。1走目で演算したマーカ間の仮想速度を機体目標速度に設定する。
-void second_speed_control(){
+void second_speed_control(){
+ if(course_data[Row][2]==Default_Speed){//標準速度区間のとき
+ S_Kp=S_KP_DEFAULT;
+ S_Kd=S_KD_DEFAULT;
+ }else if(course_data[Row][2]==Low_Speed){//低速走行区間のとき
+ S_Kp=S_KP_LOW;
+ S_Kd=S_KD_LOW;
+ }else if(course_data[Row][2]==Medium_Speed){//中速走行区間のとき
+ S_Kp=S_KP_MEDIUM;
+ S_Kd=S_KD_MEDIUM;
+ }else if(course_data[Row][2]==High_Speed){//高速走行区間のとき
+ S_Kp=S_KP_HIGH;
+ S_Kd=S_KD_HIGH;
+ }else{//
+ S_Kp=S_KP_DEFAULT_HIGH;
+ S_Kd=S_KD_DEFAULT_HIGH;
+ }
Target_Speed_A=course_data[Row][2];//記憶走行で演算した仮想速度を使う
- Target_Speed_B=course_data[Row][2];
-// 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]);
+ Target_Speed_B=course_data[Row][2];
}
@@ -354,8 +362,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*SSKP; //ラインセンサ比例成分の演算
- Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*SSKD; //ラインセンサ微分成分の演算
+ Sensor_P=All_Sensor_Data*S_Kp; //ラインセンサ比例成分の演算
+ Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_Kd; //ラインセンサ微分成分の演算
Sensor_PD=Sensor_P+Sensor_D;
////モータ現在速度の取得
@@ -465,12 +473,13 @@
lcd.locate(0,1);
lcd.print(Speed_Str);
if(Sw==0){
- Medium_Speed=MEDIUM_SPEED+(Enc_B_Rotate/16);//標準速度調整
- sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換
+ Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
+ sprintf(Speed_Str,"%04d",Default_Speed);//速度情報文字列変換
}
else if(Sw==1) {//2走目。加減速による高速走行
- High_Speed=HIGH_SPEED+(Enc_B_Rotate/16);
- sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換
+ Low_Speed=LOW_SPEED+(Enc_B_Rotate/16);
+ Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);
+ sprintf(Speed_Str,"%04d",Low_Speed);//速度情報文字列変換
}
else if(Sw==2)//3走目。加減速による超高速走行
Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16);
@@ -485,10 +494,25 @@
lcd.locate(0,0);
lcd.print("GO!!");
wait(2);
+ if(Sw==0){ //1走目のとき
+ S_Kp=S_KP_DEFAULT;
+ S_Kd=S_KD_DEFAULT;
+ Target_Speed_A=Default_Speed;
+ Target_Speed_B=Default_Speed;
+ }else if(Sw==1){ //2走目のとき
+ S_Kp=S_KP_DEFAULT;
+ S_Kd=S_KD_DEFAULT;
+ Target_Speed_A=Default_Speed;
+ Target_Speed_B=Default_Speed;
+ }else{ //3走目以降のとき
+ S_Kp=S_KP_DEFAULT_HIGH;
+ S_Kd=S_KD_DEFAULT_HIGH;
+ Target_Speed_A=Default_High_Speed;
+ Target_Speed_B=Default_High_Speed;
+ }
Stop_Distance=0;
Machine_Status&=0x7F;//ストップ状態解除
- Target_Speed_A=Medium_Speed;
- Target_Speed_B=Medium_Speed;
+
}else{//機体走行中であったとき