島根短大 ライントレーサ制作チーム
最終調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 24:5fd9d128e587
- Parent:
- 23:def04f2e894f
- Child:
- 25:0dbd9ab9fd86
diff -r def04f2e894f -r 5fd9d128e587 main.cpp
--- a/main.cpp Fri Nov 22 10:52:35 2019 +0000
+++ b/main.cpp Sat Nov 23 01:59:23 2019 +0000
@@ -6,31 +6,32 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 450 //1走目の基本速度[mm/sec]
-#define DEFAULT_SPEED1 700
-#define DEFAULT_SPEED2 950
-#define DEFAULT_SPEED3 800
-#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm]
-#define TURN_POWER 0.6 //コースアウト時の旋回力
-#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um]
-#define INTERRUPT_TIME 3000 //割りこみ周期[us]
-#define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値
- //シリアル通信でSensor_Digital値を確認し調整する。
-#define MARKER_WIDTH 10000 //マーカ幅[um]（ビニルテープ幅19000[um]以内）
- //コースの傷によってマーカ誤検知する場合は値を大きくする。
-#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
+#define DEFAULT_SPEED 450 //1走目の基本速度
+#define LOW_SPEED 450 //2走目の低速速度[mm/sec]
+#define MEDIUM_SPEED 700 //2走目の低速速度[mm/sec]
+#define HIGH_SPEED 800 //2走目の低速速度[mm/sec]
+#define DEFAULT_HIGH_SPEED 950 //3走目の低速速度[mm/sec]
+#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm]
+#define TURN_POWER 0.6 //コースアウト時の旋回力
+#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um]
+#define INTERRUPT_TIME 3000 //割りこみ周期[us]
+#define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値
+ //シリアル通信でSensor_Digital値を確認し調整する。
+#define MARKER_WIDTH 10000 //マーカ幅[um]（ビニルテープ幅19000[um]以内）
+ //コースの傷によってマーカ誤検知する場合は値を大きくする。
+#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
//モータ速度のゲイン関連(むやみに調整しない)
-#define M_KP 0.002f //P(比例)制御成分
-#define M_KD 0.001f //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_K1 1.0f //float演算させる値には必ずfを付ける
+#define S_K2 2.4f //2倍
+#define S_K3 4.7f //4倍
//ラインセンサ各種制御成分
-#define S_KP 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい
-#define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define S_KP 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい
+#define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
#define S_KP1 0.5f
#define S_KP2 0.8f
@@ -44,16 +45,16 @@
//スイッチ状態の定義
-#define PUSH 0 //スイッチ押したときの状態
-#define PULL 1 //スイッチ離したときの状態
+#define PUSH 0 //スイッチ押したときの状態
+#define PULL 1 //スイッチ離したときの状態
//機体状態の定義
-#define STOP 0x80 //機体停止状態
-#define RUN_START 0x40 //スタートマーカ通過
-#define RUN_COURSE_LOUT 0x20 //左コースアウト状態
-#define RUN_COURSE_CENTER 0x18 //ライン中央走行状態
-#define RUN_COURSE_ROUT 0x04 //右コースアウト状態
-#define SECOND_RUN 0x02 //機体停止状態
-#define TUARD_RUN 0x01 //機体設定モード
+#define STOP 0x80 //機体停止状態
+#define RUN_START 0x40 //スタートマーカ通過
+#define RUN_COURSE_LOUT 0x20 //左コースアウト状態
+#define RUN_COURSE_CENTER 0x18 //ライン中央走行状態
+#define RUN_COURSE_ROUT 0x04 //右コースアウト状態
+#define SECOND_RUN 0x02 //機体停止状態
+#define TUARD_RUN 0x01 //機体設定モード
//デジタル入力オブジェクト定義
@@ -84,72 +85,72 @@
Serial PC(USBTX,USBRX);
CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ
-Ticker timer; //タイマ割込み用
-TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2)
-TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
-AQM0802 lcd(I2C_SDA,I2C_SCL); //液晶制御用
+Ticker timer; //タイマ割込み用
+TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2)
+TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
+AQM0802 lcd(I2C_SDA,I2C_SCL); //液晶制御用
//使用変数の定義
-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}; //ラインセンサ偏差
-double Sensor_P =0.0f; //ラインセンサP(比例成分)制御量
-double Sensor_D =0.0f; //ラインセンサD(微分成分)制御量
-double Sensor_PD=0.0f; //ラインセンサP,D成分の合計
-char Gray_Str[5]; //LCD閾値表示用文字列
-float Gray=DEFAULT_GRAY;
-long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納
-long int Enc_A_Rotate=0,Enc_B_Rotate=0;
-long int Stop_Distance=STOP_DISTANCE;
+int Sw_Ptn = PULL; //スイッチの現在情報
+int Old_Sw_Ptn = PULL; //スイッチの過去の情報
+int Sw = 0; //スイッチの押された回数
+char Coner_str[3]; //LCD用配列。コーナマーカ通過数表示
+ ///各ラインセンサ情報格納用
+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}; //ラインセンサ偏差
+double Sensor_P =0.0f; //ラインセンサP(比例成分)制御量
+double Sensor_D =0.0f; //ラインセンサD(微分成分)制御量
+double Sensor_PD=0.0f; //ラインセンサP,D成分の合計
+char Gray_Str[5]; //LCD閾値表示用文字列
+float Gray=DEFAULT_GRAY; //白黒のしきい値設定
+long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数。現在速度推定に使用する。
+long int Enc_A_Rotate=0,Enc_B_Rotate=0; //エンコーダパルス数。停止時の機体速度調整に使用。
+long int Stop_Distance=STOP_DISTANCE; //ストップ時の徐行→停止に使う
+ /////↓エンコーダパルス数。コース記憶時のエンコーダパルスの蓄積に使用する。
+long int Memory_Enc_Count_A=0,Memory_Enc_Count_B=0;
+char MemoryA_Str[5]; //LCD表示用。左モータの走行距離格納
+char MemoryB_Str[5]; //LCD表示用。右モータの走行距離格納
+long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]。
+ //機体の現在速度の推定に使用する。タイマ割込みごとにリセットされる。
+ ////↓左右タイヤの走行距離格納。1走目のコース記憶処理に使用する。マーカ通過ごとにリセット。
+long int Distance_memory_A=0, Distance_memory_B=0;
-long int memory_A=0; //移動距離格納
-long int memory_B=0;
-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 SSKP=0;
-long int SSKD=0;
+long int SSKP=0;////???
+long int SSKD=0;////???
long int Marker_Run_Distance=0;
-long int Speed_A=0, Speed_B=0; //現在速度
-
-long int Low_Speed=DEFAULT_SPEED;
-long int Medium_Speed=DEFAULT_SPEED1;
-long int High_Speed=DEFAULT_SPEED2;
-long int High1_Speed=DEFAULT_SPEED3;
-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}; //
-float Motor_A_P,Motor_B_P; //モータ速度制御P成分
-float Motor_A_D,Motor_B_D; //モータ速度制御D成分
-float Motor_A_PD,Motor_B_PD; //モータ速度制御PD合成
-float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力
+long int Speed_A=0, Speed_B=0; //機体の現在速度を格納
+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走目以降の高速
+long int Default_High_Speed = DEFAULT_HIGH_SPEED;//3走目の高速
+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}; //
+float Motor_A_P,Motor_B_P; //モータ速度制御P成分
+float Motor_A_D,Motor_B_D; //モータ速度制御D成分
+float Motor_A_PD,Motor_B_PD; //モータ速度制御PD合成
+float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力
unsigned char Sensor_Digital =0x00;
unsigned char Old_Sensor_Digital=0x00;
int Sensor_Cnt=0;
-unsigned char Machine_Status =STOP; //機体状態
-unsigned char Old_Machine_Status=0x00; //過去の機体状態
-int Marker_Pass_Flag = 0;
-int Old_Marker_Pass_Flag=0;
-int Corner_Flag=0;
-int SG_Flag=0;
-int SG_Cnt=0;
-int Cross_Flag=0;
-long int Imaginary_Speed=0;
+unsigned char Machine_Status =STOP; //機体状態
+unsigned char Old_Machine_Status=0x00; //過去の機体状態
+int Marker_Pass_Flag = 0; //マーカ通過中であることを示すフラグ
+int Old_Marker_Pass_Flag=0; //過去のマーカ通過情報
+int Corner_Flag=0; //コーナセンサがマーカを検知したことを示すフラグ
+int SG_Flag=0; //スタート・ゴールセンサがマーカを検知したことを示すフラグ
+int SG_Cnt=0; //スタート・ゴールセンサのマーカ通過数
+int Cross_Flag=0; //交差点を通過したことを示すフラグ
+long int Imaginary_Speed=0; //曲率演算にて求めた２走目以降の仮想走行速度
+float course_data[100][3]; //コース情報の記憶用配列マーカ数は行数分まで対応し、
+ //列情報は左タイヤ走行距離、右タイヤ走行距離、演算で求めた2走目走行速度を格納する。
+int Row=0; //行情報を格納。マーカ通過ごとに加算し、走行終了後リセットされる。
-
-int Row=0; //行変数
-float course_data[100][3]; //記憶走行用配列
-
-void sensor_analog_read(){
+void sensor_analog_read(){//ラインセンサの情報取得処理。
S1_Data=s1.read();
S2_Data=s2.read();
S3_Data=s3.read();
@@ -160,8 +161,9 @@
S8_Data=s8.read();
}
-
-void sensor_digital_read(){//８つのフォトリフレクタの入力を8ビットのデジタルパターンに変換
+//８つのフォトリフレクタの入力を8ビットのデジタルパターンに変換。センサの状態をデジタルパターンで
+//扱うために使用する。（if文などの条件式でセンサ情報を使いやすいようにするため）
+void sensor_digital_read(){
Sensor_Cnt=0;
Old_Sensor_Digital=Sensor_Digital;
if(S1_Data>Gray){
@@ -196,6 +198,8 @@
}else Sensor_Digital &= 0xFE; //0ビット目のみマスク（0にする。）
}
+//機体の状態をデジタルパターンでセットする関数。モニタリング、条件分岐で
+//機体の状態が交差点通過中なのか停止状態なのか判定するために使用する。
void Machine_Status_Set(){
Old_Machine_Status=Machine_Status;
@@ -215,51 +219,52 @@
Machine_Status &= 0xFB;//右コースアウト情報のみマスク
}
}
-void coner_curvature(){
- if(Sw==0){
- Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得
- Enc_Count_B=-encoder_b.Get();
- Distance_memory_A=(Enc_Count_A*PULSE_TO_UM);
- Distance_memory_B=(Enc_Count_B*PULSE_TO_UM);
- course_data[Row][0]=(float)Distance_memory_A;
- 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;
- 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]);
- PC.printf("speed:%.2f\n\r",course_data[Row][2]);
- }
- else if(Sw==1){
- 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]);
- }
- else{
- Distance_memory_A=(Enc_Count_A*PULSE_TO_UM);
- Distance_memory_B=(Enc_Count_B*PULSE_TO_UM);
- }
+
+//コーナマーカ検知時に動作。
+//エンコーダから取得されたパルス数を距離換算し、事前用意した記憶用配列に格納する。
+void corner_curvature(){
+ course_data[Row][0]=0;//予め配列の情報は初期化しておく。
+ course_data[Row][1]=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]*1.8f)) || ((course_data[Row][1])>(course_data[Row][0]*1.8f))){//左が右より80%以上早いか20%以上遅いとき
+ Imaginary_Speed=Low_Speed;//急カーブは低速とする。
+ SSKP=S_KP1;//ゲインを高めに設定
+ SSKD=S_KD1;
+ }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(){
+ 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]);
+}
+
+
//タイマ割り込み1[ms]周期
void timer_interrupt(){
@@ -269,10 +274,29 @@
//機体状態の取得
Machine_Status_Set();
-
+
//交差点の認識
if(Sensor_Cnt>=CROSS_JUDGE )Cross_Flag=1;//ラインセンサ4つ以上検知状態の時は交差点を示す。
+ //エンコーダパルス数の取得
+ 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;
+ //////機体停止時の徐行処理に使用するエンコーダパルス数の蓄積
+ Enc_A_Rotate+=Enc_Count_A;
+ Enc_B_Rotate+=Enc_Count_B;
+
+ //////機体停止時の徐行用の走行距離演算
+ if(Machine_Status&STOP){//機体停止状態の時
+ 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;
+ }
//各種マーカの検知
Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避
if(Sensor_Digital&0x81){ //マーカセンサ検知時
@@ -282,22 +306,40 @@
if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない
}else Marker_Pass_Flag=0;//マーカ通過終了
+ if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
+ Marker_Run_Distance+=(Distance_A+Distance_B)/2;
+ }
//マーカ通過後、マーカ種類判別
if((Old_Marker_Pass_Flag==1)&&(Marker_Pass_Flag==0)){//マーカ通過後
- if(Marker_Run_Distance>MARKER_WIDTH){//マーカ幅がもっともらしいとき
- if(Cross_Flag==1);//交差点の時は何もしない
- else if((SG_Flag==1)&&(SG_Cnt==0)){//ゴールスタートマーカの時⇒1回目
+ if(Marker_Run_Distance>MARKER_WIDTH){ //マーカ幅がもっともらしいとき
+ if(Cross_Flag==1); //交差点の時は何もしない
+ else if((SG_Flag==1)&&(SG_Cnt==0)){ //ゴールスタートマーカの時⇒1回目
SG_Cnt=1;
+ if(Sw==0){ //1走目のとき
+ corner_curvature(); //曲率に応じた速度推定処理
+ Row++;
+ }else if(Sw==1){ //2走目のとき
+ ++Row;//一つ先のコース情報を読む。
+ second_speed_control();//記憶情報から目標速度を設定する処理
+ }else{//3走目は定速で高速走行
+ Target_Speed_A=Default_High_Speed;
+ Target_Speed_B=Default_High_Speed;
+ }
}else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目
Machine_Status|=STOP; //機体停止状態へ
- Row=0;
- SG_Cnt=0;
- }else if(Corner_Flag==1){//コーナマーカの時
- Distance_memory_A=0;
- Distance_memory_B=0;
- Coner_c++;
- coner_curvature();
- Row++;
+ Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化
+ SG_Cnt=0;//スタート・ゴールマーカ情報のリセット
+ }else if(Corner_Flag==1){//コーナマーカの時
+ if(Sw==0){ //1走目のとき
+ corner_curvature(); //曲率に応じた速度推定処理
+ Row++;
+ }else if(Sw==1){ //2走目のとき
+ ++Row;//一つ先のコース情報を読む
+ second_speed_control();//記憶情報から目標速度を設定する処理
+ }else{//3走目は定速で高速走行
+ Target_Speed_A=Default_High_Speed;
+ Target_Speed_B=Default_High_Speed;
+ }
}
}else{//マーカではなく、誤検知だった場合。
//何もしない
@@ -306,7 +348,7 @@
SG_Flag=0;
Cross_Flag=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);
@@ -317,76 +359,17 @@
Sensor_PD=Sensor_P+Sensor_D;
////モータ現在速度の取得
- Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得
- Enc_Count_B=-encoder_b.Get();
- Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納
+ Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納
Distance_B=(Enc_Count_B*PULSE_TO_UM);
-
- Distance_memory_A=(Enc_Count_A*PULSE_TO_UM);
- Distance_memory_B=(Enc_Count_B*PULSE_TO_UM);
-
Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
-
- if(Machine_Status&STOP){//機体停止状態の時
- Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積
- if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400;
- if(Enc_A_Rotate>6400)Enc_A_Rotate=6400;
- Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積
- 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(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
- Marker_Run_Distance+=(Distance_A+Distance_B)/2;
- }
- //エンコーダ関連情報のリセット
- Distance_A=0;
- Distance_B=0;
-
- encoder_a.Set(0);
- encoder_b.Set(0);
-
- memory_A=Distance_memory_A;
- memory_B=Distance_memory_B;
-
- /////各モータの目標速度の設定
- if(Sw==0){
- Target_Speed_A=Medium_Speed;
- Target_Speed_B=Medium_Speed;
-
- 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);
- }
- else if(Sw==1) {
- Target_Speed_A=High_Speed;
- Target_Speed_B=High_Speed;
-
- 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[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);
-*/
-
+ Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
//P成分演算
Motor_A_P=Motor_A_Diff[0]*M_KP;
@@ -405,8 +388,7 @@
if(Motor_A_Pwm>0.95f)Motor_A_Pwm=0.95f;
else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f;
if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f;
- else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f;
-
+ else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f;
//モータへの出力
if(!(Machine_Status&STOP)){//マシンが停止状態でなければ
if(Machine_Status&RUN_COURSE_LOUT){ //左端センサ振り切れた時
@@ -426,7 +408,17 @@
}else{//停止状態の時はモータへの出力は無効
motor_a=0;
motor_b=0;
- }
+ }
+
+ //割込み終了時の各種パラメータリセット処理
+ //エンコーダ関連情報のリセット
+ encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット
+ encoder_b.Set(0);
+
+ Enc_Count_A = 0;//速度制御用エンコーダパルス数情報のリセット
+ Enc_Count_B = 0;//
+ Distance_A=0; //速度制御用距離情報のリセット
+ Distance_B=0;
}
int main() {
@@ -455,17 +447,14 @@
//lcd.print(MemoryB_Str);
wait(5);
Gray=DEFAULT_GRAY;
- Medium_Speed=DEFAULT_SPEED1;
- Sw++;
- Coner_c=0;
+ Medium_Speed=MEDIUM_SPEED;
+ 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);
-
+ sprintf(Coner_str,"%d",Row); //マーカ通過数の表示
lcd.locate(0,0);
lcd.print(Gray_Str);
lcd.print(" ");
@@ -476,22 +465,21 @@
lcd.locate(0,1);
lcd.print(Speed_Str);
if(Sw==0){
- Medium_Speed=DEFAULT_SPEED1+(Enc_B_Rotate/16);//標準速度調整
+ Medium_Speed=MEDIUM_SPEED+(Enc_B_Rotate/16);//標準速度調整
sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換
}
- else if(Sw==1) {
- High_Speed=DEFAULT_SPEED2+(Enc_B_Rotate/16);
+ else if(Sw==1) {//2走目。加減速による高速走行
+ High_Speed=HIGH_SPEED+(Enc_B_Rotate/16);
sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換
}
- else if(Sw==2)
- High1_Speed=DEFAULT_SPEED3+(Enc_B_Rotate/16);
- sprintf(Speed_Str,"%04d",High1_Speed);//速度情報文字列変換
+ else if(Sw==2)//3走目。加減速による超高速走行
+ Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16);
+ sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換
}
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);
@@ -499,8 +487,8 @@
wait(2);
Stop_Distance=0;
Machine_Status&=0x7F;//ストップ状態解除
- //Target_Speed_A=Medium_Speed;
- //Target_Speed_B=Medium_Speed;
+ Target_Speed_A=Medium_Speed;
+ Target_Speed_B=Medium_Speed;
}else{//機体走行中であったとき
@@ -509,7 +497,6 @@
SG_Flag=0;
Cross_Flag=0;
Marker_Run_Distance=0;//マーカ通過距離情報リセット
-
Machine_Status |= STOP;//機体停止状態にする。
lcd.locate(0,0);
lcd.print(" ");