yusaku aoki
LCD表示系の整理。現状の問題としては、配列への左右移動距離の記憶ができていない様子。2走目で常にHIGH_SPEEDとなってしまうので、エンコーダパルス関係の蓄積がうまくできているか?左右同じ情報が演算されていないか？といった部分を疑ってデバッグする必要がある。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 7:cfbf8d4a4d36
- Parent:
- 6:afd8f0d02c8d
- Child:
- 8:15b4e1d7a2c5
--- a/main.cpp Tue Aug 27 05:37:18 2019 +0000
+++ b/main.cpp Wed Aug 28 00:13:57 2019 +0000
@@ -1,10 +1,6 @@
-////ライントレースサンプルver3
-///モータ速度制御プログラムを追加。
-///ラインセンサPD制御を追加。
-///M_KP,M_KDは別途straight_speed_controlのプログラムで検証した値をセットする。
-///フォトリフレクタのゲイン、ラインセンサの各種成分の調整を行い、
-///ラインをきれいにトレースできるよう挑戦する。
-
+////ライントレースサンプルver5
+///マーカ検知機能の追加
+///スタートマーカ検知後、ゴールマーカの検知で停止するプログラム
#include "mbed.h"
#include "CRotaryEncoder.h"
#include "TB6612.h"
@@ -16,6 +12,10 @@
#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
#define INTERRUPT_TIME 1000 //割りこみ周期[us]
#define GRAY 0.2f //フォトリフレクタデジタル入力の閾値
+ //シリアル通信でSensor_Digital値を確認し調整する。
+#define MARKER_WIDTH 10 //マーカ幅[mm]（ビニルテープ幅19[mm]以内）
+ //コースの傷によってマーカ誤検知する場合は値を大きくする。
+#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
//モータ速度のゲイン関連(むやみに調整しない)
#define M_KP 0.002f //P(比例)制御成分
#define M_KD 0.001f //D(微分)制御成分
@@ -28,8 +28,6 @@
//ラインセンサ各種制御成分
#define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい
#define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
-
-
//////////☆★☆★☆★☆★☆★//////////////
//機体状態の定義
@@ -67,7 +65,8 @@
float Sensor_D=0.0f; //ラインセンサD(微分成分)制御量
float Sensor_PD=0.0f; //ラインセンサP,D成分の合計
long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納
-long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]
+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 Target_Speed_A=0,Target_Speed_B=0; //目標速度
long int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納
@@ -77,10 +76,15 @@
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 =0x00; //機体状態
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;
void sensor_analog_read(){
S1_Data=s1.read();
S2_Data=s2.read();
@@ -91,28 +95,46 @@
S7_Data=s7.read();
S8_Data=s8.read();
}
+
+
void sensor_digital_read(){//８つのフォトリフレクタの入力を8ビットのデジタルパターンに変換
+ Sensor_Cnt=0;
Old_Sensor_Digital=Sensor_Digital;
- if(S1_Data>GRAY)Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。)
- else Sensor_Digital &= 0x7F; //7ビット目のみマスク（0にする。）
- if(S2_Data>GRAY)Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xBF; //6ビット目のみマスク（0にする。）
- if(S3_Data>GRAY)Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xDF; //5ビット目のみマスク（0にする。）
- if(S4_Data>GRAY)Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xEF; //4ビット目のみマスク（0にする。）
- if(S5_Data>GRAY)Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xF7; //3ビット目のみマスク（0にする。）
- if(S6_Data>GRAY)Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xFB; //2ビット目のみマスク（0にする。）
- if(S7_Data>GRAY)Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xFD; //1ビット目のみマスク（0にする。）
- if(S8_Data>GRAY)Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。)
- else Sensor_Digital &= 0xFE; //0ビット目のみマスク（0にする。）
+ if(S1_Data>GRAY){
+ Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。)
+ }else Sensor_Digital &= 0x7F; //7ビット目のみマスク（0にする。）
+ if(S2_Data>GRAY){
+ Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。)
+ Sensor_Cnt++;
+ }else Sensor_Digital &= 0xBF; //6ビット目のみマスク（0にする。）
+ if(S3_Data>GRAY){
+ Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。)
+ Sensor_Cnt++;
+ }else Sensor_Digital &= 0xDF; //5ビット目のみマスク（0にする。）
+ if(S4_Data>GRAY){
+ Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。)
+ Sensor_Cnt++;
+ }else Sensor_Digital &= 0xEF; //4ビット目のみマスク（0にする。）
+ if(S5_Data>GRAY){
+ Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。)
+ Sensor_Cnt++;
+ }else Sensor_Digital &= 0xF7; //3ビット目のみマスク（0にする。）
+ if(S6_Data>GRAY){
+ Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。)
+ Sensor_Cnt++;
+ }else Sensor_Digital &= 0xFB; //2ビット目のみマスク（0にする。）
+ if(S7_Data>GRAY){
+ Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。)
+ Sensor_Cnt++;
+ }else Sensor_Digital &= 0xFD; //1ビット目のみマスク（0にする。）
+ if(S8_Data>GRAY){
+ Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。)
+ }else Sensor_Digital &= 0xFE; //0ビット目のみマスク（0にする。）
}
void Machine_Status_Set(){
- Old_Machine_Status=Machine_Status;
+ Old_Machine_Status=Machine_Status;
+
//機体がライン中央に位置するとき
if(Sensor_Digital&RUN_COURSE_CENTER )Machine_Status|=RUN_COURSE_CENTER;
else Machine_Status &= 0xE7;//ライン中央情報のマスク
@@ -131,10 +153,46 @@
}
void timer_interrupt(){
+
//ラインセンサ情報取得
sensor_analog_read();
sensor_digital_read();
+
+ //機体状態の取得
Machine_Status_Set();
+
+ //交差点の認識
+ if(Sensor_Cnt>=CROSS_JUDGE )Cross_Flag=1;//ラインセンサ4つ以上検知状態の時は交差点を示す。
+
+ //各種マーカの検知
+ Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避
+ if(Sensor_Digital&0x81){ //マーカセンサ検知時
+ Marker_Pass_Flag=1; //マーカ通過中フラグをON
+ if(Sensor_Digital&0x80)Corner_Flag=1; //コーナセンサの検知
+ if(Sensor_Digital&0x01)SG_Flag=1; //スタートゴールセンサの検知
+ if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない
+ }else Marker_Pass_Flag=0;//マーカ通過終了
+
+ //マーカ通過後、マーカ種類判別
+ 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回目
+ SG_Cnt=1;
+ }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目
+ Machine_Status|=STOP; //機体停止状態へ
+ SG_Cnt=0;
+ }else if(Corner_Flag==1){//コーナマーカの時
+ //地区大会では何もしない
+ }
+ }else{//マーカではなく、誤検知だった場合。
+ //何もしない
+ }
+ Corner_Flag=0;
+ 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);
@@ -152,8 +210,12 @@
Distance_B=(Enc_Count_B*PULSE_TO_UM);
Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
+ 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);
@@ -191,7 +253,11 @@
}else if(Machine_Status&RUN_COURSE_ROUT){
motor_a=-(TURN_POWER);
motor_b=-(-TURN_POWER);
- }else{
+ }else if(Cross_Flag==1){//交差点通過中
+ motor_a=-0.3;//交差点なので控えめの速度で直進
+ motor_b=-0.3;
+ }
+ else{
motor_a=-Motor_A_Pwm;
motor_b=-Motor_B_Pwm;
}
@@ -205,8 +271,9 @@
timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
while(1){
wait(1);
- PC.printf("Sensor_Digital:0x%02x\t Old_Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t Old_Machine_Status:0x%02x\r\n"\
- ,Sensor_Digital,Old_Sensor_Digital\
- ,Machine_Status,Old_Machine_Status);//表示
+ //センサ検知情報、機体情報の表示
+ PC.printf("Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t",Sensor_Digital,Machine_Status);
+ //スタートゴールマーカ検知回数の表示
+ PC.printf("SG_Cnt:%d\r\n",SG_Cnt);
}
}
\ No newline at end of file