Diff: main.cpp

Revision:

11:2cd6f8be124e

Parent:

10:e1eb10665472

Child:

12:dc4c569248d7

--- a/main.cpp	Wed Aug 28 09:44:41 2019 +0000
+++ b/main.cpp	Wed Aug 28 23:14:20 2019 +0000
@@ -1,4 +1,4 @@
-////ライントレースサンプルver7
+////ライントレースサンプルver7_1
 #include "mbed.h"
 #include "CRotaryEncoder.h"
 #include "TB6612.h"
@@ -6,9 +6,7 @@
 
 
 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define     FIRST_SPEED     500     //1走目の基本速度[mm/sec]
-#define     SECOND_SPEED    750     //2走目の基本速度[mm/sec]
-#define     THIRD_SPEED     1000    //3走目の基本速度[mm/sec]
+#define     DEFAULT_SPEED   800     //1走目の基本速度[mm/sec]
 #define     TURN_POWER      0.5     //コースアウト時の旋回力
 #define     PULSE_TO_UM     30      //エンコーダ1パルス当たりのタイヤ移動距離[um]
 #define     INTERRUPT_TIME  1000    //割りこみ周期[us]
@@ -45,9 +43,6 @@
 #define SECOND_RUN          0x02   //機体停止状態
 #define TUARD_RUN           0x01   //機体設定モード
 
-#define FIRST   1
-#define SECOND  2
-#define THIRD   3
 
 //デジタル入力オブジェクト定義
 DigitalIn   push_sw(D13);
@@ -72,7 +67,6 @@
 //使用変数の定義
 int    Sw_Ptn;
 int    Old_Sw_Ptn;
-int    Run_Mode=FIRST;
 double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
 double  All_Sensor_Data;      //ラインセンサ総データ量
 double Sensor_Diff[2]={0,0}; //ラインセンサ偏差
@@ -84,7 +78,8 @@
 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 Default_Speed=FIRST_SPEED;
+long int Machine_Speed=DEFAULT_SPEED;
+char     Speed_Str[5];                  //LCD速度表示用文字列
 long int Target_Speed_A=0,Target_Speed_B=0;  //目標速度
 long int Motor_A_Diff[2]={0,0};         //過去の速度偏差と現在の速度偏差を格納
 long int Motor_B_Diff[2]={0,0};         //
@@ -233,7 +228,7 @@
     
     if(Machine_Status&STOP){//機体停止状態の時
         Enc_B_Rotate+=Enc_Count_B;//モード設定用に右エンコーダ値の蓄積
-        if(Enc_B_Rotate<0)Enc_B_Rotate=0;
+        if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
         if(Enc_B_Rotate>6400)Enc_B_Rotate=6400;
     }
     if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
@@ -246,8 +241,8 @@
     encoder_b.Set(0);
     
     /////各モータの目標速度の設定
-    Target_Speed_A=Default_Speed;
-    Target_Speed_B=Default_Speed;
+    Target_Speed_A=Machine_Speed;
+    Target_Speed_B=Machine_Speed;
     
     /////モータの速度制御
     //過去の速度偏差を退避
@@ -305,7 +300,8 @@
     lcd.locate(0,0);
     lcd.print("STOP");
     lcd.locate(0,1);
-    lcd.print("FIRST");        
+    sprintf(Speed_Str,"%04d",Machine_Speed);
+    lcd.print(Speed_Str);        
     while(1){
         Old_Sw_Ptn=Sw_Ptn;  //過去のスイッチ入力情報を退避
         Sw_Ptn=push_sw;     //現在のスイッチ入力情報の取得
@@ -316,38 +312,12 @@
             lcd.print("STOP");            
         }
         if(Machine_Status&STOP){//機体停止状態の時
-            if((Enc_B_Rotate/1600)<1){//右エンコーダが1回転未満であったら
-                if(Run_Mode!=FIRST){
-                    lcd.cls();//表示クリア
-                    lcd.locate(0,0);
-                    lcd.print("STOP");
-                    lcd.locate(0,1);
-                    lcd.print("FIRST");
-                    Run_Mode=FIRST;
-                    Default_Speed=FIRST_SPEED;
-                }               
-            }else if(((Enc_B_Rotate/1600)>=1)&&((Enc_B_Rotate/1600)<2)){//右エンコーダが1回転以上、2回転未満であったら
-                if(Run_Mode!=SECOND){
-                    lcd.cls();//表示クリア
-                    lcd.locate(0,0);
-                    lcd.print("STOP");                    
-                    lcd.locate(0,1);
-                    lcd.print("SECOND");//2走目のモードであることを示す。
-                    Run_Mode=SECOND;
-                    Default_Speed=SECOND_SPEED; 
-                }           
-            }else if(((Enc_B_Rotate/1600)>=2)&&((Enc_B_Rotate/1600)<3)){//右エンコーダが2回転以上、3回転未満であったら
-                if(Run_Mode!=THIRD){
-                    lcd.cls();//表示クリア
-                    lcd.locate(0,0);
-                    lcd.print("STOP");                    
-                    lcd.locate(0,1);
-                    lcd.print("THIRD");//2走目のモードであることを示す。
-                    Run_Mode=THIRD;
-                    Default_Speed=THIRD_SPEED;
-                }             
-            }
-            
+            Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
+            sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換
+            lcd.locate(0,1);
+            lcd.print("      ");
+            lcd.locate(0,1);
+            lcd.print(Speed_Str);            
         }