Diff: omni.cpp

Revision:

9:a30169882a15

Parent:

0:979565e955a5

--- a/omni.cpp	Mon Jul 03 14:23:36 2017 +0000
+++ b/omni.cpp	Wed Aug 23 07:24:34 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,83 +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;
 }
 
-Omni::Omni(int wheels, double initDegree) :
-    wheels(wheels), initDegree(initDegree)
+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;
+}
+
+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] = 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 = 0;
-    double parallelMin = 0;
-
-    for(int i = 0; i < wheels; i++) {
-        parallel[i] = sin(toRadians(parallelVector[0] + initDegree)) * parallelVector[1];
-    }
-    for(int i = 0; i < wheels - 1; i++) {
-        parallelMax = fmax(parallel[i], parallel[i + 1]);
-        parallelMin = fmin(parallel[i], parallel[i + 1]);
-    }
-    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