LCD表示系の整理。現状の問題としては、配列への左右移動距離の記憶ができていない様子。2走目で常にHIGH_SPEEDとなってしまうので、エンコーダパルス関係の蓄積がうまくできているか?左右同じ情報が演算されていないか?といった部分を疑ってデバッグする必要がある。

Dependencies:   mbed AQM0802 CRotaryEncoder TB6612FNG

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);//速度情報文字列変換               
         }