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

Dependencies:   mbed AQM0802 CRotaryEncoder TB6612FNG

Revision:
20:3b35311f4576
Parent:
19:edf765724d2d
Child:
21:97cc65e61580
--- a/main.cpp	Mon Nov 18 11:51:49 2019 +0000
+++ b/main.cpp	Tue Nov 19 11:12:26 2019 +0000
@@ -50,6 +50,7 @@
 //デジタル入力オブジェクト定義
 DigitalIn   push_sw(D13);
 /////アナログ入力オブジェクト定義//////////
+/*  
 AnalogIn    s1(D3);
 AnalogIn    s2(A6);
 AnalogIn    s3(A5);
@@ -58,8 +59,8 @@
 AnalogIn    s6(A2);
 AnalogIn    s7(A1);
 AnalogIn    s8(A0);
+*/
 
-/*
 AnalogIn    s1(A1);
 AnalogIn    s2(D3);
 AnalogIn    s3(A6);
@@ -68,7 +69,8 @@
 AnalogIn    s6(A3);
 AnalogIn    s7(A2);
 AnalogIn    s8(A0);
-*/
+
+
 ///////////////////////////////////////  
 Serial      PC(USBTX,USBRX);
 CRotaryEncoder encoder_a(D1,D0);    //モータAのエンコーダ
@@ -336,12 +338,11 @@
     
     /////各モータの目標速度の設定
     if(Sw==0){
-        
         Motor_A_Diff[1]=Motor_A_Diff[0];
         Motor_B_Diff[1]=Motor_B_Diff[0];
         
-        Target_Speed_A=Medium_Speed;
-        Target_Speed_B=Medium_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);
@@ -351,8 +352,8 @@
         Motor_A_Diff[1]=Motor_A_Diff[0];
         Motor_B_Diff[1]=Motor_B_Diff[0];   
         
-        Target_Speed_A1=High_Speed;
-        Target_Speed_B1=High_Speed;
+//        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);
@@ -369,7 +370,7 @@
     
     Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
     Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
-    */
+*/
     
 
     //P成分演算
@@ -390,7 +391,7 @@
     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){         //左端センサ振り切れた時
@@ -410,20 +411,15 @@
     }else{//停止状態の時はモータへの出力は無効
         motor_a=0;
         motor_b=0;        
-   }*/                    
+   }                   
 }
 
-
-
-
 int main() { 
     timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート 
     lcd.cls();//表示クリア
     lcd.locate(0,0);
     lcd.print("STOP");
     lcd.locate(0,1);
-    //lcd.print("     ");
-    
     sprintf(Speed_Str,"%04d",Medium_Speed);
     lcd.print(Speed_Str);        
     
@@ -454,23 +450,23 @@
             Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整
             sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換
             sprintf(Coner_str,"%d",Coner_c);            
-             
+            
+            lcd.locate(0,0); 
             lcd.print(Gray_Str);
             lcd.print(" ");  
-            lcd.print(Coner_str);             
+            lcd.print(Coner_str);
+            
+            lcd.locate(0,1);
+            lcd.print("      ");
+            lcd.locate(0,1);
+            lcd.print(Speed_Str);              
             if(Sw==0){
                 Medium_Speed=DEFAULT_SPEED1+(Enc_B_Rotate/16);//標準速度調整
                 sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換
             }else {
                 High_Speed=DEFAULT_SPEED2+(Enc_B_Rotate/16);
                 sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換
-            }
-                            
-            lcd.locate(0,1);
-            lcd.print("      ");
-            lcd.locate(0,1);
-            lcd.print(Speed_Str);     
-                            
+            }                                                          
         }
         
                  
@@ -483,7 +479,11 @@
                 lcd.print("GO!!");
                 wait(2);
                 Stop_Distance=0;
-                Machine_Status&=0x7F;//ストップ状態解除   
+                Machine_Status&=0x7F;//ストップ状態解除 
+                Target_Speed_A=Medium_Speed;
+                Target_Speed_B=Medium_Speed;
+                
+                  
             }else{//機体走行中であったとき
                 //各種フラグのクリア
                 Corner_Flag=0;