2019NHK_teamA_auto_measuring wheel
Dependencies: QEI R1370MeasuringWheel
Revision 8:8ea251946b2a, committed 2019-09-03
- Comitter:
- ec30109b
- Date:
- Tue Sep 03 05:33:38 2019 +0000
- Parent:
- 6:d4dba3255f35
- Commit message:
- keisokurin
Changed in this revision
measuring_wheel.cpp | Show annotated file Show diff for this revision Revisions of this file |
measuring_wheel.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r d4dba3255f35 -r 8ea251946b2a measuring_wheel.cpp --- a/measuring_wheel.cpp Mon Aug 27 06:14:20 2018 +0000 +++ b/measuring_wheel.cpp Tue Sep 03 05:33:38 2019 +0000 @@ -1,118 +1,135 @@ #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), - mainMicon(PC_12,PD_2, 115200), - r1370(PC_10, PC_11), - led(LED2) + w1(channel1_1, channel1_2, NC, 100, QEI::X4_ENCODING), + w2(channel2_1, channel2_2, NC, 100, QEI::X4_ENCODING), + w3(channel3_1, channel3_2, NC, 100, QEI::X4_ENCODING), + serial(mainTX, mainRX), + r1370(R1370TX, R1370RX) +{ + serial.baud(115200); + serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER); + miniX = 0; + miniY = 0; + X = 0; + Y = 0; + phi = 0; + loopCounter = 0; -{ + /*けいそくりんの設定*/ + //ホイールの向き radian[0] = 0.0; - radian[1] = -2.0*PI/3.0; - radian[2] = 2.0*PI/3.0; + radian[1] = -2.0*PIII/3.0; + radian[2] = 2.0*PIII/3.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)); -// ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1); - //mainMicon.attach(callback(this, &MeasuringWheel::resetposition));//受信割り込み -// angleTiker.attach(callback(this, &MeasuringWheel::rawAngle),0.1); -} - - -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(); + subY = 0; + subX = 0; + wheel(0) = -diameter[0]*PIII/400.0*w1.getPulses(); + wheel(1) = -diameter[1]*PIII/400.0*w2.getPulses(); + wheel(2) = -diameter[2]*PIII/400.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; -// r1370.update(); yawdegree = r1370.getAngle(); - yaw = yawdegree; - yaw *= PI/180; -// yaw *= PI/180; - for(int i = 0; i < 3; i++) { - subX += wheel[i]*cos(radian[i]); - subY += wheel[i]*sin(radian[i]); + yaw = -1.0*yawdegree; + yaw *= PIII/180.0; + + ans = dec.solve(wheel); + + subX = ans(0); + subY = ans(1); + dphi = ans(2); + + 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); } - 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; + minusX = (X + miniX) * -1.0; // 2019AのはXとYがマイナスなので + minusY = (Y + miniY) * -1.0; + phi += dphi; + + X_ = minusX + 32768; + Y_ = minusY + 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); - mainMicon.putc(r1370.upbit()); - mainMicon.putc(r1370.downbit()); - mainMicon.putc((unsigned char)(upBitX + downBitY + upBitY + downBitY + r1370.upbit() + r1370.downbit())); - + downBitY = Y_ & 0xff; + txdata[0] = upBitX; + txdata[1] = downBitX; + txdata[2] = upBitY; + txdata[3] = downBitY; + txdata[4] = r1370.upbit(); + txdata[5] = r1370.downbit(); + serial.sendData(txdata, BUFFER_SIZE); } void MeasuringWheel::threadloop() { - while(true){ + while(true) { computeXY(); - } } +} -//void MeasuringWheel::rawAngle() -//{ -// r1370.update(); -// yaw = r1370.getRate(); -//} - -float MeasuringWheel::getjyroAngle() +double MeasuringWheel::getjyroAngle() { - return yawdegree; + //return phi; } -float MeasuringWheel::getOutX() +double MeasuringWheel::getOutX() { - return X; + return minusX; } -float MeasuringWheel::getOutY() +double MeasuringWheel::getOutY() { - return Y; + return minusY; } -float MeasuringWheel::getWheel1() +double MeasuringWheel::getWheel1() { return w1.getPulses(); } -float MeasuringWheel::getWheel2() +double MeasuringWheel::getWheel2() { return w2.getPulses(); } -float MeasuringWheel::getWheel3() +double MeasuringWheel::getWheel3() { return w3.getPulses(); } @@ -121,13 +138,8 @@ void MeasuringWheel::resetposition() { - get = mainMicon.getc(); if(get == 'R') { X = 0; Y = 0; - led = !led; } - - } -
diff -r d4dba3255f35 -r 8ea251946b2a measuring_wheel.h --- a/measuring_wheel.h Mon Aug 27 06:14:20 2018 +0000 +++ b/measuring_wheel.h Tue Sep 03 05:33:38 2019 +0000 @@ -4,53 +4,55 @@ #include "mbed.h" #include "QEI.h" #include "R1370.h" -//#include "OmniPosition.h" -#define PI 3.141592653589793 +#include "pin_config.h" +#include "SerialMultiByte.h" +#include "Dense.h" + +#define FIRST_HEDDER 0xEE +#define SECOND_HEDDER 0xFF +#define BUFFER_SIZE 6 class MeasuringWheel { public : MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2); -// bool - bool wheelDiameter(float diameter1, float diameter2, float diameter3); - - float getOutX(); - float getOutY(); + double getOutX(); + double getOutY(); void transmissionXY(); - float getWheel1(),getWheel2(),getWheel3(); - float getjyroAngle(); -// OmniPosition r1370; - + double getWheel1(),getWheel2(),getWheel3(); + double getjyroAngle(); + double yaw; private : - float diameter[3]; - float radian[3]; - float wheel[3],r,X,Y,subX,subY; + static const double PIII = 3.14159265358979; + double diameter[3]; + double distance[3]; + double radian[3]; + Eigen::Matrix3d coefficient; + Eigen::Vector3d wheel; + Eigen::FullPivHouseholderQR<Eigen::Matrix3d> dec; + Eigen::Vector3d ans; + double miniX,miniY,X,Y,subX,subY,minusX,minusY,dphi, phi; + int loopCounter; void computeXY(); void threadloop(); - - QEI w1; QEI w2; QEI w3; + SerialMultiByte serial; R1370 r1370; - float Cdif, yaw, yawdegree; -// Ticker ticker; + double Cdif, yawdegree; Thread thread; uint16_t X_, Y_,ofsetX, ofsetY, yawdegree_; char upBitX, upBitY, upBitAngle; char downBitX,downBitY, downBitAngle; - Serial mainMicon; char get; - DigitalOut led; - void resetposition(); - -// unsigned char upBitangle down; -// void rawAngle(); - - - + void resetposition(); + uint8_t txdata[BUFFER_SIZE]; + + + }; -#endif \ No newline at end of file +#endif