Diff: omni_wheel.cpp

Revision:

0:952a0ff1bf46

Child:

1:e9b590a5b27a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omni_wheel.cpp	Thu Sep 14 05:51:25 2017 +0000
@@ -0,0 +1,58 @@
+#include "omni_wheel.h"
+
+OmniWheel::OmniWheel()
+{
+    wheel = new Wheel[4];
+    wheelNumber = 4;
+}
+
+OmniWheel::OmniWheel(int wheelNumber)
+    : wheelNumber(wheelNumber)
+{
+    wheel = new Wheel[wheelNumber];
+}
+
+void OmniWheel::computeXY(double X, double Y, double gX, double gY, double moment)
+{
+    computeCircular(hypot(X, Y), atan2(Y, X), gX, gY, moment);
+}
+
+void OmniWheel::computeXY(double X, double Y, double moment)
+{
+    computeCircular(hypot(X, Y), atan2(Y, X), 0, 0, moment);
+}
+
+void OmniWheel::computeCircular(double r, double theta, double gX, double gY, double moment)
+{
+    if(wheelNumber <= 0) return;
+    double shiftOut[wheelNumber];
+    double rotateOut[wheelNumber];
+    double shiftMax = -1.0;
+    double rotateMax = -1.0;
+    double shiftMin = 1.0;
+    double rotateMin = 1.0;
+
+    for(int i = 0; i < wheelNumber; i++) {
+        shiftOut[i] = wheel[i].calculateShift(r, theta);
+        rotateOut[i] = wheel[i].calculateRotate(gX, gY, moment);
+        if(shiftOut[i] > shiftMax) shiftMax = shiftOut[i];
+        if(shiftOut[i] < shiftMin) shiftMin = shiftOut[i];
+        if(rotateOut[i] > rotateMax) rotateMax = rotateOut[i];
+        if(rotateOut[i] < rotateMin) rotateMin = rotateOut[i];
+    }
+    if(shiftMax + rotateMax > 1.0 || shiftMin + shiftMin < -1.0) {
+        for(int i = 0; i < wheelNumber; i++) {
+            wheel[i].setOutput(
+                (shiftOut[i] + rotateOut[i]) *
+                1.0 / (
+                    ((fabs(shiftMax) > fabs(shiftMin))?fabs(shiftMax):fabs(shiftMin)) +
+                    ((fabs(rotateMax) > fabs(rotateMin))?fabs(rotateMax):fabs(rotateMin))
+                )
+            );
+        }
+    } else {
+        for(int i = 0; i < wheelNumber; i++) {
+            wheel[i].setOutput(shiftOut[i] + rotateOut[i]);
+        }
+    }
+}