Revision 2:f34a22b28ac5, committed 2018-07-31

Comitter:

tanabe2000

Date:

Tue Jul 31 08:30:51 2018 +0000

Parent:

1:3f01bf4d7e56

Commit message:

ver2.1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OmniPosition.lib	Tue Jul 31 08:30:51 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/OmniPosition/#47676abdf529

--- a/QEI.lib	Wed Jul 04 09:32:06 2018 +0000
+++ b/QEI.lib	Tue Jul 31 08:30:51 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#40523b009a03
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca

--- 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;
+    }
+
+
+}
+

--- a/measuring_wheel.h	Wed Jul 04 09:32:06 2018 +0000
+++ b/measuring_wheel.h	Tue Jul 31 08:30:51 2018 +0000
@@ -3,6 +3,8 @@
 
 #include "mbed.h"
 #include "QEI.h"
+//#include "r1307.h"
+#include "OmniPosition.h"
 #define PI 3.141592653589793
 
 class MeasuringWheel
@@ -16,27 +18,33 @@
     float getOutX();
     float getOutY();
     void transmissionXY();
-     
+    float getWheel1(),getWheel2(),getWheel3();
+    float getjyroAngle();
+    OmniPosition r1370;
 
 private :
     float diameter[3];
     float radian[3];
-    float wheel[3],r,X,Y;
+    float wheel[3],r,X,Y,subX,subY;
     void computeXY();
+    
        
 
     QEI w1;
     QEI w2;
     QEI w3;
+//    R1307 r1370;
+    float Cdif, yaw;
     Ticker ticker;
-    Ticker ticker1;
     uint16_t X_, Y_,ofsetX, ofsetY;
     char upBitX, upBitY;
     char downBitX,downBitY;
     Serial mainMicon;
     char get;
     DigitalOut led;
-//    void resetpisithon(); 
+    void resetpisithon(); 
+//    void rawAngle();
+    
     
 };