Revision 8:15b4e1d7a2c5, committed 2019-08-28

Comitter:

yusaku0125

Date:

Wed Aug 28 04:32:56 2019 +0000

Parent:

7:cfbf8d4a4d36

Commit message:

test;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AQM0802.lib	Wed Aug 28 04:32:56 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/yasuyuki/code/AQM0802/#6fa303916aa8

--- a/main.cpp	Wed Aug 28 00:13:57 2019 +0000
+++ b/main.cpp	Wed Aug 28 04:32:56 2019 +0000
@@ -4,10 +4,13 @@
 #include "mbed.h"
 #include "CRotaryEncoder.h"
 #include "TB6612.h"
+#include "AQM0802.h"
 
 
 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define     DEFAULT_SPEED   900     //機体の直進速度1000[mm/s] 
+#define     FIRST_SPEED     500     //1走目の基本速度[mm/sec]
+#define     SECOND_SPEED    750     //2走目の基本速度[mm/sec]
+#define     THIRD_SPEED     1000    //3走目の基本速度[mm/sec]
 #define     TURN_POWER      0.5     //コースアウト時の旋回力
 #define     PULSE_TO_UM     30      //エンコーダ1パルス当たりのタイヤ移動距離[um]
 #define     INTERRUPT_TIME  1000    //割りこみ周期[us]
@@ -30,6 +33,11 @@
 #define     S_KD            0.3f    //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
 //////////☆★☆★☆★☆★☆★//////////////
 
+
+
+//スイッチ状態の定義
+#define PUSH    0       //スイッチ押したときの状態
+#define PULL    1       //スイッチ離したときの状態
 //機体状態の定義
 #define STOP                0x80   //機体停止状態
 #define RUN_START           0x40   //スタートマーカ通過
@@ -39,6 +47,8 @@
 #define SECOND_RUN          0x02   //機体停止状態
 #define TUARD_RUN           0x01   //機体設定モード
 
+//デジタル入力オブジェクト定義
+DigitalIn   push_sw(D13);
 /////アナログ入力オブジェクト定義//////////
 AnalogIn    s1(D3);
 AnalogIn    s2(A6);
@@ -55,29 +65,34 @@
 Ticker      timer;              //タイマ割込み用 
 TB6612      motor_a(D2,D7,D6);  //モータA制御用(pwma,ain1,ain2)
 TB6612      motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
-
+AQM0802     lcd(I2C_SDA,I2C_SCL);       //液晶制御用
 
 //使用変数の定義
-float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
-float All_Sensor_Data;      //ラインセンサ総データ量
-float Sensor_Diff[2]={0,0}; //ラインセンサ偏差
-float Sensor_P=0.0f;        //ラインセンサP(比例成分)制御量
-float Sensor_D=0.0f;        //ラインセンサD(微分成分)制御量
-float Sensor_PD=0.0f;       //ラインセンサP,D成分の合計
+int    Sw_Ptn;
+int    Old_Sw_Ptn;
+int    Push_Cnt;
+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}; //ラインセンサ偏差
+double Sensor_P =0.0f;        //ラインセンサP(比例成分)制御量
+double Sensor_D =0.0f;        //ラインセンサD(微分成分)制御量
+double Sensor_PD=0.0f;       //ラインセンサP,D成分の合計
 long int Enc_Count_A=0,Enc_Count_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 Default_Speed=0;
 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};   
-float Motor_A_P,Motor_B_P;              //モータP制御成分
-float Motor_A_D,Motor_B_D;              //モータD制御成分
+long int Motor_A_Diff[2]={0,0};         //過去の速度偏差と現在の速度偏差を格納
+long int Motor_B_Diff[2]={0,0};         //
+float Motor_A_P,Motor_B_P;              //モータ速度制御P成分
+float Motor_A_D,Motor_B_D;              //モータ速度制御D成分
+float Motor_A_PD,Motor_B_PD;            //モータ速度制御PD合成
 float Motor_A_Pwm,Motor_B_Pwm;          //モータへの出力
 unsigned char Sensor_Digital    =0x00;
 unsigned char Old_Sensor_Digital=0x00;
 int   Sensor_Cnt=0;
-unsigned char Machine_Status    =0x00;                 //機体状態
+unsigned char Machine_Status    =STOP;                 //機体状態
 unsigned char Old_Machine_Status=0x00;             //過去の機体状態
 int Marker_Pass_Flag = 0;
 int Old_Marker_Pass_Flag=0;
@@ -152,6 +167,8 @@
     }     
 }
 
+
+//タイマ割り込み1[ms]周期
 void timer_interrupt(){
     
     //ラインセンサ情報取得
@@ -213,15 +230,15 @@
     if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
         Marker_Run_Distance+=(Distance_A+Distance_B)/2;
     }
+    //エンコーダ関連情報のリセット
     Distance_A=0;
     Distance_B=0;
-    
     encoder_a.Set(0);
     encoder_b.Set(0);
     
     /////各モータの目標速度の設定
-    Target_Speed_A=DEFAULT_SPEED;
-    Target_Speed_B=DEFAULT_SPEED;
+    Target_Speed_A=Default_Speed;
+    Target_Speed_B=Default_Speed;
     
     /////モータの速度制御
     //過去の速度偏差を退避
@@ -236,22 +253,26 @@
     //D成分演算
     Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD;
     Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD;
-    
-    Motor_A_Pwm=Motor_A_P+Motor_A_D+Sensor_PD;
-    Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD;
+    //モータ速度制御のPD合成
+    Motor_A_PD=Motor_A_P+Motor_A_D;
+    Motor_B_PD=Motor_B_P+Motor_B_D;
+    //最終的なモータ制御量の合成    
+    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;
     else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f;
         
-    //モータへの出力
-    
+    //モータへの出力    
     if(!(Machine_Status&STOP)){//マシンが停止状態でなければ
-        if(Machine_Status&RUN_COURSE_LOUT){
-            motor_a=-(-TURN_POWER);
+        if(Machine_Status&RUN_COURSE_LOUT){         //左端センサ振り切れた時
+            motor_a=-(-TURN_POWER); //左旋回
             motor_b=-(TURN_POWER);    
-        }else if(Machine_Status&RUN_COURSE_ROUT){
-            motor_a=-(TURN_POWER);    
+        }else if(Machine_Status&RUN_COURSE_ROUT){   //右端センサ振り切れた時
+            motor_a=-(TURN_POWER);  //右旋回    
             motor_b=-(-TURN_POWER);
         }else if(Cross_Flag==1){//交差点通過中
             motor_a=-0.3;//交差点なので控えめの速度で直進
@@ -267,13 +288,47 @@
     }                       
 }
 
+
+
 int main() { 
-    timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート     
+    timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート 
+    lcd.cls();//表示クリア
+    lcd.locate(0,0);
+    lcd.print("STOP");    
     while(1){
-        wait(1);
+        Old_Sw_Ptn=Sw_Ptn;  //過去のスイッチ入力情報を退避
+        Sw_Ptn=push_sw;     //現在のスイッチ入力情報の取得
+                 
+        if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
+            wait(0.2);//チャタリング動作落ち着くまでの待ち時間
+            Push_Cnt++;
+            if(Push_Cnt>=3)Push_Cnt=3;//スイッチ押下回数の上限は３   
+            if(Push_Cnt==1){
+                lcd.cls();//表示クリア
+                lcd.locate(0,0);
+                lcd.print("FIRST");
+                wait(2);                
+                Machine_Status&=0x7F;//STOP状態の解除
+                Default_Speed=FIRST_SPEED;
+            }else if(Push_Cnt==2){
+                lcd.cls();//表示クリア
+                lcd.locate(0,0);
+                lcd.print("SECOND");
+                wait(2);                
+                Machine_Status&=0x7F;//STOP状態の解除            
+                Default_Speed=SECOND_SPEED;
+            }else if(Push_Cnt==3){
+                lcd.cls();//表示クリア
+                lcd.locate(0,0);
+                lcd.print("THIRD");
+                wait(2);                
+                Machine_Status&=0x7F;//STOP状態の解除            
+                Default_Speed=THIRD_SPEED;
+            }            
+        }
         //センサ検知情報、機体情報の表示
-        PC.printf("Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t",Sensor_Digital,Machine_Status);
+//        PC.printf("Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t",Sensor_Digital,Machine_Status);
         //スタートゴールマーカ検知回数の表示
-        PC.printf("SG_Cnt:%d\r\n",SG_Cnt);                
+//        PC.printf("SG_Cnt:%d\r\n",SG_Cnt);                
     }
 }
\ No newline at end of file