島根短大 ライントレーサ制作チーム
1走目→記憶走行 2走目→加減速走行
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 14:7ed78f52f40e
- Parent:
- 13:97be8e29ae50
- Child:
- 15:cfe79ebfcb25
--- a/main.cpp Thu Aug 29 01:07:15 2019 +0000
+++ b/main.cpp Mon Nov 11 06:25:06 2019 +0000
@@ -7,12 +7,13 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
#define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec]
-#define TURN_POWER 0.47 //コースアウト時の旋回力
-#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
+#define DEFAULT_SPEED1 900
+#define TURN_POWER 0.6 //コースアウト時の旋回力
+#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um]
#define INTERRUPT_TIME 1000 //割りこみ周期[us]
-#define DEFAULT_GRAY 0.5f //フォトリフレクタデジタル入力の閾値
+#define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値
//シリアル通信でSensor_Digital値を確認し調整する。
-#define MARKER_WIDTH 15 //マーカ幅[mm]（ビニルテープ幅19[mm]以内）
+#define MARKER_WIDTH 10000 //マーカ幅[um]（ビニルテープ幅19000[um]以内）
//コースの傷によってマーカ誤検知する場合は値を大きくする。
#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
//モータ速度のゲイン関連(むやみに調整しない)
@@ -21,12 +22,12 @@
//フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
#define S_K1 1.0f //float演算させる値には必ずfを付ける
-#define S_K2 2.0f //2倍
-#define S_K3 4.0f //4倍
+#define S_K2 2.4f //2倍
+#define S_K3 4.7f //4倍
//ラインセンサ各種制御成分
-#define S_KP 1.07f //ラインセンサ比例成分。大きいほど曲がりやすい
-#define S_KD 0.7f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define S_KP 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい
+#define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
//////////☆★☆★☆★☆★☆★//////////////
@@ -47,13 +48,13 @@
//デジタル入力オブジェクト定義
DigitalIn push_sw(D13);
/////アナログ入力オブジェクト定義//////////
-AnalogIn s1(A1);
-AnalogIn s2(D3);
-AnalogIn s3(A6);
-AnalogIn s4(A5);
-AnalogIn s5(A4);
-AnalogIn s6(A3);
-AnalogIn s7(A2);
+AnalogIn s1(D3);
+AnalogIn s2(A6);
+AnalogIn s3(A5);
+AnalogIn s4(A4);
+AnalogIn s5(A3);
+AnalogIn s6(A2);
+AnalogIn s7(A1);
AnalogIn s8(A0);
///////////////////////////////////////
Serial PC(USBTX,USBRX);
@@ -67,6 +68,9 @@
//使用変数の定義
int Sw_Ptn;
int Old_Sw_Ptn;
+int Sw=0;
+int Coner_c=0;
+char Coner_str[3];
double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
double All_Sensor_Data; //ラインセンサ総データ量
double Sensor_Diff[2]={0,0}; //ラインセンサ偏差
@@ -78,12 +82,19 @@
long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納
long int Enc_A_Rotate=0,Enc_B_Rotate=0;
+/*long int memory_A=0;
+long int memory_B=0;
+char memoryA_Str[5];
+char memoryB_Str[5];*/
+
long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]
long int Marker_Run_Distance=0;
long int Speed_A=0, Speed_B=0; //現在速度
long int Machine_Speed=DEFAULT_SPEED;
+long int Machine_Speed1=DEFAULT_SPEED1;
char Speed_Str[5]; //LCD速度表示用文字列
long int Target_Speed_A=0,Target_Speed_B=0; //目標速度
+long int Target_Speed_A1=0,Target_Speed_B1=0; //目標速度
long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納
long int Motor_B_Diff[2]={0,0}; //
float Motor_A_P,Motor_B_P; //モータ速度制御P成分
@@ -201,7 +212,7 @@
Machine_Status|=STOP; //機体停止状態へ
SG_Cnt=0;
}else if(Corner_Flag==1){//コーナマーカの時
- //地区大会では何もしない
+ Coner_c++;
}
}else{//マーカではなく、誤検知だった場合。
//何もしない
@@ -247,16 +258,41 @@
encoder_b.Set(0);
/////各モータの目標速度の設定
- Target_Speed_A=Machine_Speed;
- Target_Speed_B=Machine_Speed;
+ if(Sw==0){
+
+ Motor_A_Diff[1]=Motor_A_Diff[0];
+ Motor_B_Diff[1]=Motor_B_Diff[0];
+
+ Target_Speed_A=Machine_Speed;
+ Target_Speed_B=Machine_Speed;
+
+ Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
+ Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
+ }
+ else {
+
+ Motor_A_Diff[1]=Motor_A_Diff[0];
+ Motor_B_Diff[1]=Motor_B_Diff[0];
+
+ Target_Speed_A1=Machine_Speed1;
+ Target_Speed_B1=Machine_Speed1;
+
+ Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
+ Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
+ }
/////モータの速度制御
//過去の速度偏差を退避
- Motor_A_Diff[1]=Motor_A_Diff[0];
+ /* Motor_A_Diff[1]=Motor_A_Diff[0];
Motor_B_Diff[1]=Motor_B_Diff[0];
//現在の速度偏差を取得。
Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
+
+ Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
+ Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
+ */
+
//P成分演算
Motor_A_P=Motor_A_Diff[0]*M_KP;
Motor_B_P=Motor_B_Diff[0]*M_KP;
@@ -311,37 +347,52 @@
while(1){
Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得
- if((!(Old_Machine_Status&=STOP))&&(Machine_Status&=STOP)){//走行終了時
+ if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時
lcd.locate(0,0);
lcd.print(" ");
lcd.locate(0,0);
- lcd.print("STOP");
+ lcd.print("STOP");
+ Sw++;
+
}
+
if(Machine_Status&STOP){//機体停止状態の時
Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整
sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換
+ sprintf(Coner_str,"%d",Coner_c);
lcd.locate(0,0);
lcd.print(" ");
lcd.locate(0,0);
- lcd.print(Gray_Str);
- Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
- sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換
+ lcd.print(Gray_Str);
+ lcd.print(" ");
+ lcd.print(Coner_str);
+ if(Sw==0){
+ Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
+ sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換
+ }else {
+ Machine_Speed1=DEFAULT_SPEED1+(Enc_B_Rotate/16);
+ sprintf(Speed_Str,"%04d",Machine_Speed1);//速度情報文字列変換
+ }
+
lcd.locate(0,1);
lcd.print(" ");
lcd.locate(0,1);
- lcd.print(Speed_Str);
+ lcd.print(Speed_Str);
+
+
}
if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
if(Machine_Status&STOP){//機体停止状態の時
+ Coner_c=0;
lcd.locate(0,0);
lcd.print(" ");
lcd.locate(0,0);
lcd.print("GO!!");
wait(2);
- Machine_Status&=0x7F;//ストップ状態解除
+ Machine_Status&=0x7F;//ストップ状態解除
}else{//機体走行中であったとき
//各種フラグのクリア
Corner_Flag=0;