Revision 0:4d2cd14e1e7e, committed 2022-09-17

Comitter:

yuseis

Date:

Sat Sep 17 08:10:03 2022 +0000

Commit message:

test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Sep 17 08:10:03 2022 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+
+Timer t;
+QEI enc_X(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
+QEI enc_Y(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
+
+Serial pc(USBTX,USBRX,9600);//tera term の準備
+Ticker flipper;//タイム関数用
+int cnt;//エンコーダの値
+RawSerial sabertooth1(pinname_1,NC,115200);//mota_1のモータードライバー出力PINの設定
+RawSerial sabertooth1(pinname_2,NC,115200);//mota_2のモータードライバー出力PINの設定
+RawSerial sabertooth2(pinname_3,NC,115200);//mota_3のモータードライバー出力PINの設定
+RawSerial sabertooth2(pinname_4,NC,115200);//mota_4のモータードライバー出力PINの設定
+DIgitalIn swich(pinname);//スタートスイッチの入力PINの設定
+AnalogIn laser(pinname);//ボール感知センサーの入力PINの設定
+double hennsa = 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 = 1.5;//PID制御のPの係数
+double Ki = 0.7;//PID制御のIの係数
+double Kd = 0.7;//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_X;//目的地のX座標
+double dst_Y;//目的地のY座標
+int ball;//ボールセンサーの値
+int exist;//while文の条件のballの値
+bool flag = false;
+
+void flip()//タイマー割込み
+{
+    cnt_X = enc_X.getPulses();//エンコーダーの値の代入
+    cnt_y = 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 / (bunnkanou * 4) + Ki * I_X / (bunnkanou * 4) + Kd * D_X / (bunnkanou * 4);//PID制御の計算
+    P_Y = Kp * hennsa_Y / (bunnkanou * 4) + Ki * I_Y / (bunnkanou * 4) + Kd * D_Y / (bunnkanou * 4);//PID制御の計算
+    ball = laser.read();
+    if(ball >= 1) {
+        ball == 1;
+    } else if(ball < 1) {
+        ball == 0;
+    }
+    flag = true;
+}
+
+int main()//出力と方向の指定
+{
+    while(swich.read()) {
+        flipper.attach(&flip,0.01);
+        dst_X = 334.5;//目的地座標の設定(PA)
+        dst_Y = 600;//目的座標の設定(PA)
+        exist = 1;//while文の条件の設定
+        //一回目の目的座標の再設定(PB)
+        if(ball == 1) {
+            dst_X = 643.3;
+            dst_Y = 1154;
+            exist = 0;//while文の条件の再設定
+            //回収機構が動き終わったらスタートする文
+        }
+        //モーターの動き
+        while(ball == exist) {
+            if(flag) {
+                //移動の出力設定
+                if(abs(hennsa_X) > 1 && abs(hennsa_Y) > 1) {
+                    PW1 = P_Y - P_X;
+                    PW2 = P_Y + P_X;
+                    PW3 = -1.0 * (P_Y - P_X);
+                    PW4 = -1.0 * (P_Y + P_X);
+                //二回目の目的座標の再設定(PC)   
+                } else if(dst_X = 643.3 && dst_Y = 1154 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
+                    dst_X = 96.5;
+                    dst_Y = 2135;
+                    PW13 = 0;
+                    PW24 = 0;
+                //三回目の目的座標の再設定(PD)
+                } else if(dst_X = 96.5 && dst_Y = 2135 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
+                    dst_X = 437;
+                    dst_Y = 2135;
+                    PW13 = 0;
+                    PW24 = 0;
+                }
+                pc.printf("%d %lf \r\n",cnt,P);
+                //モーターの出力
+                sabertooth1.printf("M1:%d\r\n",PW1);
+                sabertooth1.printf("M2:%d\r\n",PW2);
+                sabertooth2.printf("M1:%d\r\n",PW3);
+                sabertooth2.printf("M2:%d\r\n",PW4);
+                flag = false;
+
+            }
+        }
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Sep 17 08:10:03 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file