まだ使えません
Dependencies: QEI OmniPosition
measuring_wheel.cpp@2:f34a22b28ac5, 2018-07-31 (annotated)
- Committer:
- tanabe2000
- Date:
- Tue Jul 31 08:30:51 2018 +0000
- Revision:
- 2:f34a22b28ac5
- Parent:
- 1:3f01bf4d7e56
ver2.1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tanabe2000 | 0:505dd5510add | 1 | #include "measuring_wheel.h" |
tanabe2000 | 0:505dd5510add | 2 | |
tanabe2000 | 0:505dd5510add | 3 | MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2): |
tanabe2000 | 0:505dd5510add | 4 | w1(channel1_1, channel1_2, NC, 500, QEI::X4_ENCODING), |
tanabe2000 | 0:505dd5510add | 5 | w2(channel2_1, channel2_2, NC, 500, QEI::X4_ENCODING), |
tanabe2000 | 1:3f01bf4d7e56 | 6 | w3(channel3_1, channel3_2, NC, 500, QEI::X4_ENCODING), |
tanabe2000 | 1:3f01bf4d7e56 | 7 | mainMicon(PC_10, PC_11, 115200), |
tanabe2000 | 2:f34a22b28ac5 | 8 | r1370(PC_6, PC_7), |
tanabe2000 | 1:3f01bf4d7e56 | 9 | led(LED2) |
tanabe2000 | 0:505dd5510add | 10 | |
tanabe2000 | 0:505dd5510add | 11 | { |
tanabe2000 | 2:f34a22b28ac5 | 12 | radian[0] = 0.0; |
tanabe2000 | 2:f34a22b28ac5 | 13 | radian[1] = -2.0*PI/3.0; |
tanabe2000 | 2:f34a22b28ac5 | 14 | radian[2] = 2.0*PI/3.0; |
tanabe2000 | 1:3f01bf4d7e56 | 15 | ticker.attach(callback(this, &MeasuringWheel::computeXY),0.01); |
tanabe2000 | 2:f34a22b28ac5 | 16 | // ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1); |
tanabe2000 | 2:f34a22b28ac5 | 17 | mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み |
tanabe2000 | 2:f34a22b28ac5 | 18 | // angleTiker.attach(callback(this, &MeasuringWheel::rawAngle),0.1); |
tanabe2000 | 0:505dd5510add | 19 | } |
tanabe2000 | 0:505dd5510add | 20 | |
tanabe2000 | 0:505dd5510add | 21 | |
tanabe2000 | 0:505dd5510add | 22 | bool MeasuringWheel::wheelDiameter(float diameter1, float diameter2, float diameter3) |
tanabe2000 | 0:505dd5510add | 23 | { |
tanabe2000 | 0:505dd5510add | 24 | diameter[0] = diameter1; |
tanabe2000 | 0:505dd5510add | 25 | diameter[1] = diameter2; |
tanabe2000 | 0:505dd5510add | 26 | diameter[2] = diameter3; |
tanabe2000 | 0:505dd5510add | 27 | return 1; |
tanabe2000 | 0:505dd5510add | 28 | } |
tanabe2000 | 0:505dd5510add | 29 | |
tanabe2000 | 0:505dd5510add | 30 | void MeasuringWheel::computeXY() |
tanabe2000 | 0:505dd5510add | 31 | { |
tanabe2000 | 0:505dd5510add | 32 | |
tanabe2000 | 0:505dd5510add | 33 | wheel[0] = diameter[0]*PI/2000.0*w1.getPulses(); |
tanabe2000 | 0:505dd5510add | 34 | wheel[1] = diameter[1]*PI/2000.0*w2.getPulses(); |
tanabe2000 | 0:505dd5510add | 35 | wheel[2] = diameter[2]*PI/2000.0*w3.getPulses(); |
tanabe2000 | 0:505dd5510add | 36 | w1.reset(); |
tanabe2000 | 0:505dd5510add | 37 | w2.reset(); |
tanabe2000 | 0:505dd5510add | 38 | w3.reset(); |
tanabe2000 | 0:505dd5510add | 39 | r = (wheel[0] + wheel[1] + wheel[2])/3.0; |
tanabe2000 | 0:505dd5510add | 40 | wheel[0] -= r; |
tanabe2000 | 0:505dd5510add | 41 | wheel[1] -= r; |
tanabe2000 | 0:505dd5510add | 42 | wheel[2] -= r; |
tanabe2000 | 2:f34a22b28ac5 | 43 | // r1370.update(); |
tanabe2000 | 2:f34a22b28ac5 | 44 | yaw = r1370.getAngle(); |
tanabe2000 | 2:f34a22b28ac5 | 45 | yaw *= PI/180; |
tanabe2000 | 0:505dd5510add | 46 | for(int i = 0; i < 3; i++) { |
tanabe2000 | 2:f34a22b28ac5 | 47 | subX = wheel[i]*cos(radian[i]); |
tanabe2000 | 2:f34a22b28ac5 | 48 | subY = wheel[i]*sin(radian[i]); |
tanabe2000 | 0:505dd5510add | 49 | } |
tanabe2000 | 2:f34a22b28ac5 | 50 | X += subX*cos((float)yaw) - subY*sin((float)yaw); |
tanabe2000 | 2:f34a22b28ac5 | 51 | Y += subX*sin((float)yaw) - subY*cos((float)yaw); |
tanabe2000 | 2:f34a22b28ac5 | 52 | X+=subX; |
tanabe2000 | 2:f34a22b28ac5 | 53 | Y+=subY; |
tanabe2000 | 2:f34a22b28ac5 | 54 | |
tanabe2000 | 2:f34a22b28ac5 | 55 | |
tanabe2000 | 2:f34a22b28ac5 | 56 | X_ = X + 32768; |
tanabe2000 | 2:f34a22b28ac5 | 57 | Y_ = Y + 32768; |
tanabe2000 | 2:f34a22b28ac5 | 58 | upBitX = (X_ >> 8) & 0xff; |
tanabe2000 | 2:f34a22b28ac5 | 59 | downBitX = X_ & 0xff; |
tanabe2000 | 2:f34a22b28ac5 | 60 | upBitY = (Y_ >> 8) & 0xff; |
tanabe2000 | 2:f34a22b28ac5 | 61 | downBitY = Y_ & 0xff; |
tanabe2000 | 2:f34a22b28ac5 | 62 | mainMicon.putc(72); |
tanabe2000 | 2:f34a22b28ac5 | 63 | mainMicon.putc(42); |
tanabe2000 | 2:f34a22b28ac5 | 64 | mainMicon.putc(upBitX); |
tanabe2000 | 2:f34a22b28ac5 | 65 | mainMicon.putc(downBitX); |
tanabe2000 | 2:f34a22b28ac5 | 66 | mainMicon.putc(upBitY); |
tanabe2000 | 2:f34a22b28ac5 | 67 | mainMicon.putc(downBitY); |
tanabe2000 | 2:f34a22b28ac5 | 68 | |
tanabe2000 | 2:f34a22b28ac5 | 69 | } |
tanabe2000 | 2:f34a22b28ac5 | 70 | |
tanabe2000 | 2:f34a22b28ac5 | 71 | //void MeasuringWheel::rawAngle() |
tanabe2000 | 2:f34a22b28ac5 | 72 | //{ |
tanabe2000 | 2:f34a22b28ac5 | 73 | // r1370.update(); |
tanabe2000 | 2:f34a22b28ac5 | 74 | // yaw = r1370.getRate(); |
tanabe2000 | 2:f34a22b28ac5 | 75 | //} |
tanabe2000 | 2:f34a22b28ac5 | 76 | |
tanabe2000 | 2:f34a22b28ac5 | 77 | float MeasuringWheel::getjyroAngle() |
tanabe2000 | 2:f34a22b28ac5 | 78 | { |
tanabe2000 | 2:f34a22b28ac5 | 79 | return yaw; |
tanabe2000 | 0:505dd5510add | 80 | } |
tanabe2000 | 0:505dd5510add | 81 | |
tanabe2000 | 0:505dd5510add | 82 | float MeasuringWheel::getOutX() |
tanabe2000 | 0:505dd5510add | 83 | { |
tanabe2000 | 0:505dd5510add | 84 | return X; |
tanabe2000 | 0:505dd5510add | 85 | } |
tanabe2000 | 0:505dd5510add | 86 | |
tanabe2000 | 0:505dd5510add | 87 | float MeasuringWheel::getOutY() |
tanabe2000 | 0:505dd5510add | 88 | { |
tanabe2000 | 0:505dd5510add | 89 | return Y; |
tanabe2000 | 0:505dd5510add | 90 | } |
tanabe2000 | 0:505dd5510add | 91 | |
tanabe2000 | 2:f34a22b28ac5 | 92 | float MeasuringWheel::getWheel1() |
tanabe2000 | 2:f34a22b28ac5 | 93 | { |
tanabe2000 | 2:f34a22b28ac5 | 94 | return w1.getPulses(); |
tanabe2000 | 2:f34a22b28ac5 | 95 | } |
tanabe2000 | 2:f34a22b28ac5 | 96 | |
tanabe2000 | 2:f34a22b28ac5 | 97 | float MeasuringWheel::getWheel2() |
tanabe2000 | 0:505dd5510add | 98 | { |
tanabe2000 | 2:f34a22b28ac5 | 99 | return w2.getPulses(); |
tanabe2000 | 2:f34a22b28ac5 | 100 | } |
tanabe2000 | 2:f34a22b28ac5 | 101 | |
tanabe2000 | 2:f34a22b28ac5 | 102 | float MeasuringWheel::getWheel3() |
tanabe2000 | 2:f34a22b28ac5 | 103 | { |
tanabe2000 | 2:f34a22b28ac5 | 104 | return w3.getPulses(); |
tanabe2000 | 1:3f01bf4d7e56 | 105 | } |
tanabe2000 | 0:505dd5510add | 106 | |
tanabe2000 | 2:f34a22b28ac5 | 107 | //void MeasuringWheel::transmissionXY() |
tanabe2000 | 1:3f01bf4d7e56 | 108 | //{ |
tanabe2000 | 1:3f01bf4d7e56 | 109 | //} |
tanabe2000 | 1:3f01bf4d7e56 | 110 | |
tanabe2000 | 2:f34a22b28ac5 | 111 | void MeasuringWheel::resetpisithon() |
tanabe2000 | 2:f34a22b28ac5 | 112 | { |
tanabe2000 | 2:f34a22b28ac5 | 113 | get = mainMicon.getc(); |
tanabe2000 | 2:f34a22b28ac5 | 114 | if(get == 'R') { |
tanabe2000 | 2:f34a22b28ac5 | 115 | X = 0; |
tanabe2000 | 2:f34a22b28ac5 | 116 | Y = 0; |
tanabe2000 | 2:f34a22b28ac5 | 117 | // Cdif = yaw + 540 - r1370.getRate(); |
tanabe2000 | 2:f34a22b28ac5 | 118 | // Cdif %= 360; |
tanabe2000 | 2:f34a22b28ac5 | 119 | // Cdif -= 180; |
tanabe2000 | 2:f34a22b28ac5 | 120 | led = !led; |
tanabe2000 | 2:f34a22b28ac5 | 121 | } |
tanabe2000 | 2:f34a22b28ac5 | 122 | |
tanabe2000 | 2:f34a22b28ac5 | 123 | |
tanabe2000 | 2:f34a22b28ac5 | 124 | } |
tanabe2000 | 2:f34a22b28ac5 | 125 |