Diff: omni.cpp

Revision:

0:979565e955a5

Child:

5:f2fc4a0b6a6f

Child:

9:a30169882a15

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omni.cpp	Mon Jul 03 14:23:36 2017 +0000
@@ -0,0 +1,100 @@
+#include "omni.h"
+
+inline double fmax(double a, double b)
+{
+    if(a > b) {
+        return a;
+    } else {
+        return b;
+    }
+}
+
+inline double fmin(double a, double b)
+{
+    if(a < b) {
+        return a;
+    } else {
+        return b;
+    }
+}
+
+inline double toRadians(double degrees) {
+    return degrees * (M_PI / 180.0);
+}
+
+Omni::Omni(int wheels, double initDegree) :
+    wheels(wheels), initDegree(initDegree)
+{
+    for(int i = 0; i < wheels; i++) {
+        wheel[i] = 0;
+    }
+}
+
+bool Omni::computeXY(double parallelVector[], double 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]);
+
+    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) {
+        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)
+{
+    wheels = wheel;
+}
+
+void Omni::setInitDegree(double degree)
+{
+    initDegree = degree;
+}
+
+double Omni::getOutput(int num)
+{
+    return wheel[num];
+}