yusaku aoki
LCD表示系の整理。現状の問題としては、配列への左右移動距離の記憶ができていない様子。2走目で常にHIGH_SPEEDとなってしまうので、エンコーダパルス関係の蓄積がうまくできているか?左右同じ情報が演算されていないか？といった部分を疑ってデバッグする必要がある。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 27:9e234e087e70
- Parent:
- 26:7d4150475983
--- a/main.cpp Sat Nov 23 07:09:32 2019 +0000
+++ b/main.cpp Mon Nov 25 03:17:25 2019 +0000
@@ -1,6 +1,6 @@
-//簡易コースを使って走行テストすること。
-//加減速走行時は1msごとの割込み処理の精度が重要になるので
-//printf等の時間のかかる処理は控えるようにする。
+//LCD表示系を一新した
+//1行目に走行モード、機体状態、記憶マーカ数を表示
+//2行目にDefault_Speedの値を表示する。
////ライントレースサンプル
@@ -11,28 +11,28 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 750 //1走目の基本速度
-#define LOW_SPEED 450 //2走目の低速速度[mm/sec]
-#define MEDIUM_SPEED 800 //2走目の低速速度[mm/sec]
-#define HIGH_SPEED 1500 //2走目の低速速度[mm/sec]
-#define DEFAULT_HIGH_SPEED 950 //3走目の低速速度[mm/sec]
-#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm]
+#define DEFAULT_SPEED 300 //1走目の基本速度
+#define LOW_SPEED 300 //2走目の低速速度[mm/sec]
+#define MEDIUM_SPEED 500 //2走目の低速速度[mm/sec]
+#define HIGH_SPEED 750 //2走目の低速速度[mm/sec]
+#define DEFAULT_HIGH_SPEED 700 //3走目の低速速度[mm/sec]
+#define STOP_DISTANCE 100000 //停止距離200000[um]⇒20[cm]
#define TURN_POWER 0.6 //コースアウト時の旋回力
#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um]
-#define INTERRUPT_TIME 3000 //割りこみ周期[us]
+#define INTERRUPT_TIME 1000 //割りこみ周期[us]
#define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値
//シリアル通信でSensor_Digital値を確認し調整する。
#define MARKER_WIDTH 10000 //マーカ幅[um]（ビニルテープ幅19000[um]以内）
//コースの傷によってマーカ誤検知する場合は値を大きくする。
#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
//モータ速度のゲイン関連(むやみに調整しない)
-#define M_KP 0.001f //P(比例)制御成分
-#define M_KD 0.0005f //D(微分)制御成分
+#define M_KP 0.002f //P(比例)制御成分
+#define M_KD 0.001f //D(微分)制御成分
//フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
#define S_K1 1.0f //float演算させる値には必ずfを付ける
-#define S_K2 2.4f //2倍
-#define S_K3 4.7f //4倍
+#define S_K2 2.0f //2倍
+#define S_K3 3.0f //4倍
//ラインセンサ各種制御成分
@@ -46,7 +46,7 @@
#define S_KD_HIGH 1.0f
#define S_KD_MEDIUM 1.0f
#define S_KD_LOW 1.0f
-#define S_KD_DEFAULT_HIGH 1.0f
+#define S_KD_DEFAULT_HIGH 0.7f
//////////☆★☆★☆★☆★☆★//////////////
@@ -77,19 +77,7 @@
AnalogIn s7(A1);
AnalogIn s8(A0);
-/*
-AnalogIn s1(A1);
-AnalogIn s2(D3);
-AnalogIn s3(A6);
-AnalogIn s4(A5);
-AnalogIn s5(A4);
-AnalogIn s6(A3);
-AnalogIn s7(A2);
-AnalogIn s8(A0);
-*/
-
///////////////////////////////////////
-//Serial PC(USBTX,USBRX);
CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ
Ticker timer; //タイマ割込み用
@@ -100,7 +88,7 @@
//使用変数の定義
int Sw_Ptn = PULL; //スイッチの現在情報
int Old_Sw_Ptn = PULL; //スイッチの過去の情報
-int Sw = 0; //スイッチの押された回数
+int Sw_Cnt = 0; //スイッチの押された回数
char Coner_str[3]; //LCD用配列。コーナマーカ通過数表示
///各ラインセンサ情報格納用
double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
@@ -153,9 +141,11 @@
int SG_Cnt=0; //スタート・ゴールセンサのマーカ通過数
int Cross_Flag=0; //交差点を通過したことを示すフラグ
long int Imaginary_Speed=0; //曲率演算にて求めた２走目以降の仮想走行速度
-float course_data[100][3]; //コース情報の記憶用配列マーカ数は行数分まで対応し、
+long long int course_data[100][3]; //コース情報の記憶用配列マーカ数は行数分まで対応し、
//列情報は左タイヤ走行距離、右タイヤ走行距離、演算で求めた2走目走行速度を格納する。
int Row=0; //行情報を格納。マーカ通過ごとに加算し、走行終了後リセットされる。
+int Marker_Cnt=0;
+
void sensor_analog_read(){//ラインセンサの情報取得処理。
S1_Data=s1.read();
@@ -232,20 +222,21 @@
void corner_curvature(){
course_data[Row][0]=0;//予め配列の情報は初期化しておく。
course_data[Row][1]=0;//
+ Imaginary_Speed=0;
////まずはマーカ間の走行距離を取得する。
course_data[Row][0]=(Memory_Enc_Count_A*PULSE_TO_UM);//左タイヤのマーカ間の走行距離を格納
course_data[Row][1]=(Memory_Enc_Count_B*PULSE_TO_UM);//右タイヤのマーカ間の走行距離を格納
//左右タイヤの走行距離の差分を用いた曲率演算
- if(((course_data[Row][0])>(course_data[Row][1]*4.0f)) || ((course_data[Row][1])>(course_data[Row][0]*4.0f))){//左が右より80%以上早いか20%以上遅いとき
- Imaginary_Speed=Low_Speed;//急カーブは低速とする。
- }else if((course_data[Row][0]>(course_data[Row][1]*2.5f)) || (course_data[Row][1]>(course_data[Row][0]*2.5f))){//左が右より50%以上早いか50%以上遅いとき
+ if(((course_data[Row][0])<(course_data[Row][1]*1.2f)) || ((course_data[Row][1])<(course_data[Row][0]*1.2f))){//左右の回転差が20%以下の時
+ Imaginary_Speed=High_Speed;//高速とする。
+ }else if((course_data[Row][0]<(course_data[Row][1]*1.5)) || (course_data[Row][1]<(course_data[Row][0]*1.5))){//左右の回転差が５0%以下の時
Imaginary_Speed=Medium_Speed;//中カーブは中速とする。
- }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;//緩いカーブは高速とする。
+ }else if((course_data[Row][0]<(course_data[Row][1]*2)) || (course_data[Row][1]<(course_data[Row][0]*2))){//左右の回転差が100%以下の時
+ Imaginary_Speed=Low_Speed;//低速
}
else{
- Imaginary_Speed=High_Speed;//直線は高速とする。
+ Imaginary_Speed=Low_Speed;//低速
}
course_data[Row][2]=Imaginary_Speed; //仮想の演算速度を格納
@@ -274,6 +265,36 @@
}
+/////LCD表示用関数。機体の現在状態を表示する。
+void display_print(void){
+ char lcd_top_str[8],lcd_low_str[8]; //lcd表示用配列を用意 各行分
+ char stop_or_go; //機体停止状態'-'か走行状態'G'を格納
+ //////表示クリア
+ lcd.locate(0,0);
+ lcd.print(" ");
+ lcd.locate(0,1);
+ lcd.print(" ");
+
+ ////////0行目の表示//////////////////////////////
+ if(Machine_Status&STOP){ //機体停止状態のとき
+ stop_or_go='-';
+ }else if(!(Machine_Status&STOP)){ //機体走行状態のとき
+ stop_or_go='G';
+ }
+ sprintf(lcd_top_str,"%d:%c %03d",Sw_Cnt,stop_or_go,Marker_Cnt);//0行の表示文字の整理
+ lcd.locate(0,0); //表示位置を0行目にセット
+ lcd.print(lcd_top_str); //表示
+
+ ////////1行目の表示/////////////////////////////
+ if(Sw_Cnt<=2){ //1走目か2走目のとき
+ sprintf(lcd_low_str,"SPD:%04d",Default_Speed); //1行の表示文字の整理
+ }else sprintf(lcd_low_str,"SPD:%04d",Default_High_Speed); //3走目のとき
+ lcd.locate(0,1); //表示位置を1行目にセット
+ lcd.print(lcd_low_str); //表示
+}
+
+
+
//タイマ割り込み1[ms]周期
void timer_interrupt(){
@@ -290,24 +311,12 @@
//エンコーダパルス数の取得
Enc_Count_A=encoder_a.Get();
Enc_Count_B=-encoder_b.Get();
- //////コース記憶に使用するエンコーダパルス数の蓄積処理
- Memory_Enc_Count_A+=Enc_Count_A;
- Memory_Enc_Count_B+=Enc_Count_B;
-
- //////機体停止時の徐行用の走行距離演算
- if(Machine_Status&STOP){//機体停止状態の時
- if(Stop_Distance>=STOP_DISTANCE){
- Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積
- Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積
- }
- if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400;
- if(Enc_A_Rotate>6400)Enc_A_Rotate=6400;
- if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
- if(Enc_B_Rotate>6400)Enc_B_Rotate=6400;
- if(Stop_Distance<0)Stop_Distance=0;
- if(Stop_Distance>STOP_DISTANCE)Stop_Distance=STOP_DISTANCE;
- }
+ if(!(Machine_Status&STOP)){//機体走行中のとき
+ //////コース記憶に使用するエンコーダパルス数の蓄積処理
+ Memory_Enc_Count_A+=Enc_Count_A;
+ Memory_Enc_Count_B+=Enc_Count_B;
+ }
//各種マーカの検知
Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避
if(Sensor_Digital&0x81){ //マーカセンサ検知時
@@ -324,44 +333,54 @@
if(Cross_Flag==1); //交差点の時は何もしない
else if((SG_Flag==1)&&(SG_Cnt==0)){ //ゴールスタートマーカの時⇒1回目
SG_Cnt=1;
- if(Sw==0){ //1走目のとき
- Memory_Enc_Count_A=0;
- Memory_Enc_Count_B=0;
- corner_curvature(); //曲率に応じた速度推定処理
+ if(Sw_Cnt==1){ //1走目のとき
+ corner_curvature(); //曲率に応じた速度推定処理
+ Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア
+ Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア
Row++;
- }else if(Sw==1){ //2走目のとき
- ++Row;//一つ先のコース情報を読む
- second_speed_control();//記憶情報から目標速度を設定する処理
+ }else if(Sw_Cnt==2){ //2走目のとき
+ second_speed_control(); //記憶情報から目標速度を設定する処理
+ Row++; //次のコース情報へ
}else{//3走目は定速で高速走行
Target_Speed_A=Default_High_Speed;
Target_Speed_B=Default_High_Speed;
}
}else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目
- corner_curvature(); //曲率に応じた速度推定処理
- Row++;
+ if(Sw_Cnt==1){
+ corner_curvature(); //曲率に応じた速度推定処理
+ Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア
+ Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア
+ }else if(Sw_Cnt==2){ //2走目のとき 何もしない
+ }else; //3走目のとき 何もしない
+ Marker_Cnt=Row; //マーカカウント数をLCD表示用に退避させる
Machine_Status|=STOP; //機体停止状態へ
- Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化
- SG_Cnt=0;//スタート・ゴールマーカ情報のリセット
- }else if(Corner_Flag==1){//コーナマーカの時
- if(Sw==0){ //1走目のとき
- corner_curvature(); //曲率に応じた速度推定処理
+ Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化
+ SG_Cnt=0; //スタート・ゴールマーカ情報のリセット
+ }else if(Corner_Flag==1){ //コーナマーカの時
+ if(Sw_Cnt==1){ //1走目のとき
+ corner_curvature(); //曲率に応じた速度推定処理
+ Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア
+ Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア
Row++;
- }else if(Sw==1){ //2走目のとき
- ++Row;//一つ先のコース情報を読む
- second_speed_control();//記憶情報から目標速度を設定する処理
+ }else if(Sw_Cnt==2){ //2走目のとき
+ second_speed_control(); //記憶情報から目標速度を設定する処理
+ Row++; //次のコース情報へ
}else{//3走目は定速で高速走行
Target_Speed_A=Default_High_Speed;
Target_Speed_B=Default_High_Speed;
}
}
- }else{//マーカではなく、誤検知だった場合。
- //何もしない
- }
+ }else;//マーカではなく、誤検知だった場合。何もしない
+
+
+
+ //////マーカ通過後は各種マーカ判定用フラグ、パラメータのリセット処理を行う。
Corner_Flag=0;
SG_Flag=0;
Cross_Flag=0;
- Marker_Run_Distance=0;//マーカ通過距離情報リセット
- }
+ Marker_Run_Distance=0; //マーカ通過距離情報リセット
+
+ }/////////////////各種マーカ判定処理の終了
//センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
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);
@@ -419,13 +438,33 @@
motor_b=-Motor_B_Pwm;
}
}else{//停止状態の時はモータへの出力は無効
- motor_a=0;
- motor_b=0;
- }
+ if(Stop_Distance<STOP_DISTANCE){
+ motor_a=-Motor_A_Pwm/4;
+ motor_b=-Motor_B_Pwm/4;
+
+ }else{
+ motor_a=0;
+ motor_b=0;
+ }
+ }
+ //////機体停止時の徐行用の走行距離演算
+ if(Machine_Status&STOP){//機体停止状態の時
+ if(Stop_Distance>=STOP_DISTANCE){
+ Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積
+ Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積
+ }
+ if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400;
+ if(Enc_A_Rotate>6400)Enc_A_Rotate=6400;
+ if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
+ if(Enc_B_Rotate>6400)Enc_B_Rotate=6400;
+ if((Distance_A>=0)&&(Distance_B>=0))Stop_Distance+=(Distance_A+Distance_B)/2;
+ if(Stop_Distance<0)Stop_Distance=0;
+ if(Stop_Distance>STOP_DISTANCE)Stop_Distance=STOP_DISTANCE;
+ }
if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
Marker_Run_Distance+=(Distance_A+Distance_B)/2;
- }
-
+ }
+
//割込み終了時の各種パラメータリセット処理
//エンコーダ関連情報のリセット
encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット
@@ -440,78 +479,34 @@
int main() {
timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
lcd.cls();//表示クリア
- lcd.locate(0,0);
- lcd.print("STOP");
- lcd.locate(0,1);
-
while(1){
Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得
- if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時
- //sprintf(MemoryA_Str,"%d",memory_A);
- //sprintf(MemoryB_Str,"%d",memory_B);
- lcd.locate(0,0);
- lcd.print(" ");
- lcd.locate(0,0);
- lcd.print("STOP");
-
- //lcd.locate(0,0);
- //lcd.print(MemoryA_Str);
- //lcd.locate(0,1);
- //lcd.print(MemoryB_Str);
- wait(5);
- Gray=DEFAULT_GRAY;
- Medium_Speed=MEDIUM_SPEED;
- Sw++;
- }
+ display_print(); //LCDに現在の機体状態を表示
- if(Machine_Status&STOP){//機体停止状態の時
- Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整
- sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換
- sprintf(Coner_str,"%d",Row); //マーカ通過数の表示
- lcd.locate(0,0);
- lcd.print(Gray_Str);
- lcd.print(" ");
- lcd.print(Coner_str);
-
- lcd.locate(0,1);
- lcd.print(" ");
-
- if(Sw==0){
- Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
- sprintf(Speed_Str,"%04d",Default_Speed);//速度情報文字列変換
- }
- else if(Sw==1) {//2走目。加減速による高速走行
- 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);
- sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換
- }
- }
- lcd.locate(0,1);
- lcd.print(Speed_Str);
+ if(Machine_Status&STOP){//機体停止状態の時
+ if(Sw_Cnt<=2)Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16); //1,2走目標準速度調整
+ else if(Sw_Cnt>2)Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16); //3走目
+ }
if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
if(Machine_Status&STOP){//機体停止状態の時
- lcd.locate(0,0);
- lcd.print(" ");
- lcd.locate(0,0);
- lcd.print("GO!!");
- wait(2);
- if(Sw==0){ //1走目のとき
+ Sw_Cnt++;
+ wait(0.5);
+ display_print(); //LCDに現在の機体状態を表示
+ wait(1.5);
+ Corner_Flag=0;
+ SG_Flag=0;
+ Cross_Flag=0;
+ Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化
+ SG_Cnt=0; //スタート・ゴールマーカ情報のリセット
+ Marker_Run_Distance=0;//マーカ通過距離情報リセット
+ if(Sw_Cnt<=2){ //1,2走目のとき
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;
@@ -520,20 +515,16 @@
}
Stop_Distance=0;
Machine_Status&=0x7F;//ストップ状態解除
-
-
-
+
}else{//機体走行中であったとき
//各種フラグのクリア
Corner_Flag=0;
SG_Flag=0;
Cross_Flag=0;
+ Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化
+ SG_Cnt=0; //スタート・ゴールマーカ情報のリセット
Marker_Run_Distance=0;//マーカ通過距離情報リセット
- Machine_Status |= STOP;//機体停止状態にする。
- lcd.locate(0,0);
- lcd.print(" ");
- lcd.locate(0,0);
- lcd.print("STOP");
+ Machine_Status |= STOP;//機体停止状態にする。
}
}
}