島根短大 ライントレーサ制作チーム
走行１回目デフォルト 走行２回目記憶走行
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 6:afd8f0d02c8d
- Parent:
- 5:f635f1f01d2d
- Child:
- 7:cfbf8d4a4d36
diff -r f635f1f01d2d -r afd8f0d02c8d main.cpp
--- a/main.cpp Mon Aug 26 09:25:22 2019 +0000
+++ b/main.cpp Tue Aug 27 05:37:18 2019 +0000
@@ -11,42 +11,52 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 500 //機体の直進速度1000[mm/s]
-#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
-#define INTERRUPT_TIME 1000 //割りこみ周期[us]
-
+#define DEFAULT_SPEED 900 //機体の直進速度1000[mm/s]
+#define TURN_POWER 0.5 //コースアウト時の旋回力
+#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
+#define INTERRUPT_TIME 1000 //割りこみ周期[us]
+#define GRAY 0.2f //フォトリフレクタデジタル入力の閾値
//モータ速度のゲイン関連(むやみに調整しない)
-#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.0f //2倍
-#define S_K3 4.0f //4倍
+#define S_K1 1.0f //float演算させる値には必ずfを付ける
+#define S_K2 2.0f //2倍
+#define S_K3 4.0f //4倍
//ラインセンサ各種制御成分
-#define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい
-#define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい
+#define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
//////////☆★☆★☆★☆★☆★//////////////
+//機体状態の定義
+#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 //機体設定モード
+
/////アナログ入力オブジェクト定義//////////
-AnalogIn s1(D3);
-AnalogIn s2(A6);
-AnalogIn s3(A5);
-AnalogIn s4(A4);
-AnalogIn s5(A3);
-AnalogIn s6(A2);
-AnalogIn s7(A1);
-AnalogIn s8(A0);
+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);
-CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
-CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ
-Ticker timer; //タイマ割込み用
+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)
+TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
//使用変数の定義
@@ -65,10 +75,13 @@
float Motor_A_P,Motor_B_P; //モータP制御成分
float Motor_A_D,Motor_B_D; //モータD制御成分
float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力
+unsigned char Sensor_Digital =0x00;
+unsigned char Old_Sensor_Digital=0x00;
+unsigned char Machine_Status =0x00; //機体状態
+unsigned char Old_Machine_Status=0x00; //過去の機体状態
-void timer_interrupt(){
- Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避
- //各種センサ情報取得
+
+void sensor_analog_read(){
S1_Data=s1.read();
S2_Data=s2.read();
S3_Data=s3.read();
@@ -76,13 +89,62 @@
S5_Data=s5.read();
S6_Data=s6.read();
S7_Data=s7.read();
- S8_Data=s8.read();
+ S8_Data=s8.read();
+}
+void sensor_digital_read(){//８つのフォトリフレクタの入力を8ビットのデジタルパターンに変換
+ 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にする。）
+}
+
+void Machine_Status_Set(){
+ Old_Machine_Status=Machine_Status;
+ //機体がライン中央に位置するとき
+ if(Sensor_Digital&RUN_COURSE_CENTER )Machine_Status|=RUN_COURSE_CENTER;
+ else Machine_Status &= 0xE7;//ライン中央情報のマスク
+ if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x40)){//左センサコースアウト時
+ Machine_Status|=RUN_COURSE_LOUT;//左コースアウト状態のビットをセット
+ }else if((Machine_Status&RUN_COURSE_LOUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){
+ //左コースアウト状態かつ機体がライン中央に復帰したとき
+ Machine_Status &= 0xDF;//左コースアウト情報のみマスク
+ }
+ if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x02)){//右センサコースアウト時
+ Machine_Status|=RUN_COURSE_ROUT;//右コースアウト状態のビットをセット
+ }else if((Machine_Status&RUN_COURSE_ROUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){
+ //右コースアウト状態かつ機体がライン中央に復帰したとき
+ Machine_Status &= 0xFB;//右コースアウト情報のみマスク
+ }
+}
+
+void timer_interrupt(){
+ //ラインセンサ情報取得
+ sensor_analog_read();
+ sensor_digital_read();
+ Machine_Status_Set();
+
//センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
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);
+ Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避
Sensor_Diff[0]=All_Sensor_Data;
Sensor_P=All_Sensor_Data*S_KP; //ラインセンサ比例成分の演算
Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD; //ラインセンサ微分成分の演算
- Sensor_PD=Sensor_P+Sensor_D;
+ Sensor_PD=Sensor_P+Sensor_D;
+
+
////モータ現在速度の取得
Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得
Enc_Count_B=-encoder_b.Get();
@@ -102,7 +164,7 @@
/////モータの速度制御
//過去の速度偏差を退避
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);
@@ -115,19 +177,36 @@
Motor_A_Pwm=Motor_A_P+Motor_A_D+Sensor_PD;
Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD;
- if(Motor_A_Pwm>1.0f)Motor_A_Pwm=1.0f;
- else if(Motor_A_Pwm<-1.0f)Motor_A_Pwm=-1.0f;
- if(Motor_B_Pwm>1.0f)Motor_B_Pwm=1.0f;
- else if(Motor_B_Pwm<-1.0f)Motor_B_Pwm=-1.0f;
- //最終的には符号を逆転して出力
- motor_a=-Motor_A_Pwm;
- motor_b=-Motor_B_Pwm;
+ 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;
+
+ //モータへの出力
+
+ if(!(Machine_Status&STOP)){//マシンが停止状態でなければ
+ if(Machine_Status&RUN_COURSE_LOUT){
+ motor_a=-(-TURN_POWER);
+ motor_b=-(TURN_POWER);
+ }else if(Machine_Status&RUN_COURSE_ROUT){
+ motor_a=-(TURN_POWER);
+ motor_b=-(-TURN_POWER);
+ }else{
+ motor_a=-Motor_A_Pwm;
+ motor_b=-Motor_B_Pwm;
+ }
+ }else{//停止状態の時はモータへの出力は無効
+ motor_a=0;
+ motor_b=0;
+ }
}
int main() {
timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
while(1){
wait(1);
- PC.printf("spd_a:%d[mm/sec] spd_b:%d[mm/sec]\r\n",Speed_A,Speed_B);//表示
+ 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);//表示
}
}
\ No newline at end of file