島根短大 ライントレーサ制作チーム
最終調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 16:017874772ea7
- Parent:
- 15:cfe79ebfcb25
- Child:
- 17:b29e2c88b3c5
diff -r cfe79ebfcb25 -r 017874772ea7 main.cpp
--- a/main.cpp Fri Nov 15 01:32:08 2019 +0000
+++ b/main.cpp Mon Nov 18 04:31:02 2019 +0000
@@ -1,4 +1,4 @@
-////ライントレースサンプルver7_1
+////ライントレースサンプル
#include "mbed.h"
#include "CRotaryEncoder.h"
#include "TB6612.h"
@@ -6,11 +6,13 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec]
-#define DEFAULT_SPEED1 900
+#define DEFAULT_SPEED 300 //1走目の基本速度[mm/sec]
+#define DEFAULT_SPEED1 600
+#define DEFAULT_SPEED2 900
+#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm]
#define TURN_POWER 0.6 //コースアウト時の旋回力
#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um]
-#define INTERRUPT_TIME 1000 //割りこみ周期[us]
+#define INTERRUPT_TIME 3000 //割りこみ周期[us]
#define DEFAULT_GRAY 0.2f //フォトリフレクタデジタル入力の閾値
//シリアル通信でSensor_Digital値を確認し調整する。
#define MARKER_WIDTH 10000 //マーカ幅[um]（ビニルテープ幅19000[um]以内）
@@ -56,6 +58,17 @@
AnalogIn s6(A2);
AnalogIn s7(A1);
AnalogIn s8(A0);
+
+/*
+AnalogIn s1(A1);
+AnalogIn s2(D3);
+AnalogIn s3(A6);
+AnalogIn s4(A5);
+AnalogIn s5(A4);
+AnalogIn s6(A3);
+AnalogIn s7(A2);
+AnalogIn s8(A0);
+*/
///////////////////////////////////////
Serial PC(USBTX,USBRX);
CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
@@ -69,7 +82,7 @@
int Sw_Ptn;
int Old_Sw_Ptn;
int Sw=0;
-int Coner_c=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; //ラインセンサ総データ量
@@ -81,18 +94,21 @@
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_A=0;
-long int memory_B=0;
-char MemoryA_Str[5];
+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 Marker_Run_Distance=0;
long int Speed_A=0, Speed_B=0; //現在速度
-long int Machine_Speed=DEFAULT_SPEED;
-long int Machine_Speed1=DEFAULT_SPEED1;
+
+long int Low_Speed=DEFAULT_SPEED;
+long int Medium_Speed=DEFAULT_SPEED1;
+long int High_Speed=DEFAULT_SPEED2;
char Speed_Str[5]; //LCD速度表示用文字列
long int Target_Speed_A=0,Target_Speed_B=0; //目標速度
long int Target_Speed_A1=0,Target_Speed_B1=0; //目標速度
@@ -113,6 +129,10 @@
int SG_Flag=0;
int SG_Cnt=0;
int Cross_Flag=0;
+
+int Row=0; //行変数
+float course_data[100][3]; //記憶走行用配列
+
void sensor_analog_read(){
S1_Data=s1.read();
S2_Data=s2.read();
@@ -213,7 +233,25 @@
Machine_Status|=STOP; //機体停止状態へ
SG_Cnt=0;
}else if(Corner_Flag==1){//コーナマーカの時
- Coner_c++;
+
+ Distance_memory_A=0;
+ Distance_memory_B=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]=Distance_memory_A;
+ course_data[Row][1]=Distance_memory_B;
+ course_data[Row][2]=Medium_Speed;
+
+ /*PC.printf("左:%.2f",course_data[Row][0]);
+ PC.printf("右:%.2f",course_data[Row][1]);
+ PC.printf("速度:%.2f",course_data[Row][2]);
+ */
+
+ Row++;
+
}
}else{//マーカではなく、誤検知だった場合。
//何もしない
@@ -236,14 +274,14 @@
////モータ現在速度の取得
Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得
Enc_Count_B=-encoder_b.Get();
- Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納
- Distance_B=(Enc_Count_B*PULSE_TO_UM);
+ //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);//走行速度演算[mm/s]
- Speed_B=(Distance_B*1000);
+ 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;//閾値用に左エンコーダ値の蓄積
@@ -252,13 +290,16 @@
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;
+ //Distance_A=0;
+ //Distance_B=0;
+
encoder_a.Set(0);
encoder_b.Set(0);
@@ -266,13 +307,13 @@
memory_B=Distance_memory_B;
/////各モータの目標速度の設定
- /*if(Sw==0){
+ if(Sw==0){
Motor_A_Diff[1]=Motor_A_Diff[0];
Motor_B_Diff[1]=Motor_B_Diff[0];
- Target_Speed_A=Machine_Speed;
- Target_Speed_B=Machine_Speed;
+ Target_Speed_A=Medium_Speed;
+ Target_Speed_B=Medium_Speed;
Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
@@ -282,8 +323,8 @@
Motor_A_Diff[1]=Motor_A_Diff[0];
Motor_B_Diff[1]=Motor_B_Diff[0];
- Target_Speed_A1=Machine_Speed1;
- Target_Speed_B1=Machine_Speed1;
+ Target_Speed_A1=High_Speed;
+ Target_Speed_B1=High_Speed;
Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
@@ -291,7 +332,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);
@@ -299,9 +340,9 @@
Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
- */
+
- /*//P成分演算
+ //P成分演算
Motor_A_P=Motor_A_Diff[0]*M_KP;
Motor_B_P=Motor_B_Diff[0]*M_KP;
//D成分演算
@@ -314,7 +355,7 @@
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;
@@ -339,7 +380,7 @@
}else{//停止状態の時はモータへの出力は無効
motor_a=0;
motor_b=0;
- }*/
+ }
}
int main() {
@@ -348,54 +389,54 @@
lcd.locate(0,0);
lcd.print("STOP");
lcd.locate(0,1);
- lcd.print(" ");
+ //lcd.print(" ");
- //sprintf(Speed_Str,"%04d",Machine_Speed);
- //lcd.print(Speed_Str);
+ sprintf(Speed_Str,"%04d",Medium_Speed);
+ lcd.print(Speed_Str);
while(1){
Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得
if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時
- sprintf(MemoryA_Str,"%d",memory_A);
- sprintf(MemoryB_Str,"%d",memory_B);
+ //sprintf(MemoryA_Str,"%d",memory_A);
+ //sprintf(MemoryB_Str,"%d",memory_B);
lcd.locate(0,0);
lcd.print(" ");
lcd.locate(0,0);
- //lcd.print("STOP");
+ lcd.print("STOP");
- lcd.locate(0,0);
- lcd.print(MemoryA_Str);
- lcd.locate(0,1);
- lcd.print(MemoryB_Str);
-
- Sw++;
-
+ //lcd.locate(0,0);
+ //lcd.print(MemoryA_Str);
+ //lcd.locate(0,1);
+ //lcd.print(MemoryB_Str);
+ wait(5);
+ Gray=DEFAULT_GRAY;
+ Medium_Speed=DEFAULT_SPEED1;
+
+ 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(Gray_Str,"%3.2f",Gray);//速度情報文字列変換
+ sprintf(Coner_str,"%d",Coner_c);
- //lcd.print(Gray_Str);
- //lcd.print(" ");
- //lcd.print(Coner_str);
+ lcd.print(Gray_Str);
+ lcd.print(" ");
+ lcd.print(Coner_str);
if(Sw==0){
- Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
- sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換
-
+ Medium_Speed=DEFAULT_SPEED1+(Enc_B_Rotate/16);//標準速度調整
+ sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換
}else {
- Machine_Speed1=DEFAULT_SPEED1+(Enc_B_Rotate/16);
- sprintf(Speed_Str,"%04d",Machine_Speed1);//速度情報文字列変換
+ High_Speed=DEFAULT_SPEED2+(Enc_B_Rotate/16);
+ sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換
}
- /*lcd.locate(0,1);
+ lcd.locate(0,1);
lcd.print(" ");
lcd.locate(0,1);
- lcd.print(Speed_Str);*/
+ lcd.print(Speed_Str);
}
@@ -408,6 +449,7 @@
lcd.locate(0,0);
lcd.print("GO!!");
wait(2);
+ Stop_Distance=0;
Machine_Status&=0x7F;//ストップ状態解除
}else{//機体走行中であったとき
//各種フラグのクリア