Diff: measuring_wheel.cpp

Revision:

0:505dd5510add

Child:

1:3f01bf4d7e56

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/measuring_wheel.cpp	Mon Jul 02 08:38:07 2018 +0000
@@ -0,0 +1,88 @@
+#include "measuring_wheel.h"
+
+MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2):
+    w1(channel1_1, channel1_2, NC, 500, QEI::X4_ENCODING),
+    w2(channel2_1, channel2_2, NC, 500, QEI::X4_ENCODING),
+    w3(channel3_1, channel3_2, NC, 500, QEI::X4_ENCODING)
+
+{
+    radian[0] = 0;
+    radian[1] = -2*PI/3;
+    radian[2] = 2*PI/3;
+    ticker.attach(callback(this, &MeasuringWheel::computeXY),0.001);
+}
+
+
+bool MeasuringWheel::wheelDiameter(float diameter1, float diameter2, float diameter3)
+{
+    diameter[0] = diameter1;
+    diameter[1] = diameter2;
+    diameter[2] = diameter3;
+    return 1;
+}
+
+void MeasuringWheel::computeXY()
+{
+
+    wheel[0] =  diameter[0]*PI/2000.0*w1.getPulses();
+    wheel[1] =  diameter[1]*PI/2000.0*w2.getPulses();
+    wheel[2] =  diameter[2]*PI/2000.0*w3.getPulses();
+    w1.reset();
+    w2.reset();
+    w3.reset();
+    r = (wheel[0] + wheel[1] + wheel[2])/3.0;
+    wheel[0] -= r;
+    wheel[1] -= r;
+    wheel[2] -= r;
+    for(int i = 0; i < 3; i++) {
+        X += wheel[i]*cos(radian[i]);
+        Y += wheel[i]*sin(radian[i]);
+    }
+}
+
+float MeasuringWheel::getOutX()
+{
+    return X;
+}
+
+float MeasuringWheel::getOutY()
+{
+    return Y;
+}
+
+TransmissionXY::TransmissionXY(PinName serialTX,PinName serialRX): mainMicon(serialTX, serialRX, 115200), led(LED2)
+{
+//    mainMicon.attach(callback(this, &TransmissionXY::resetpisithon));
+}
+
+void TransmissionXY::resetpisithon()
+{
+    get = mainMicon.getc();
+    if(get == 'R') {
+        ofsetX = X_;
+        ofsetY = Y_;
+        led = !led;
+    }
+    X_ -= ofsetX;
+    Y_ -= ofsetY;
+
+}
+
+bool TransmissionXY::sendMeasuring(MeasuringWheel &request)
+{
+
+    X_ = request.getOutX()+32768;
+    Y_ = request.getOutY()+32768;
+    upBitX = (X_ >> 8) & 0xff;
+    downBitX = X_ & 0xff;
+    upBitY = (Y_ >> 8) & 0xff;
+    downBitY = Y_ & 0xff;
+    mainMicon.putc(72);
+    mainMicon.putc(42);
+    mainMicon.putc(upBitX);
+    mainMicon.putc(downBitX);
+    mainMicon.putc(upBitY);
+    mainMicon.putc(downBitY);
+
+    return 1;
+}
\ No newline at end of file