Diff: lib/measuring_wheel/measuring_wheel.cpp

Revision:

0:3ad208cbea5f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/measuring_wheel/measuring_wheel.cpp	Fri Sep 13 02:19:03 2019 +0000
@@ -0,0 +1,123 @@
+#include "measuring_wheel.h"
+
+MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX):
+    w1(channel1_1, channel1_2, NC, PPR, QEI::X4_ENCODING),
+    w2(channel2_1, channel2_2, NC, PPR, QEI::X4_ENCODING),
+    w3(channel3_1, channel3_2, NC, PPR, QEI::X4_ENCODING),
+    r1370(R1370TX, R1370RX)
+{
+    miniX = 0;
+    miniY = 0;
+    X = 0;
+    Y = 0;
+    loopCounter = 0;
+
+    /*けいそくりんの設定*/
+    //ホイールの向き
+    radian[0] = -1 * PIII / 2.0;
+    radian[1] = PIII / 6.0;
+    radian[2] = 5.0 * PIII / 6.0;
+    //直径
+    diameter[0] = 49.0;
+    diameter[1] = 49.0;
+    diameter[2] = 49.0;
+    //中心からの距離
+    distance[0] = 124.2;
+    distance[1] = 124.2;
+    distance[2] = 124.2;
+
+    coefficient(0) = cos(radian[0]);
+    coefficient(1) = cos(radian[1]);
+    coefficient(2) = cos(radian[2]);
+    coefficient(3) = sin(radian[0]);
+    coefficient(4) = sin(radian[1]);
+    coefficient(5) = sin(radian[2]);
+    coefficient(6) = distance[0];
+    coefficient(7) = distance[1];
+    coefficient(8) = distance[2];
+
+    dec = coefficient.fullPivHouseholderQr();
+
+    thread.start(callback(this, &MeasuringWheel::threadloop));
+}
+
+void MeasuringWheel::computeXY()
+{
+    subY = 0;
+    subX = 0;
+    wheel(0) =  -diameter[0]*PIII/(PPR*4.0)*w1.getPulses();
+    wheel(1) =  -diameter[1]*PIII/(PPR*4.0)*w2.getPulses();
+    wheel(2) =  -diameter[2]*PIII/(PPR*4.0)*w3.getPulses();
+    w1.reset();
+    w2.reset();
+    w3.reset();
+    yawdegree = r1370.getAngle();
+    yaw = -1.0*yawdegree;
+    yaw *= PIII/180.0;
+
+    ans = dec.solve(wheel);
+
+    subX = ans(0);
+    subY = ans(1);
+
+    loopCounter++;
+    if(loopCounter > 1000000){
+        loopCounter = 0;
+        X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
+        Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
+        miniX = 0;
+        miniY = 0;
+    }else{
+        miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
+        miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
+    }
+
+    minusX = (X + miniX) * -1;
+    minusY = (Y + miniY) * -1;
+}
+
+void MeasuringWheel::threadloop()
+{
+    while(true) {
+        computeXY();
+    }
+}
+
+double MeasuringWheel::getjyroAngle()
+{
+    return r1370.getAngle();
+    //return phi;
+}
+
+double MeasuringWheel::getOutX()
+{
+    return minusX;
+}
+
+double MeasuringWheel::getOutY()
+{
+    return -1*minusY;
+}
+
+double MeasuringWheel::getWheel1()
+{
+    return w1.getPulses();
+}
+
+double MeasuringWheel::getWheel2()
+{
+    return w2.getPulses();
+}
+
+double MeasuringWheel::getWheel3()
+{
+    return w3.getPulses();
+}
+
+
+
+void MeasuringWheel::resetposition()
+{
+    X = 0;
+    Y = 0;
+}