Revision 5:27963aaa8f4a, committed 2019-08-26

Comitter:

yusaku0125

Date:

Mon Aug 26 04:25:40 2019 +0000

Parent:

4:c88cf4e101e2

Commit message:

timer_interrupt

Changed in this revision

diff -r c88cf4e101e2 -r 27963aaa8f4a main.cpp
--- a/main.cpp	Wed Aug 21 01:34:17 2019 +0000
+++ b/main.cpp	Mon Aug 26 04:25:40 2019 +0000
@@ -9,11 +9,9 @@
 #include "TB6612.h"
 #include "AQM0802.h"
 #include "mpu6050.h"
-AQM0802 lcd(D4,D5);
-MPU6050 mpu(D4, D5);
 TB6612      motor_a(D2,D7,D6);  //モータA制御用(pwma,ain1,ain2)
 TB6612      motor_b(D10,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
-
+Ticker      timer;     //タイマ割込み用
 Serial      pc(USBTX,USBRX);    //USBシリアル通信用
 
 AnalogIn s1(D3);
@@ -40,37 +38,17 @@
 //ラインセンサ比例制御成分
 #define S_KP    0.3f    //ラインセンサによるモータ制御量
                         //大きいほど曲がりやすい
-
-//ジャイロセンサのゲイン
-//#define G_KP    0.75f
-#define G_KP    0.0f    //ジャイロ未使用の場合は0.0(制御量無し)にしておく
-
 ////////////////////////////////////////////////////////////////
 
 
-
-
-
-
 //使用変数の定義
 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
 float All_Sensor_Data;
 float Sensor_P;
 float Motor_A_Pwm,Motor_B_Pwm;
-char  Str_S[4],Str_G[4];
-double Gx,Gy,Gz;    //Z角速度情報を格納
-float Gz_F;         //Ｚ軸データint型変換格納
-float Gyro_P;       //ジャイロセンサP成分
 
-int main() {
-    lcd.init();         //LCD初期化
-    lcd.locate(0,0);    //桁、行    
-    mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。
-    
-         
-    while(1) {
-       wait_ms(10);
-       //各種センサ情報取得
+void timer_interrupt(){
+        //各種センサ情報取得
        S1_Data=s1.read();
        S2_Data=s2.read();
        S3_Data=s3.read();
@@ -79,35 +57,16 @@
        S6_Data=s6.read();
        S7_Data=s7.read();
        S8_Data=s8.read();
-
-        //★☆角速度情報の取得☆★
-        mpu.readGyroscope(Gx, Gy, Gz);//関数仕様上３軸すべて角速度取得する。
-        Gz_F=(float)Gz/1000;   //doubleは大きすぎるのでfloat型へ変換  
-        Gyro_P=Gz_F*G_KP;                  
+              
         
        //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
        All_Sensor_Data=-(S2_Data*S_K3+S3_Data*S_K2+S4_Data*S_K1)+(S5_Data*S_K1+S6_Data*S_K2+S7_Data*S_K3);
        Sensor_P=All_Sensor_Data*S_KP;//センサ比例成分の演算
 
-        //LCD1行目にセンサ合計値の表示
-       lcd.locate(0,0);//桁、行
-       lcd.print("        ");//表示クリア
-       lcd.locate(0,0);
-       lcd.print("S:");
-       sprintf(Str_S,"%+1.2f",Sensor_P);//センサ比例成分を文字列変換       
-       lcd.print(Str_S);
- 
-        //★☆LCDへの角速度表示☆★        
-        lcd.locate(0,1);//桁、行
-        lcd.print("        ");//表示クリア
-        lcd.locate(0,1);
-        lcd.print("G:");
-        sprintf(Str_G,"%+1.2f",Gz_F);
-        lcd.print(Str_G);  
        
        //モータ制御量の演算
-       Motor_A_Pwm=DEFAULT_SPEED + Sensor_P - Gyro_P;
-       Motor_B_Pwm=DEFAULT_SPEED - Sensor_P + Gyro_P;       
+       Motor_A_Pwm=DEFAULT_SPEED + Sensor_P;
+       Motor_B_Pwm=DEFAULT_SPEED - Sensor_P;       
        //モータ出力は±1.0が上下限度なので限界値を設定する。
        if(Motor_A_Pwm> 1.0f)Motor_A_Pwm=1.0f;
        else if(Motor_A_Pwm< -1.0f)Motor_A_Pwm=-1.0f;
@@ -116,7 +75,11 @@
        
        //最終的には符号を逆転して出力
        motor_a=-Motor_A_Pwm;
-       motor_b=-Motor_B_Pwm;
+       motor_b=-Motor_B_Pwm;  
+}
 
+int main() { 
+    timer.attach_us(&timer_interrupt,1000);//1ms単位     
+    while(1) {
     }
 }