Revision 0:ae2c570afbe8, committed 2018-05-28

Comitter:

surpace0924

Date:

Mon May 28 23:15:55 2018 +0000

Commit message:

Initial commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/README.md	Mon May 28 23:15:55 2018 +0000
@@ -0,0 +1,2 @@
+# newMecanumLibrary
+回転成分関係をもう少しちゃんと考えた新たなメカナムのライブラリ

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/newMecanum.cpp	Mon May 28 23:15:55 2018 +0000
@@ -0,0 +1,43 @@
+#include "newMecanum.hpp"
+
+void newMecanum::init(float length, float width, float _maxVal)
+{
+    float vectorAngle = atan2(length, width);
+
+    // 理想的なメカナムホイールの配置（45deg）からのズレの度合い
+    rotationCorrection = cos(vectorAngle - PI / 4); 
+    
+    maxSpeed = _maxSpeed;
+}
+
+void newMecanum::calculate(float vx, float vy, float angularVelocity)
+{
+    // 計算（ヤコビ行列の中身を展開した状態で記述）
+    v[0] = vx + vy + rotationCorrection * angularVelocity;
+    v[1] = vx - vy + rotationCorrection * angularVelocity;
+    v[2] = -vx + vy + rotationCorrection * angularVelocity;
+    v[3] = -vx - vy + rotationCorrection * angularVelocity;
+
+    // ガード処理
+    float maxVal = 0;
+    for (int i = 0; i < 4; i++)
+    {
+        if (abs(v[i]) > maxVal)
+        {
+            maxVal = v[i];
+        }
+    }
+
+    if (maxVal > maxSpeed) 
+    {
+        for (int i = 0; i < 4; i++)
+        {
+            v[i] = map(v[i], -maxVal, maxVal, -maxSpeed, maxSpeed);
+        }
+    }
+}
+
+float newMecanum::getWheelVelocity(short num)
+{
+    return v[num];
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/newMecanum.hpp	Mon May 28 23:15:55 2018 +0000
@@ -0,0 +1,25 @@
+#ifndef _NEW_MECANUM_H_
+#define _NEW_MECANUM_H_
+
+#include "mbed.h"
+
+#ifndef PI
+#define PI 3.141592f
+#endif
+
+class newMecanum
+{
+  public:
+    void init(float length, float width);
+
+    void calculate(float vx, float vy, float angularVelocity);
+
+    float getWheelVelocity(short num);
+
+  private:
+    float v[4];
+    float maxSpeed;
+    float rotationCorrection;
+};
+
+#endif