Diff: main.cpp

Revision:

0:02e4c332f5b4

Child:

1:d0d24c45ce32

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 22 11:45:34 2022 +0000
@@ -0,0 +1,111 @@
+#include "mbed.h"
+#include "QEI.h"
+
+Timer t;
+QEI enc_X(PA_14,PA_13,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
+QEI enc_Y(PC_3,PC_2,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
+RawSerial sabertooth1(PC_10,NC,115200);//モータードライバー出力PINの設定
+RawSerial sabertooth2(PC_12,NC,115200);//モータードライバー出力PINの設定
+Serial pc(USBTX,USBRX,9600);//tera term の準備
+Ticker flipper;//タイム関数用
+DigitalIn swich(PA_0);//スタートスイッチの入力PINの設定
+AnalogIn laser(PA_5);//ボール感知センサーの入力PINの設定
+int i = true;
+int S;
+double kyori_X;
+double kyori_Y;
+double cnt_X;
+double cnt_Y;
+double hennsa_X = 0;//偏差値
+double hennsa_Y = 0;//偏差値
+double I_X = 0;//偏差の積分値
+double I_Y = 0;//偏差の積分値
+double D_X = 0;//偏差の微分値
+double D_Y = 0;//偏差の微分値
+double pre_hennsa_X;//Xのひとつ前の偏差
+double pre_hennsa_Y;//Yのひとつ前の偏差
+double Kp = 250;//PID制御のPの係数
+double Ki = 10;//PID制御のIの係数
+double Kd = 10;//PID制御のDの係数
+double P_X = 0;//PID制御の計算値
+double P_Y = 0;//PID制御の計算値
+double PW1 = 0;//モーターの力
+double PW2 = 0;//モーターの力
+double PW3 = 0;//モーターの力
+double PW4 = 0;//モーターの力
+double dst_Y = 1154;//目的地の座標
+double dst_X = 643.3;//目的地の座標
+double taiyanoennshuu = 150.8;
+int bunnkainou = 200;
+double ball;//ボールセンサーの値
+int exist;//while文の条件のballの値
+#define HAZIMARI 1
+#define HENNKOU 2
+#define TENNKAI 3
+bool flag = false;
+int mokuteki = HENNKOU;
+
+void flip()//タイマー割込み
+{
+    kyori_X = taiyanoennshuu * (cnt_X / (bunnkainou * 4));
+    kyori_Y = taiyanoennshuu * (cnt_Y / (bunnkainou * 4));
+    cnt_X = -1.0 * enc_X.getPulses();//エンコーダーの値の代入
+    cnt_Y = -1.0 * enc_Y.getPulses();//エンコーダーの値の代入
+    hennsa_X = dst_X - taiyanoennshuu * (cnt_X / (bunnkainou * 4));//距離の偏差
+    hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差
+    I_X += (pre_hennsa_X + hennsa_X) * 0.01  / 2;//積分計算
+    I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01  / 2;//積分計算
+    D_X =  (pre_hennsa_X - hennsa_X) * 0.01  / 2;//微分計算
+    D_X =  (pre_hennsa_X - hennsa_X) * 0.01  / 2;//微分計算
+    pre_hennsa_X = hennsa_X;//一個前の値に設定
+    pre_hennsa_Y = hennsa_Y;//一個前の値に設定
+    P_X = Kp * hennsa_X / (bunnkainou * 4) + Ki * I_X / (bunnkainou * 4) + Kd * D_X / (bunnkainou * 4);//PID制御の計算
+    P_Y = Kp * hennsa_Y / (bunnkainou * 4) + Ki * I_Y / (bunnkainou * 4) + Kd * D_Y / (bunnkainou * 4);//PID制御の計算
+    PW3 = 1.5 * (P_Y - P_X);
+    PW4 = 1.5 * (P_Y + P_X);
+    PW1 = -1.0 * 1.5 * (P_Y - P_X);
+    PW2 = -1.0 * 1.5 * (P_Y + P_X);
+    flag = true;
+}
+int main()
+{
+    flipper.attach(&flip,0.01);
+    while(i) {
+        S = swich.read();
+        pc.printf("%d \r\n",i);
+        if(S == 0) {
+            i = false;
+        }
+    }
+    flipper.attach(&flip,0.01);
+    while(1) {
+        switch(mokuteki) {
+            case HAZIMARI:
+                dst_X = 643.3;
+                dst_Y = 1154;
+                if(abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
+                    mokuteki = HENNKOU;
+                }
+                break;
+            case HENNKOU:
+                dst_X = 96.5;
+                dst_Y = 2135;
+                if(abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
+                    mokuteki = TENNKAI;
+                }
+                break;
+            case TENNKAI:
+                dst_X = 437;
+                dst_Y = 2135;
+                break;
+        }
+        pc.printf("%d %lf %lf %lf %lf %lf %lf %lf %lf \r\n",i,kyori_X,kyori_Y,P_X,P_Y,PW1,PW2,PW3,PW4);
+        sabertooth1.printf("M1:%d\r\n",(int)PW1);
+        sabertooth1.printf("M2:%d\r\n",(int)PW2);
+        sabertooth2.printf("M1:%d\r\n",(int)PW3);
+        sabertooth2.printf("M2:%d\r\n",(int)PW4);
+        flag = false;
+
+    }
+}
+