最終調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 11:2cd6f8be124e
- Parent:
- 10:e1eb10665472
- Child:
- 12:dc4c569248d7
diff -r e1eb10665472 -r 2cd6f8be124e main.cpp --- a/main.cpp Wed Aug 28 09:44:41 2019 +0000 +++ b/main.cpp Wed Aug 28 23:14:20 2019 +0000 @@ -1,4 +1,4 @@ -////ライントレースサンプルver7 +////ライントレースサンプルver7_1 #include "mbed.h" #include "CRotaryEncoder.h" #include "TB6612.h" @@ -6,9 +6,7 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define FIRST_SPEED 500 //1走目の基本速度[mm/sec] -#define SECOND_SPEED 750 //2走目の基本速度[mm/sec] -#define THIRD_SPEED 1000 //3走目の基本速度[mm/sec] +#define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec] #define TURN_POWER 0.5 //コースアウト時の旋回力 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] #define INTERRUPT_TIME 1000 //割りこみ周期[us] @@ -45,9 +43,6 @@ #define SECOND_RUN 0x02 //機体停止状態 #define TUARD_RUN 0x01 //機体設定モード -#define FIRST 1 -#define SECOND 2 -#define THIRD 3 //デジタル入力オブジェクト定義 DigitalIn push_sw(D13); @@ -72,7 +67,6 @@ //使用変数の定義 int Sw_Ptn; int Old_Sw_Ptn; -int Run_Mode=FIRST; 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}; //ラインセンサ偏差 @@ -84,7 +78,8 @@ 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 Default_Speed=FIRST_SPEED; +long int Machine_Speed=DEFAULT_SPEED; +char Speed_Str[5]; //LCD速度表示用文字列 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納 long int Motor_B_Diff[2]={0,0}; // @@ -233,7 +228,7 @@ if(Machine_Status&STOP){//機体停止状態の時 Enc_B_Rotate+=Enc_Count_B;//モード設定用に右エンコーダ値の蓄積 - if(Enc_B_Rotate<0)Enc_B_Rotate=0; + if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400; if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; } if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 @@ -246,8 +241,8 @@ encoder_b.Set(0); /////各モータの目標速度の設定 - Target_Speed_A=Default_Speed; - Target_Speed_B=Default_Speed; + Target_Speed_A=Machine_Speed; + Target_Speed_B=Machine_Speed; /////モータの速度制御 //過去の速度偏差を退避 @@ -305,7 +300,8 @@ lcd.locate(0,0); lcd.print("STOP"); lcd.locate(0,1); - lcd.print("FIRST"); + sprintf(Speed_Str,"%04d",Machine_Speed); + lcd.print(Speed_Str); while(1){ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 @@ -316,38 +312,12 @@ lcd.print("STOP"); } if(Machine_Status&STOP){//機体停止状態の時 - if((Enc_B_Rotate/1600)<1){//右エンコーダが1回転未満であったら - if(Run_Mode!=FIRST){ - lcd.cls();//表示クリア - lcd.locate(0,0); - lcd.print("STOP"); - lcd.locate(0,1); - lcd.print("FIRST"); - Run_Mode=FIRST; - Default_Speed=FIRST_SPEED; - } - }else if(((Enc_B_Rotate/1600)>=1)&&((Enc_B_Rotate/1600)<2)){//右エンコーダが1回転以上、2回転未満であったら - if(Run_Mode!=SECOND){ - lcd.cls();//表示クリア - lcd.locate(0,0); - lcd.print("STOP"); - lcd.locate(0,1); - lcd.print("SECOND");//2走目のモードであることを示す。 - Run_Mode=SECOND; - Default_Speed=SECOND_SPEED; - } - }else if(((Enc_B_Rotate/1600)>=2)&&((Enc_B_Rotate/1600)<3)){//右エンコーダが2回転以上、3回転未満であったら - if(Run_Mode!=THIRD){ - lcd.cls();//表示クリア - lcd.locate(0,0); - lcd.print("STOP"); - lcd.locate(0,1); - lcd.print("THIRD");//2走目のモードであることを示す。 - Run_Mode=THIRD; - Default_Speed=THIRD_SPEED; - } - } - + Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 + sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換 + lcd.locate(0,1); + lcd.print(" "); + lcd.locate(0,1); + lcd.print(Speed_Str); }