Diff: main.cpp

Revision:

3:623a84032d43

Parent:

2:7ca124b4dbcb

Child:

4:c88cf4e101e2

diff -r 7ca124b4dbcb -r 623a84032d43 main.cpp
--- a/main.cpp	Mon Jul 22 10:28:52 2019 +0000
+++ b/main.cpp	Mon Aug 05 10:01:04 2019 +0000
@@ -3,32 +3,101 @@
 //生成時のピン割当はマイコンピン割当通りに配置すること。
 #include "mbed.h"
 #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)
+
 Serial      pc(USBTX,USBRX);    //USBシリアル通信用
 
+AnalogIn s1(D3);
+AnalogIn s2(A6);
+AnalogIn s3(A5);
+AnalogIn s4(A4);
+AnalogIn s5(A3);
+AnalogIn s6(A2);
+AnalogIn s7(A1);
+AnalogIn s8(A0);
+
+
+//フォトリフレクタのゲイン
+#define S_K1    1.0f    //float演算させる値には必ずfを付ける
+#define S_K2    2.0f
+#define S_K3    4.0f
+#define S_KP    0.3f
+
+//ジャイロセンサのゲイン
+#define G_KP    0.75f
+
+#define DEFAULT_SPEED 0.3f
+
+//使用変数の定義
+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() {
-    float   motor_speed;        //モータスピード情報格納用
-    char    input_data;         //キーボード入力情報格納用
+    lcd.init();         //LCD初期化
+    lcd.locate(0,0);    //桁、行    
+    mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。
+    
+         
     while(1) {
-        input_data=pc.getc();   //キーボード入力情報取得
-        motor_speed=0.5; //モータスピード(低速運転させるため２分の１の値とする。)
-        switch(input_data){
-            case    '1':    motor_a=motor_speed;    //モータA正転
-                            break;    
-            case    '2':    motor_a=0;              //モータAブレーキ
-                            break;
-            case    '3':    motor_a=-motor_speed;   //モータA逆転
-                            break;                                        
-            case    '7':    motor_b=motor_speed;    //モータB正転
-                            break;    
-            case    '8':    motor_b=0;              //モータBブレーキ
-                            break;
-            case    '9':    motor_b=-motor_speed;   //モータB正転
-                            break;              
-            default    :    motor_a=0;
-                            motor_b=0;              //両方モータブレーキ
-                            break;        
-        }
+       wait_ms(10);
+       //各種センサ情報取得
+       S1_Data=s1.read();
+       S2_Data=s2.read();
+       S3_Data=s3.read();
+       S4_Data=s4.read();
+       S5_Data=s5.read();
+       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;       
+       //モータ出力は±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;
+       if(Motor_B_Pwm> 1.0f)Motor_B_Pwm=1.0f;
+       else if(Motor_B_Pwm< -1.0f)Motor_B_Pwm=-1.0f;       
+       
+       //最終的には符号を逆転して出力
+       motor_a=-Motor_A_Pwm;
+       motor_b=-Motor_B_Pwm;
+
     }
 }