Diff: measuring_wheel.cpp

Revision:

2:f34a22b28ac5

Parent:

1:3f01bf4d7e56

--- a/measuring_wheel.cpp	Wed Jul 04 09:32:06 2018 +0000
+++ b/measuring_wheel.cpp	Tue Jul 31 08:30:51 2018 +0000
@@ -5,15 +5,17 @@
     w2(channel2_1, channel2_2, NC, 500, QEI::X4_ENCODING),
     w3(channel3_1, channel3_2, NC, 500, QEI::X4_ENCODING),
     mainMicon(PC_10, PC_11, 115200),
+    r1370(PC_6, PC_7),
     led(LED2)
 
 {
-    radian[0] = 0;
-    radian[1] = -2*PI/3;
-    radian[2] = 2*PI/3;
+    radian[0] = 0.0;
+    radian[1] = -2.0*PI/3.0;
+    radian[2] = 2.0*PI/3.0;
     ticker.attach(callback(this, &MeasuringWheel::computeXY),0.01);
-    ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
-//    mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み
+//    ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
+    mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み
+//    angleTiker.attach(callback(this, &MeasuringWheel::rawAngle),0.1);
 }
 
 
@@ -38,10 +40,43 @@
     wheel[0] -= r;
     wheel[1] -= r;
     wheel[2] -= r;
+//    r1370.update();
+    yaw = r1370.getAngle();
+    yaw *= PI/180;
     for(int i = 0; i < 3; i++) {
-        X += wheel[i]*cos(radian[i]);
-        Y += wheel[i]*sin(radian[i]);
+        subX = wheel[i]*cos(radian[i]);
+        subY = wheel[i]*sin(radian[i]);
     }
+    X += subX*cos((float)yaw) - subY*sin((float)yaw);
+    Y += subX*sin((float)yaw) - subY*cos((float)yaw);
+    X+=subX;
+    Y+=subY;
+
+    
+    X_ = X + 32768;
+    Y_ = Y + 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);
+
+}
+
+//void MeasuringWheel::rawAngle()
+//{
+//    r1370.update();
+//    yaw = r1370.getRate();
+//}
+
+float MeasuringWheel::getjyroAngle()
+{
+    return yaw;
 }
 
 float MeasuringWheel::getOutX()
@@ -54,31 +89,37 @@
     return Y;
 }
 
-void MeasuringWheel::transmissionXY()
+float MeasuringWheel::getWheel1()
+{
+    return w1.getPulses();
+}
+
+float MeasuringWheel::getWheel2()
 {
-    X_ = X + 32768;
-    Y_ = Y + 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 w2.getPulses();
+}
+
+float MeasuringWheel::getWheel3()
+{
+    return w3.getPulses();
 }
 
-//void MeasuringWheel::resetpisithon()
+//void MeasuringWheel::transmissionXY()
 //{
-//    get = mainMicon.getc();
-//    if(get != 'R') {
-//        //X = 0;
-////        Y = 0;
-////        led = !led;
-//    }
-//
-//
 //}
 
+void MeasuringWheel::resetpisithon()
+{
+    get = mainMicon.getc();
+    if(get == 'R') {
+        X = 0;
+        Y = 0;
+//        Cdif = yaw + 540 - r1370.getRate();
+//        Cdif %= 360;
+//        Cdif -= 180;
+        led = !led;
+    }
+
+
+}
+