measuring_wheel.cpp

00001 #include "measuring_wheel.h"
00002 
00003 MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX):
00004     w1(channel1_1, channel1_2, NC, PPR, QEI::X4_ENCODING),
00005     w2(channel2_1, channel2_2, NC, PPR, QEI::X4_ENCODING),
00006     w3(channel3_1, channel3_2, NC, PPR, QEI::X4_ENCODING),
00007     r1370(R1370TX, R1370RX)
00008 {
00009     miniX = 0;
00010     miniY = 0;
00011     X = 0;
00012     Y = 0;
00013     loopCounter = 0;
00014 
00015     /*けいそくりんの設定*/
00016     //ホイールの向き
00017     radian[0] = -1 * PIII / 2.0;
00018     radian[1] = PIII / 6.0;
00019     radian[2] = 5.0 * PIII / 6.0;
00020     //直径
00021     diameter[0] = 49.0;
00022     diameter[1] = 49.0;
00023     diameter[2] = 49.0;
00024     //中心からの距離
00025     distance[0] = 124.2;
00026     distance[1] = 124.2;
00027     distance[2] = 124.2;
00028 
00029     coefficient(0) = cos(radian[0]);
00030     coefficient(1) = cos(radian[1]);
00031     coefficient(2) = cos(radian[2]);
00032     coefficient(3) = sin(radian[0]);
00033     coefficient(4) = sin(radian[1]);
00034     coefficient(5) = sin(radian[2]);
00035     coefficient(6) = distance[0];
00036     coefficient(7) = distance[1];
00037     coefficient(8) = distance[2];
00038 
00039     dec = coefficient.fullPivHouseholderQr();
00040 
00041     thread.start(callback(this, &MeasuringWheel::threadloop));
00042 }
00043 
00044 void MeasuringWheel::computeXY()
00045 {
00046     subY = 0;
00047     subX = 0;
00048     wheel(0) =  -diameter[0]*PIII/(PPR*4.0)*w1.getPulses();
00049     wheel(1) =  -diameter[1]*PIII/(PPR*4.0)*w2.getPulses();
00050     wheel(2) =  -diameter[2]*PIII/(PPR*4.0)*w3.getPulses();
00051     w1.reset();
00052     w2.reset();
00053     w3.reset();
00054     yawdegree = r1370.getAngle();
00055     yaw = -1.0*yawdegree;
00056     yaw *= PIII/180.0;
00057 
00058     ans = dec.solve(wheel);
00059 
00060     subX = ans(0);
00061     subY = ans(1);
00062 
00063     loopCounter++;
00064     if(loopCounter > 1000000){
00065         loopCounter = 0;
00066         X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
00067         Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
00068         miniX = 0;
00069         miniY = 0;
00070     }else{
00071         miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
00072         miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
00073     }
00074 
00075     minusX = (X + miniX) * -1;
00076     minusY = (Y + miniY) * -1;
00077 }
00078 
00079 void MeasuringWheel::threadloop()
00080 {
00081     while(true) {
00082         computeXY();
00083     }
00084 }
00085 
00086 double MeasuringWheel::getjyroAngle()
00087 {
00088     return r1370.getAngle();
00089     //return phi;
00090 }
00091 
00092 double MeasuringWheel::getOutX()
00093 {
00094     return minusX;
00095 }
00096 
00097 double MeasuringWheel::getOutY()
00098 {
00099     return -1*minusY;
00100 }
00101 
00102 double MeasuringWheel::getWheel1()
00103 {
00104     return w1.getPulses();
00105 }
00106 
00107 double MeasuringWheel::getWheel2()
00108 {
00109     return w2.getPulses();
00110 }
00111 
00112 double MeasuringWheel::getWheel3()
00113 {
00114     return w3.getPulses();
00115 }
00116 
00117 
00118 
00119 void MeasuringWheel::resetposition()
00120 {
00121     X = 0;
00122     Y = 0;
00123 }