島根短大 ライントレーサ制作チーム
最終調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 22:b40f7d0c0f12
- Parent:
- 21:97cc65e61580
- Child:
- 23:def04f2e894f
--- a/main.cpp Wed Nov 20 04:41:18 2019 +0000
+++ b/main.cpp Fri Nov 22 04:20:20 2019 +0000
@@ -6,9 +6,10 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 300 //1走目の基本速度[mm/sec]
-#define DEFAULT_SPEED1 800
-#define DEFAULT_SPEED2 900
+#define DEFAULT_SPEED 450 //1走目の基本速度[mm/sec]
+#define DEFAULT_SPEED1 700
+#define DEFAULT_SPEED2 950
+#define DEFAULT_SPEED3 800
#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm]
#define TURN_POWER 0.6 //コースアウト時の旋回力
#define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um]
@@ -50,7 +51,7 @@
//デジタル入力オブジェクト定義
DigitalIn push_sw(D13);
/////アナログ入力オブジェクト定義//////////
-/*
+
AnalogIn s1(D3);
AnalogIn s2(A6);
AnalogIn s3(A5);
@@ -59,8 +60,8 @@
AnalogIn s6(A2);
AnalogIn s7(A1);
AnalogIn s8(A0);
-*/
+/*
AnalogIn s1(A1);
AnalogIn s2(D3);
AnalogIn s3(A6);
@@ -69,7 +70,7 @@
AnalogIn s6(A3);
AnalogIn s7(A2);
AnalogIn s8(A0);
-
+*/
///////////////////////////////////////
Serial PC(USBTX,USBRX);
@@ -111,9 +112,9 @@
long int Low_Speed=DEFAULT_SPEED;
long int Medium_Speed=DEFAULT_SPEED1;
long int High_Speed=DEFAULT_SPEED2;
+long int High1_Speed=DEFAULT_SPEED3;
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; //目標速度
long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納
long int Motor_B_Diff[2]={0,0}; //
float Motor_A_P,Motor_B_P; //モータ速度制御P成分
@@ -205,7 +206,43 @@
}
}
void coner_curvature(){
- if((motor_a>(motor_b*1.8)) || (motor_a<(motor_b*0.2))){//左が右より80%以上早いか20%以上遅いとき
+ if(Sw==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]=(float)Distance_memory_A;
+ course_data[Row][1]=(float)Distance_memory_B;
+ //キョクリツ演算
+ if((Distance_memory_A>(Distance_memory_B*1.8)) || (Distance_memory_A<(Distance_memory_B*0.2))){//左が右より80%以上早いか20%以上遅いとき
+ Imaginary_Speed=Low_Speed;
+ }else if((Distance_memory_A>(Distance_memory_B*1.5)) || (Distance_memory_A<(Distance_memory_B*0.5))){//左が右より50%以上早いか50%以上遅いとき
+ Imaginary_Speed=Medium_Speed;
+ }else if((Distance_memory_A>(Distance_memory_B*1.2)) || (Distance_memory_A<(Distance_memory_A*0.8))){//左が右より20%以上早いか20%以上遅いとき⇒直線
+ Imaginary_Speed=High_Speed;
+ }
+ else{
+ Imaginary_Speed=High_Speed;
+ }
+ course_data[Row][2]=Imaginary_Speed; //仮想の演算速度ヲ格納
+ PC.printf("left:%.2f\t",course_data[Row][0]);
+ PC.printf("right:%.2f\t",course_data[Row][1]);
+ PC.printf("speed:%.2f\n\r",course_data[Row][2]);
+ }
+ else if(Sw==1){
+ Target_Speed_A=course_data[Row][2];//記憶走行で演算した仮想速度を使う
+ Target_Speed_B=course_data[Row][2];
+ PC.printf("left:%.2f\t",course_data[Row][0]);
+ PC.printf("right:%.2f\t",course_data[Row][1]);
+ PC.printf("speed:%.2f\n\r",course_data[Row][2]);
+ }
+ else{
+ Distance_memory_A=(Enc_Count_A*PULSE_TO_UM);
+ Distance_memory_B=(Enc_Count_B*PULSE_TO_UM);
+ }
+
+ /*
+ if((motor_a>(motor_b*1.8)) || (motor_a<(motor_b*0.2))){//左が右より80%以上早いか20%以上遅いとき
Imaginary_Speed=Low_Speed;
Target_Speed_A=Imaginary_Speed;
Target_Speed_B=Imaginary_Speed;
@@ -221,15 +258,16 @@
}
else{
Imaginary_Speed=High_Speed;
- Target_Speed_A=Imaginary_Speed;
+ Target_Speed_A=Imaginary_Speed;
Target_Speed_B=Imaginary_Speed;
}
- course_data[Row][0]=(float)Distance_memory_A;
+*/
+ /*course_data[Row][0]=(float)Distance_memory_A;
course_data[Row][1]=(float)Distance_memory_B;
course_data[Row][2]=Imaginary_Speed;
PC.printf("left:%.2f\t",course_data[Row][0]);
PC.printf("right:%.2f\t",course_data[Row][1]);
- PC.printf("speed:%.2f\n\r",course_data[Row][2]);
+ PC.printf("speed:%.2f\n\r",course_data[Row][2]);*/
}
@@ -262,27 +300,15 @@
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;
+ Machine_Status|=STOP; //機体停止状態へ
+ Row=0;
+ SG_Cnt=0;
}else if(Corner_Flag==1){//コーナマーカの時
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]=Imaginary_Speed;
-
- PC.printf("左:%.2f",course_data[Row][0]);
- PC.printf("右:%.2f",course_data[Row][1]);
- PC.printf("速度:%.2f",course_data[Row][2]);
- */
+ Coner_c++;
coner_curvature();
- Row++;
-
+ Row++;
}
}else{//マーカではなく、誤検知だった場合。
//何もしない
@@ -347,16 +373,17 @@
Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
}
- else {
- Target_Speed_A1=High_Speed;
- Target_Speed_B1=High_Speed;
+ else if(Sw==1) {
+ Target_Speed_A=High_Speed;
+ Target_Speed_B=High_Speed;
Motor_A_Diff[1]=Motor_A_Diff[0];
Motor_B_Diff[1]=Motor_B_Diff[0];
- Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
- Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
+ Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
+ Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
}
+
/*
/////モータの速度制御
@@ -439,9 +466,9 @@
//lcd.print(MemoryB_Str);
wait(5);
Gray=DEFAULT_GRAY;
- Medium_Speed=DEFAULT_SPEED1;
-
+ Medium_Speed=DEFAULT_SPEED1;
Sw++;
+ Coner_c=0;
}
if(Machine_Status&STOP){//機体停止状態の時
@@ -462,10 +489,14 @@
if(Sw==0){
Medium_Speed=DEFAULT_SPEED1+(Enc_B_Rotate/16);//標準速度調整
sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換
- }else {
+ }
+ else if(Sw==1) {
High_Speed=DEFAULT_SPEED2+(Enc_B_Rotate/16);
sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換
- }
+ }
+ else if(Sw==2)
+ High1_Speed=DEFAULT_SPEED3+(Enc_B_Rotate/16);
+ sprintf(Speed_Str,"%04d",High1_Speed);//速度情報文字列変換
}