島根短大 ライントレーサ制作チーム
配列を使用し記憶走行を行う
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 10:e1eb10665472
- Parent:
- 9:1c28fcc1e9b8
- Child:
- 11:2cd6f8be124e
--- a/main.cpp Wed Aug 28 07:53:37 2019 +0000
+++ b/main.cpp Wed Aug 28 09:44:41 2019 +0000
@@ -1,6 +1,4 @@
-////ライントレースサンプルver5
-///マーカ検知機能の追加
-///スタートマーカ検知後、ゴールマーカの検知で停止するプログラム
+////ライントレースサンプルver7
#include "mbed.h"
#include "CRotaryEncoder.h"
#include "TB6612.h"
@@ -14,9 +12,9 @@
#define TURN_POWER 0.5 //コースアウト時の旋回力
#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
#define INTERRUPT_TIME 1000 //割りこみ周期[us]
-#define GRAY 0.2f //フォトリフレクタデジタル入力の閾値
+#define GRAY 0.6f //フォトリフレクタデジタル入力の閾値
//シリアル通信でSensor_Digital値を確認し調整する。
-#define MARKER_WIDTH 10 //マーカ幅[mm]（ビニルテープ幅19[mm]以内）
+#define MARKER_WIDTH 15 //マーカ幅[mm]（ビニルテープ幅19[mm]以内）
//コースの傷によってマーカ誤検知する場合は値を大きくする。
#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
//モータ速度のゲイン関連(むやみに調整しない)
@@ -47,6 +45,10 @@
#define SECOND_RUN 0x02 //機体停止状態
#define TUARD_RUN 0x01 //機体設定モード
+#define FIRST 1
+#define SECOND 2
+#define THIRD 3
+
//デジタル入力オブジェクト定義
DigitalIn push_sw(D13);
/////アナログ入力オブジェクト定義//////////
@@ -70,7 +72,7 @@
//使用変数の定義
int Sw_Ptn;
int Old_Sw_Ptn;
-int Push_Cnt;
+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}; //ラインセンサ偏差
@@ -78,10 +80,11 @@
double Sensor_D =0.0f; //ラインセンサD(微分成分)制御量
double Sensor_PD=0.0f; //ラインセンサP,D成分の合計
long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納
+long int Enc_B_Rotate=0;
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=0;
+long int Default_Speed=FIRST_SPEED;
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}; //
@@ -226,7 +229,13 @@
Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納
Distance_B=(Enc_Count_B*PULSE_TO_UM);
Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
- Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
+ Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
+
+ 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(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
Marker_Run_Distance+=(Distance_A+Distance_B)/2;
}
@@ -294,42 +303,75 @@
timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
lcd.cls();//表示クリア
lcd.locate(0,0);
- lcd.print("STOP");
+ lcd.print("STOP");
+ lcd.locate(0,1);
+ lcd.print("FIRST");
while(1){
Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得
+ if((!(Old_Machine_Status&=STOP))&&(Machine_Status&=STOP)){//走行終了時
+ lcd.locate(0,0);
+ lcd.print(" ");
+ lcd.locate(0,0);
+ 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;
+ }
+ }
+
+ }
+
if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
- wait(0.2);//チャタリング動作落ち着くまでの待ち時間
- Push_Cnt++;
- //各種フラグのクリア
- Corner_Flag=0;
- SG_Flag=0;
- Cross_Flag=0;
- Marker_Run_Distance=0;//マーカ通過距離情報リセット
- if(Push_Cnt>=3)Push_Cnt=3;//スイッチ押下回数の上限は３
- if(Push_Cnt==1){
- lcd.cls();//表示クリア
+ if(Machine_Status&STOP){//機体停止状態の時
+ lcd.locate(0,0);
+ lcd.print(" ");
lcd.locate(0,0);
- lcd.print("FIRST");
- wait(2);
- Machine_Status&=0x7F;//STOP状態の解除
- Default_Speed=FIRST_SPEED;
- }else if(Push_Cnt==2){
- lcd.cls();//表示クリア
+ lcd.print("GO!!");
+ wait(2);
+ Machine_Status&=0x7F;//ストップ状態解除
+ }else{//機体走行中であったとき
+ //各種フラグのクリア
+ Corner_Flag=0;
+ SG_Flag=0;
+ Cross_Flag=0;
+ Marker_Run_Distance=0;//マーカ通過距離情報リセット
+
+ Machine_Status |= STOP;//機体停止状態にする。
lcd.locate(0,0);
- lcd.print("SECOND");
- wait(2);
- Machine_Status&=0x7F;//STOP状態の解除
- Default_Speed=SECOND_SPEED;
- }else if(Push_Cnt==3){
- lcd.cls();//表示クリア
+ lcd.print(" ");
lcd.locate(0,0);
- lcd.print("THIRD");
- wait(2);
- Machine_Status&=0x7F;//STOP状態の解除
- Default_Speed=THIRD_SPEED;
- }
+ lcd.print("STOP");
+ }
}
}
}
\ No newline at end of file