島根短大 ライントレーサ制作チーム
配列を使用し記憶走行を行う
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 8:15b4e1d7a2c5
- Parent:
- 7:cfbf8d4a4d36
- Child:
- 9:1c28fcc1e9b8
--- a/main.cpp Wed Aug 28 00:13:57 2019 +0000
+++ b/main.cpp Wed Aug 28 04:32:56 2019 +0000
@@ -4,10 +4,13 @@
#include "mbed.h"
#include "CRotaryEncoder.h"
#include "TB6612.h"
+#include "AQM0802.h"
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 900 //機体の直進速度1000[mm/s]
+#define FIRST_SPEED 500 //1走目の基本速度[mm/sec]
+#define SECOND_SPEED 750 //2走目の基本速度[mm/sec]
+#define THIRD_SPEED 1000 //3走目の基本速度[mm/sec]
#define TURN_POWER 0.5 //コースアウト時の旋回力
#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
#define INTERRUPT_TIME 1000 //割りこみ周期[us]
@@ -30,6 +33,11 @@
#define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
//////////☆★☆★☆★☆★☆★//////////////
+
+
+//スイッチ状態の定義
+#define PUSH 0 //スイッチ押したときの状態
+#define PULL 1 //スイッチ離したときの状態
//機体状態の定義
#define STOP 0x80 //機体停止状態
#define RUN_START 0x40 //スタートマーカ通過
@@ -39,6 +47,8 @@
#define SECOND_RUN 0x02 //機体停止状態
#define TUARD_RUN 0x01 //機体設定モード
+//デジタル入力オブジェクト定義
+DigitalIn push_sw(D13);
/////アナログ入力オブジェクト定義//////////
AnalogIn s1(D3);
AnalogIn s2(A6);
@@ -55,29 +65,34 @@
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); //液晶制御用
//使用変数の定義
-float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
-float All_Sensor_Data; //ラインセンサ総データ量
-float Sensor_Diff[2]={0,0}; //ラインセンサ偏差
-float Sensor_P=0.0f; //ラインセンサP(比例成分)制御量
-float Sensor_D=0.0f; //ラインセンサD(微分成分)制御量
-float Sensor_PD=0.0f; //ラインセンサP,D成分の合計
+int Sw_Ptn;
+int Old_Sw_Ptn;
+int Push_Cnt;
+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成分の合計
long int Enc_Count_A=0,Enc_Count_B=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 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制御成分
+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 =0x00; //機体状態
+unsigned char Machine_Status =STOP; //機体状態
unsigned char Old_Machine_Status=0x00; //過去の機体状態
int Marker_Pass_Flag = 0;
int Old_Marker_Pass_Flag=0;
@@ -152,6 +167,8 @@
}
}
+
+//タイマ割り込み1[ms]周期
void timer_interrupt(){
//ラインセンサ情報取得
@@ -213,15 +230,15 @@
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);
/////各モータの目標速度の設定
- Target_Speed_A=DEFAULT_SPEED;
- Target_Speed_B=DEFAULT_SPEED;
+ Target_Speed_A=Default_Speed;
+ Target_Speed_B=Default_Speed;
/////モータの速度制御
//過去の速度偏差を退避
@@ -236,22 +253,26 @@
//D成分演算
Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD;
Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD;
-
- Motor_A_Pwm=Motor_A_P+Motor_A_D+Sensor_PD;
- Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD;
+ //モータ速度制御のPD合成
+ Motor_A_PD=Motor_A_P+Motor_A_D;
+ Motor_B_PD=Motor_B_P+Motor_B_D;
+ //最終的なモータ制御量の合成
+ Motor_A_Pwm=Motor_A_PD+Sensor_PD;
+ Motor_B_Pwm=Motor_B_PD-Sensor_PD;
+
+ //モータ制御量の上限下限設定
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);
+ 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);
+ }else if(Machine_Status&RUN_COURSE_ROUT){ //右端センサ振り切れた時
+ motor_a=-(TURN_POWER); //右旋回
motor_b=-(-TURN_POWER);
}else if(Cross_Flag==1){//交差点通過中
motor_a=-0.3;//交差点なので控えめの速度で直進
@@ -267,13 +288,47 @@
}
}
+
+
int main() {
- timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
+ timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
+ lcd.cls();//表示クリア
+ lcd.locate(0,0);
+ lcd.print("STOP");
while(1){
- wait(1);
+ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
+ Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得
+
+ if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
+ wait(0.2);//チャタリング動作落ち着くまでの待ち時間
+ Push_Cnt++;
+ if(Push_Cnt>=3)Push_Cnt=3;//スイッチ押下回数の上限は３
+ if(Push_Cnt==1){
+ lcd.cls();//表示クリア
+ 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.locate(0,0);
+ lcd.print("SECOND");
+ wait(2);
+ Machine_Status&=0x7F;//STOP状態の解除
+ Default_Speed=SECOND_SPEED;
+ }else if(Push_Cnt==3){
+ lcd.cls();//表示クリア
+ lcd.locate(0,0);
+ lcd.print("THIRD");
+ wait(2);
+ Machine_Status&=0x7F;//STOP状態の解除
+ Default_Speed=THIRD_SPEED;
+ }
+ }
//センサ検知情報、機体情報の表示
- PC.printf("Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t",Sensor_Digital,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);
+// PC.printf("SG_Cnt:%d\r\n",SG_Cnt);
}
}
\ No newline at end of file