Soma Takahashi / measuring_wheel

2019NHK_teamA_auto_measuring wheel

Dependencies:   QEI R1370MeasuringWheel

measuring_wheel.cpp@8:8ea251946b2a, 2019-09-03 (annotated)

Committer:
ec30109b
Date:
Tue Sep 03 05:33:38 2019 +0000
Revision:
8:8ea251946b2a
Parent:
3:30045028d27e 
keisokurin

Who changed what in which revision?

UserRevisionLine numberNew 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):
ec30109b 8:8ea251946b2a 4 w1(channel1_1, channel1_2, NC, 100, QEI::X4_ENCODING),
ec30109b 8:8ea251946b2a 5 w2(channel2_1, channel2_2, NC, 100, QEI::X4_ENCODING),
ec30109b 8:8ea251946b2a 6 w3(channel3_1, channel3_2, NC, 100, QEI::X4_ENCODING),
ec30109b 8:8ea251946b2a 7 serial(mainTX, mainRX),
ec30109b 8:8ea251946b2a 8 r1370(R1370TX, R1370RX)
ec30109b 8:8ea251946b2a 9 {
ec30109b 8:8ea251946b2a 10 serial.baud(115200);
ec30109b 8:8ea251946b2a 11 serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
ec30109b 8:8ea251946b2a 12 miniX = 0;
ec30109b 8:8ea251946b2a 13 miniY = 0;
ec30109b 8:8ea251946b2a 14 X = 0;
ec30109b 8:8ea251946b2a 15 Y = 0;
ec30109b 8:8ea251946b2a 16 phi = 0;
ec30109b 8:8ea251946b2a 17 loopCounter = 0;
tanabe2000 0:505dd5510add 18
ec30109b 8:8ea251946b2a 19 /*けいそくりんの設定*/
ec30109b 8:8ea251946b2a 20 //ホイールの向き
tanabe2000 2:f34a22b28ac5 21 radian[0] = 0.0;
ec30109b 8:8ea251946b2a 22 radian[1] = -2.0*PIII/3.0;
ec30109b 8:8ea251946b2a 23 radian[2] = 2.0*PIII/3.0;
ec30109b 8:8ea251946b2a 24 //直径
ec30109b 8:8ea251946b2a 25 diameter[0] = 49.0;
ec30109b 8:8ea251946b2a 26 diameter[1] = 49.0;
ec30109b 8:8ea251946b2a 27 diameter[2] = 49.0;
ec30109b 8:8ea251946b2a 28 //中心からの距離
ec30109b 8:8ea251946b2a 29 distance[0] = 124.2;
ec30109b 8:8ea251946b2a 30 distance[1] = 124.2;
ec30109b 8:8ea251946b2a 31 distance[2] = 124.2;
ec30109b 8:8ea251946b2a 32
ec30109b 8:8ea251946b2a 33 coefficient(0) = cos(radian[0]);
ec30109b 8:8ea251946b2a 34 coefficient(1) = cos(radian[1]);
ec30109b 8:8ea251946b2a 35 coefficient(2) = cos(radian[2]);
ec30109b 8:8ea251946b2a 36 coefficient(3) = sin(radian[0]);
ec30109b 8:8ea251946b2a 37 coefficient(4) = sin(radian[1]);
ec30109b 8:8ea251946b2a 38 coefficient(5) = sin(radian[2]);
ec30109b 8:8ea251946b2a 39 coefficient(6) = distance[0];
ec30109b 8:8ea251946b2a 40 coefficient(7) = distance[1];
ec30109b 8:8ea251946b2a 41 coefficient(8) = distance[2];
ec30109b 8:8ea251946b2a 42
ec30109b 8:8ea251946b2a 43 dec = coefficient.fullPivHouseholderQr();
ec30109b 8:8ea251946b2a 44
tanabe2000 3:30045028d27e 45 thread.start(callback(this, &MeasuringWheel::threadloop));
tanabe2000 0:505dd5510add 46 }
tanabe2000 0:505dd5510add 47
tanabe2000 0:505dd5510add 48 void MeasuringWheel::computeXY()
tanabe2000 0:505dd5510add 49 {
ec30109b 8:8ea251946b2a 50 subY = 0;
ec30109b 8:8ea251946b2a 51 subX = 0;
ec30109b 8:8ea251946b2a 52 wheel(0) = -diameter[0]*PIII/400.0*w1.getPulses();
ec30109b 8:8ea251946b2a 53 wheel(1) = -diameter[1]*PIII/400.0*w2.getPulses();
ec30109b 8:8ea251946b2a 54 wheel(2) = -diameter[2]*PIII/400.0*w3.getPulses();
tanabe2000 0:505dd5510add 55 w1.reset();
tanabe2000 0:505dd5510add 56 w2.reset();
tanabe2000 0:505dd5510add 57 w3.reset();
tanabe2000 3:30045028d27e 58 yawdegree = r1370.getAngle();
ec30109b 8:8ea251946b2a 59 yaw = -1.0*yawdegree;
ec30109b 8:8ea251946b2a 60 yaw *= PIII/180.0;
ec30109b 8:8ea251946b2a 61
ec30109b 8:8ea251946b2a 62 ans = dec.solve(wheel);
ec30109b 8:8ea251946b2a 63
ec30109b 8:8ea251946b2a 64 subX = ans(0);
ec30109b 8:8ea251946b2a 65 subY = ans(1);
ec30109b 8:8ea251946b2a 66 dphi = ans(2);
ec30109b 8:8ea251946b2a 67
ec30109b 8:8ea251946b2a 68 loopCounter++;
ec30109b 8:8ea251946b2a 69 if(loopCounter > 1000000){
ec30109b 8:8ea251946b2a 70 loopCounter = 0;
ec30109b 8:8ea251946b2a 71 X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
ec30109b 8:8ea251946b2a 72 Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
ec30109b 8:8ea251946b2a 73 miniX = 0;
ec30109b 8:8ea251946b2a 74 miniY = 0;
ec30109b 8:8ea251946b2a 75 }else{
ec30109b 8:8ea251946b2a 76 miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
ec30109b 8:8ea251946b2a 77 miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
tanabe2000 0:505dd5510add 78 }
tanabe2000 2:f34a22b28ac5 79
ec30109b 8:8ea251946b2a 80 minusX = (X + miniX) * -1.0; // 2019AのはXとYがマイナスなので
ec30109b 8:8ea251946b2a 81 minusY = (Y + miniY) * -1.0;
ec30109b 8:8ea251946b2a 82 phi += dphi;
ec30109b 8:8ea251946b2a 83
ec30109b 8:8ea251946b2a 84 X_ = minusX + 32768;
ec30109b 8:8ea251946b2a 85 Y_ = minusY + 32768;
tanabe2000 2:f34a22b28ac5 86 upBitX = (X_ >> 8) & 0xff;
tanabe2000 2:f34a22b28ac5 87 downBitX = X_ & 0xff;
tanabe2000 2:f34a22b28ac5 88 upBitY = (Y_ >> 8) & 0xff;
ec30109b 8:8ea251946b2a 89 downBitY = Y_ & 0xff;
ec30109b 8:8ea251946b2a 90 txdata[0] = upBitX;
ec30109b 8:8ea251946b2a 91 txdata[1] = downBitX;
ec30109b 8:8ea251946b2a 92 txdata[2] = upBitY;
ec30109b 8:8ea251946b2a 93 txdata[3] = downBitY;
ec30109b 8:8ea251946b2a 94 txdata[4] = r1370.upbit();
ec30109b 8:8ea251946b2a 95 txdata[5] = r1370.downbit();
ec30109b 8:8ea251946b2a 96 serial.sendData(txdata, BUFFER_SIZE);
tanabe2000 2:f34a22b28ac5 97 }
tanabe2000 2:f34a22b28ac5 98
tanabe2000 3:30045028d27e 99 void MeasuringWheel::threadloop()
tanabe2000 3:30045028d27e 100 {
ec30109b 8:8ea251946b2a 101 while(true) {
tanabe2000 3:30045028d27e 102 computeXY();
tanabe2000 3:30045028d27e 103 }
ec30109b 8:8ea251946b2a 104 }
tanabe2000 3:30045028d27e 105
ec30109b 8:8ea251946b2a 106 double MeasuringWheel::getjyroAngle()
tanabe2000 2:f34a22b28ac5 107 {
tanabe2000 3:30045028d27e 108 return yawdegree;
ec30109b 8:8ea251946b2a 109 //return phi;
tanabe2000 0:505dd5510add 110 }
tanabe2000 0:505dd5510add 111
ec30109b 8:8ea251946b2a 112 double MeasuringWheel::getOutX()
tanabe2000 0:505dd5510add 113 {
ec30109b 8:8ea251946b2a 114 return minusX;
tanabe2000 0:505dd5510add 115 }
tanabe2000 0:505dd5510add 116
ec30109b 8:8ea251946b2a 117 double MeasuringWheel::getOutY()
tanabe2000 0:505dd5510add 118 {
ec30109b 8:8ea251946b2a 119 return minusY;
tanabe2000 0:505dd5510add 120 }
tanabe2000 0:505dd5510add 121
ec30109b 8:8ea251946b2a 122 double MeasuringWheel::getWheel1()
tanabe2000 2:f34a22b28ac5 123 {
tanabe2000 2:f34a22b28ac5 124 return w1.getPulses();
tanabe2000 2:f34a22b28ac5 125 }
tanabe2000 2:f34a22b28ac5 126
ec30109b 8:8ea251946b2a 127 double MeasuringWheel::getWheel2()
tanabe2000 0:505dd5510add 128 {
tanabe2000 2:f34a22b28ac5 129 return w2.getPulses();
tanabe2000 2:f34a22b28ac5 130 }
tanabe2000 2:f34a22b28ac5 131
ec30109b 8:8ea251946b2a 132 double MeasuringWheel::getWheel3()
tanabe2000 2:f34a22b28ac5 133 {
tanabe2000 2:f34a22b28ac5 134 return w3.getPulses();
tanabe2000 1:3f01bf4d7e56 135 }
tanabe2000 0:505dd5510add 136
tanabe2000 3:30045028d27e 137
tanabe2000 1:3f01bf4d7e56 138
tanabe2000 3:30045028d27e 139 void MeasuringWheel::resetposition()
tanabe2000 2:f34a22b28ac5 140 {
tanabe2000 2:f34a22b28ac5 141 if(get == 'R') {
tanabe2000 2:f34a22b28ac5 142 X = 0;
tanabe2000 2:f34a22b28ac5 143 Y = 0;
tanabe2000 2:f34a22b28ac5 144 }
tanabe2000 2:f34a22b28ac5 145 }