Revision 0:14da14ed70e0, committed 2020-12-26

Comitter:

keiji0604

Date:

Sat Dec 26 09:35:14 2020 +0000

Commit message:

aaaaaaaaaaaaaaaaaaaaaaaaa

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI2.lib	Sat Dec 26 09:35:14 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/kikoaac/code/QEI2/#49fa8827718d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw.lib	Sat Dec 26 09:35:14 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/ARES/code/RoboClaw/#f82bc24e3bd4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Dec 26 09:35:14 2020 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "RoboClaw.h"
+#define PG 200//目標値
+#define PI 3.141592//円周率
+
+RoboClaw robo(130, 115200, p10, p9);//(address, rate, rx, tx)
+Serial pc(USBTX, USBRX, 115200);
+Ticker warikomi;//割り込み処理
+
+int count_r;//4x500
+
+float k_p_pos = 1;//比例ゲイン
+float k_i_pos = 0;//積分ゲイン
+float k_d_pos = 0;//微分ゲイン
+
+float error_pos;//位置PID制御の偏差
+float error_back_pos = 0;
+
+float rev;//回転数
+float rev_back = 0;
+
+float vel;//速度
+float vel_back = 0;
+
+float an_vel;//角速度
+float an_vel_back = 0;
+
+float dist;//移動距離
+float dist_back = 0;
+
+float integ;//積分
+float diff;//微分
+
+float element;//PIDの値
+float cmd;//roboclawに送る値
+
+bool flag = false;
+
+void func()
+{
+    count_r = robo.ReadEncM1();//カウントの読み取り
+    rev = count_r / 2000.0;//分解能はおそらく500
+    dist = rev * 2.0 * PI * 67.0;//67はタイヤの半径(㎜)
+    vel = (dist - dist_back) / 0.01;
+    an_vel = (rev * 2.0 * PI - rev_back * 2.0 * PI) / 0.01;
+
+    error_pos = PG - dist;
+    integ += (error_back_pos + error_pos) / 2.0 * 0.01;
+    diff = (error_pos - error_back_pos) / 0.01;
+
+    element = k_p_pos * error_pos + k_i_pos * integ + k_d_pos * diff;
+
+    cmd = element / 0.01;
+
+    error_back_pos = error_pos;
+    dist_back = dist;
+    vel_back = vel;
+    an_vel_back = an_vel;
+
+    flag = true;
+}
+
+int main()
+{
+    //robo.ResetEnc();
+    warikomi.attach(&func, 0.01);
+
+    while(1) {
+        pc.printf("count:%d  dist:%f  vel:%f  an_vel:%f  rev:%f  error:%f\n", count_r, dist, vel, an_vel, rev, error);
+        if (flag == true) {
+            robo.SpeedM1((int)cmd);
+            flag = false;
+        }
+        wait(0.01);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Dec 26 09:35:14 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file