Revision 10:4f0b55344c73, committed 2017-08-23

Comitter:

UCHITAKE

Date:

Wed Aug 23 07:26:26 2017 +0000

Parent:

8:ab46e745cfab

Commit message:

fix commit

Changed in this revision

diff -r ab46e745cfab -r 4f0b55344c73 omni.cpp
--- a/omni.cpp	Sun Aug 06 14:49:01 2017 +0000
+++ b/omni.cpp	Wed Aug 23 07:26:26 2017 +0000
@@ -1,5 +1,13 @@
 #include "omni.h"
 
+Omni::Omni(int wheels) : wheels(wheels)
+{
+    for(int i = 0; i < wheels; i++) {
+        wheel[i] = 0;
+        radian[i] = 0;
+    }
+}
+
 inline double fmax(double a, double b)
 {
     if(a > b) {
@@ -18,87 +26,87 @@
     }
 }
 
-inline double toRadians(double degrees) {
-    return degrees * (M_PI / 180.0);
+bool isInRange(float X, float Y)
+{
+    if(X < -1 || X > 1 || Y < -1 || Y > 1) return 0;
+    if(hypot(X, Y) > 1.0) return 0;
+    return 1;
+}
+
+bool isInRange(float r)
+{
+    if(r < 0 || r > 1) return 0;
+    return 1;
+}
+
+
+bool Omni::setWheelRadian(float rad1, float rad2, float rad3)
+{
+    if(wheels != 3) return 0;
+    radian[0] = rad1;
+    radian[1] = rad2;
+    radian[2] = rad3;
+    return 1;
 }
 
-inline double toDegrees(double radians) {
-    return radians * (180.0 / M_PI);
+bool Omni::setWheelRadian(float rad1, float rad2, float rad3, float rad4)
+{
+    if(wheels != 4) return 0;
+    radian[0] = rad1;
+    radian[1] = rad2;
+    radian[2] = rad3;
+    radian[3] = rad4;
+    return 1;
 }
 
-Omni::Omni(int wheels, double initDegree) :
-    wheels(wheels), initDegree(initDegree)
+bool Omni::setWheelRadian(int wheelNumber, float rad)
 {
-    for(int i = 0; i < wheels; i++) {
-        wheel[i] = 0;
+    if(wheelNumber > 3|| wheelNumber <= 0) return 0;
+    radian[wheelNumber] = rad;
+    return 1;
+}
+
+bool Omni::computeXY(float X, float Y, float moment)
+{
+    if(isInRange(X, Y)) {
+        return computeCircular(hypot(X, Y), atan2(Y, X), moment);
+    } else {
+        return 0;
     }
 }
 
-bool Omni::computeXY(double parallelVector[], double moment)
+
+bool Omni::computeCircular(float r, float rad, float moment)
 {
-    if(wheels < 3 || wheels > 4) {
-        return 0;
-    }
-
-    double polar[2];
-
-    polar[0] = toDegrees(atan2(parallelVector[1], parallelVector[0]));
-    polar[1] = hypot(parallelVector[0], parallelVector[1]);
+    if(wheels <= 0) return 0;
+    if(isInRange(r)) {
+        double parallel[4] = {0};
+        double parallelMax = -1;
+        double parallelMin = 1;
 
-    return computePolar(polar, moment);
-}
-
-bool Omni::computePolar(double parallelVector[], double moment)
-{
-    if(wheels < 3 || wheels > 4) {
-        return 0;
-    }
-
-    if(parallelVector[1] < 0 || parallelVector[1] > 1.0) {
+        for(int i = 0; i < wheels; i++) {
+            parallel[i] = sin(rad + radian[i]) * r;
+        }
+        for(int i = 0; i < wheels; i++) {
+            parallelMax = fmax(parallel[i], parallelMax);
+            parallelMin = fmin(parallel[i], parallelMin);
+        }
+        if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
+            for(int i = 0; i < wheels; i++) {
+                parallel[i] *= (1.0 - fabs(moment));
+            }
+        }
+        for(int i = 0; i < wheels; i++) {
+            wheel[i] = parallel[i] + moment;
+        }
+        return 1;
+    } else {
         return 0;
     }
-
-    double parallel[4] = {0};
-    double parallelMax = -1;
-    double parallelMin = 1;
-
-    for(int i = 0; i < wheels; i++) {
-        parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1];
-    }
-    for(int i = 0; i < wheels; i++) {
-        parallelMax = fmax(parallel[i], parallelMax);
-        parallelMin = fmin(parallel[i], parallelMin);
-    }
-    if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
-        for(int i = 0; i < wheels; i++) {
-            parallel[i] *= (1.0 - fabs(moment));
-        }
-    }
-    for(int i = 0; i < wheels; i++) {
-        wheel[i] = parallel[i] + moment;
-    }
-    return 1;
 }
 
-bool Omni::stop()
-{
-    for(int i = 0; i < wheels; i++) {
-        wheel[i] = 0;
-    }
-    return 1;
-}
 
-void Omni::setWheels(int wheel)
+float Omni::getOutput(int wheelNumber)
 {
-    wheels = wheel;
-}
-
-void Omni::setInitDegree(double degree)
-{
-    initDegree = degree;
-}
-
-double Omni::getOutput(int num)
-{
-    return wheel[num];
-}
+    return wheel[wheelNumber];
+}
\ No newline at end of file

diff -r ab46e745cfab -r 4f0b55344c73 omni.h
--- a/omni.h	Sun Aug 06 14:49:01 2017 +0000
+++ b/omni.h	Wed Aug 23 07:26:26 2017 +0000
@@ -1,90 +1,80 @@
-/**
- * @author keitaro takeuchi
- *
- * @section DESCRIPTION
- * 3or4wheels omni libraryfor NHK2017.
- */
-
-#ifndef OMNI
-#define OMNI
-
-/**
- * Includes
- */
+/**
+ * @file omni.h
+ * @author keitaro takeuchi
+ *
+ * @section DESCRIPTION
+ * 3or4wheels omni libraryfor NHK2017.
+ */
+#ifndef OMNI_H
+#define OMNI_H
+
+/**
+ * Includes
+ */
 #include "mbed.h"
-
-/**
- * Defines
- */
+
+/**
+ * Defines
+ */
 #define M_PI 3.141592653589793
-
-/**
- * omni wheel
- */
+
+/**
+ * omni wheel
+ */
 class Omni {
-public:
-
-    /**
-     * Constructor.
-     *
-     * @param wheels 車輪数(3or4)
-     * @param initDegree 正面から車輪の角度
-     */
-    Omni(int wheels, double initDegree);
-
-    /**
-     * 位置ベクトル(x, y), 回転量から出力を計算
-     *
-     * @param parallelVector[] [x, y](-1 ~ 1)
-     * @param moment 回転量(-1 ~ 1)
-     *
-     * @return 1...success 0...failure
-     */
-    bool computeXY(double parallelVector[], double moment);
-
-    /**
-     * 角度, 大きさ(θ, volume), 回転量から出力を計算
-     *
-     * @param parallelVector[] [θ(degree), volume] volume...(-1 ~ 1)
-     * @param moment 回転量(-1 ~ 1)
-     * @return 1...success 0...failure
-     */
-    bool computePolar(double parallelVector[], double moment);
-
-    /**
-     * 全ての出力を0にする
-     *
-     * @return 1...success 0...failure
-     */
-    bool stop();
-
-    /**
-     * ホイール数を設定
-     *
-     * @param wheel ホイール数
-     */
-    void setWheels(int wheel);
-
-    /**
-     * 正面から車輪の角度を設定
-     *
-     * @param degree 正面から車輪の角度
-     */
-    void setInitDegree(double degree);
-
-    /**
-     * 出力値を取得
-     *
-     * @param wheel wheel番目のホイールの出力を設定
-     *
-     * @return 出力値
-     */
-    double getOutput(int wheel);
+public :
 
-private:
+    /**
+     * Constructor.
+     *
+     * @param wheels 車輪数(3or4)
+     */
+    Omni(int wheels);
+    
+    /**
+     * ホイールの付いている角度を設定
+     *
+     * @param rad radian
+     * @param wheelNumber 番目のホイール
+     */
+    bool setWheelRadian(float rad1, float rad2, float rad3);
+    bool setWheelRadian(float rad1, float rad2, float rad3, float rad4);
+    bool setWheelRadian(int wheelNumber, float rad);
+    
+    /**
+     * 位置ベクトル(x, y), 回転量から出力を計算
+     *
+     * @param X(-1 ~ 1)
+     * @param Y(-1 ~ 1)
+     * @param moment 回転量(-1 ~ 1)
+     *
+     * @return 1...success 0...failure
+     */
+    bool computeXY(float X, float Y, float moment);
+    
+    /**
+     * 半径, 角度radian(r, Θ), 回転量から出力を計算
+     *
+     * @param r 半径(0 ~ 1)
+     * @param rad radian(-PI ~ PI)
+     * @param moment 回転量(-1 ~ 1)
+     * @return 1...success 0...failure
+     */
+    bool computeCircular(float r, float rad, float moment);
+    
+    /**
+     * 出力値を取得
+     *
+     * @param wheelNumber wheel番目のホイールの出力を設定
+     *
+     * @return 出力値
+     */
+    float getOutput(int wheelNumber);
+private :
+protected :
     int wheels;
-    double initDegree;
-    double wheel[4];
+    float wheel[4];
+    float radian[4];
 };
 
-#endif /* OMNI */
+#endif//OMNI_H
\ No newline at end of file