Diff: main.cpp

Revision:

15:cfe79ebfcb25

Parent:

14:7ed78f52f40e

Child:

16:017874772ea7

--- a/main.cpp	Mon Nov 11 06:25:06 2019 +0000
+++ b/main.cpp	Fri Nov 15 01:32:08 2019 +0000
@@ -82,12 +82,13 @@
 long int Enc_Count_A=0,Enc_Count_B=0;        //エンコーダパルス数を格納
 long int Enc_A_Rotate=0,Enc_B_Rotate=0;
 
-/*long int memory_A=0;
+long int memory_A=0;
 long int memory_B=0;
-char    memoryA_Str[5];
-char    memoryB_Str[5];*/
+char    MemoryA_Str[5];
+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 Distance_A=0,Distance_B=0;          //タイヤ移動距離を格納[mm]
 long int Marker_Run_Distance=0; 
 long int Speed_A=0,  Speed_B=0;              //現在速度
 long int Machine_Speed=DEFAULT_SPEED;
@@ -237,8 +238,12 @@
     Enc_Count_B=-encoder_b.Get();
     Distance_A=(Enc_Count_A*PULSE_TO_UM);  //移動距離をmm単位で格納
     Distance_B=(Enc_Count_B*PULSE_TO_UM);
-    Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
-    Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
+    
+    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);
     
     if(Machine_Status&STOP){//機体停止状態の時
         Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積
@@ -257,8 +262,11 @@
     encoder_a.Set(0);
     encoder_b.Set(0);
     
+    memory_A=Distance_memory_A;
+    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];
@@ -293,7 +301,7 @@
     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成分演算
@@ -306,7 +314,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;
@@ -331,27 +339,36 @@
     }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);
-    sprintf(Speed_Str,"%04d",Machine_Speed);
-    lcd.print(Speed_Str);        
+    lcd.print("     ");
+    
+    //sprintf(Speed_Str,"%04d",Machine_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);
             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++;
                         
         }
@@ -359,27 +376,26 @@
         if(Machine_Status&STOP){//機体停止状態の時
           
             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("      ");
-            lcd.locate(0,0);
-            lcd.print(Gray_Str);
-            lcd.print(" ");  
-            lcd.print(Coner_str);             
+            //sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換
+            //sprintf(Coner_str,"%d",Coner_c);
+            
+             
+            //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);//速度情報文字列変換
+
             }else {
                 Machine_Speed1=DEFAULT_SPEED1+(Enc_B_Rotate/16);
                 sprintf(Speed_Str,"%04d",Machine_Speed1);//速度情報文字列変換
             }
                             
-            lcd.locate(0,1);
+            /*lcd.locate(0,1);
             lcd.print("      ");
             lcd.locate(0,1);
-            lcd.print(Speed_Str);
-            
+            lcd.print(Speed_Str);*/            
                             
         }