Kouki Suzuki / Mbed OS NHK2019_mae_v6

2019/09/13ver

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen

lib/measuring_wheel/measuring_wheel.cpp@0:76663617eca3, 2019-09-13 (annotated)

Committer:
skouki
Date:
Fri Sep 13 02:15:30 2019 +0000
Revision:
0:76663617eca3
2019/09/13

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skouki 0:76663617eca3 1 #include "measuring_wheel.h"
skouki 0:76663617eca3 2
skouki 0:76663617eca3 3 MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX):
skouki 0:76663617eca3 4 w1(channel1_1, channel1_2, NC, PPR, QEI::X4_ENCODING),
skouki 0:76663617eca3 5 w2(channel2_1, channel2_2, NC, PPR, QEI::X4_ENCODING),
skouki 0:76663617eca3 6 w3(channel3_1, channel3_2, NC, PPR, QEI::X4_ENCODING),
skouki 0:76663617eca3 7 r1370(R1370TX, R1370RX)
skouki 0:76663617eca3 8 {
skouki 0:76663617eca3 9 miniX = 0;
skouki 0:76663617eca3 10 miniY = 0;
skouki 0:76663617eca3 11 X = 0;
skouki 0:76663617eca3 12 Y = 0;
skouki 0:76663617eca3 13 loopCounter = 0;
skouki 0:76663617eca3 14
skouki 0:76663617eca3 15 /*けいそくりんの設定*/
skouki 0:76663617eca3 16 //ホイールの向き
skouki 0:76663617eca3 17 radian[0] = -1 * PIII / 2.0;
skouki 0:76663617eca3 18 radian[1] = PIII / 6.0;
skouki 0:76663617eca3 19 radian[2] = 5.0 * PIII / 6.0;
skouki 0:76663617eca3 20 //直径
skouki 0:76663617eca3 21 diameter[0] = 49.0;
skouki 0:76663617eca3 22 diameter[1] = 49.0;
skouki 0:76663617eca3 23 diameter[2] = 49.0;
skouki 0:76663617eca3 24 //中心からの距離
skouki 0:76663617eca3 25 distance[0] = 124.2;
skouki 0:76663617eca3 26 distance[1] = 124.2;
skouki 0:76663617eca3 27 distance[2] = 124.2;
skouki 0:76663617eca3 28
skouki 0:76663617eca3 29 coefficient(0) = cos(radian[0]);
skouki 0:76663617eca3 30 coefficient(1) = cos(radian[1]);
skouki 0:76663617eca3 31 coefficient(2) = cos(radian[2]);
skouki 0:76663617eca3 32 coefficient(3) = sin(radian[0]);
skouki 0:76663617eca3 33 coefficient(4) = sin(radian[1]);
skouki 0:76663617eca3 34 coefficient(5) = sin(radian[2]);
skouki 0:76663617eca3 35 coefficient(6) = distance[0];
skouki 0:76663617eca3 36 coefficient(7) = distance[1];
skouki 0:76663617eca3 37 coefficient(8) = distance[2];
skouki 0:76663617eca3 38
skouki 0:76663617eca3 39 dec = coefficient.fullPivHouseholderQr();
skouki 0:76663617eca3 40
skouki 0:76663617eca3 41 thread.start(callback(this, &MeasuringWheel::threadloop));
skouki 0:76663617eca3 42 }
skouki 0:76663617eca3 43
skouki 0:76663617eca3 44 void MeasuringWheel::computeXY()
skouki 0:76663617eca3 45 {
skouki 0:76663617eca3 46 subY = 0;
skouki 0:76663617eca3 47 subX = 0;
skouki 0:76663617eca3 48 wheel(0) = -diameter[0]*PIII/(PPR*4.0)*w1.getPulses();
skouki 0:76663617eca3 49 wheel(1) = -diameter[1]*PIII/(PPR*4.0)*w2.getPulses();
skouki 0:76663617eca3 50 wheel(2) = -diameter[2]*PIII/(PPR*4.0)*w3.getPulses();
skouki 0:76663617eca3 51 w1.reset();
skouki 0:76663617eca3 52 w2.reset();
skouki 0:76663617eca3 53 w3.reset();
skouki 0:76663617eca3 54 yawdegree = r1370.getAngle();
skouki 0:76663617eca3 55 yaw = -1.0*yawdegree;
skouki 0:76663617eca3 56 yaw *= PIII/180.0;
skouki 0:76663617eca3 57
skouki 0:76663617eca3 58 ans = dec.solve(wheel);
skouki 0:76663617eca3 59
skouki 0:76663617eca3 60 subX = ans(0);
skouki 0:76663617eca3 61 subY = ans(1);
skouki 0:76663617eca3 62
skouki 0:76663617eca3 63 loopCounter++;
skouki 0:76663617eca3 64 if(loopCounter > 1000000){
skouki 0:76663617eca3 65 loopCounter = 0;
skouki 0:76663617eca3 66 X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
skouki 0:76663617eca3 67 Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
skouki 0:76663617eca3 68 miniX = 0;
skouki 0:76663617eca3 69 miniY = 0;
skouki 0:76663617eca3 70 }else{
skouki 0:76663617eca3 71 miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
skouki 0:76663617eca3 72 miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
skouki 0:76663617eca3 73 }
skouki 0:76663617eca3 74
skouki 0:76663617eca3 75 minusX = (X + miniX) * -1;
skouki 0:76663617eca3 76 minusY = (Y + miniY) * -1;
skouki 0:76663617eca3 77 }
skouki 0:76663617eca3 78
skouki 0:76663617eca3 79 void MeasuringWheel::threadloop()
skouki 0:76663617eca3 80 {
skouki 0:76663617eca3 81 while(true) {
skouki 0:76663617eca3 82 computeXY();
skouki 0:76663617eca3 83 }
skouki 0:76663617eca3 84 }
skouki 0:76663617eca3 85
skouki 0:76663617eca3 86 double MeasuringWheel::getjyroAngle()
skouki 0:76663617eca3 87 {
skouki 0:76663617eca3 88 return r1370.getAngle();
skouki 0:76663617eca3 89 //return phi;
skouki 0:76663617eca3 90 }
skouki 0:76663617eca3 91
skouki 0:76663617eca3 92 double MeasuringWheel::getOutX()
skouki 0:76663617eca3 93 {
skouki 0:76663617eca3 94 return minusX;
skouki 0:76663617eca3 95 }
skouki 0:76663617eca3 96
skouki 0:76663617eca3 97 double MeasuringWheel::getOutY()
skouki 0:76663617eca3 98 {
skouki 0:76663617eca3 99 return -1*minusY;
skouki 0:76663617eca3 100 }
skouki 0:76663617eca3 101
skouki 0:76663617eca3 102 double MeasuringWheel::getWheel1()
skouki 0:76663617eca3 103 {
skouki 0:76663617eca3 104 return w1.getPulses();
skouki 0:76663617eca3 105 }
skouki 0:76663617eca3 106
skouki 0:76663617eca3 107 double MeasuringWheel::getWheel2()
skouki 0:76663617eca3 108 {
skouki 0:76663617eca3 109 return w2.getPulses();
skouki 0:76663617eca3 110 }
skouki 0:76663617eca3 111
skouki 0:76663617eca3 112 double MeasuringWheel::getWheel3()
skouki 0:76663617eca3 113 {
skouki 0:76663617eca3 114 return w3.getPulses();
skouki 0:76663617eca3 115 }
skouki 0:76663617eca3 116
skouki 0:76663617eca3 117
skouki 0:76663617eca3 118
skouki 0:76663617eca3 119 void MeasuringWheel::resetposition()
skouki 0:76663617eca3 120 {
skouki 0:76663617eca3 121 X = 0;
skouki 0:76663617eca3 122 Y = 0;
skouki 0:76663617eca3 123 }