Soma Takahashi / measuring_wheel

2019NHK_teamA_auto_measuring wheel

Dependencies:   QEI R1370MeasuringWheel

measuring_wheel.cpp@7:e74a1688680d, 2018-09-18 (annotated)

Committer:
tanabe2000
Date:
Tue Sep 18 15:22:01 2018 +0000
Revision:
7:e74a1688680d
Parent:
3:30045028d27e 
?????????????

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):
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 7:e74a1688680d 7 serial(measuringTX, measuringRX),
tanabe2000 7:e74a1688680d 8 r1370(R1370TX, R1370RX)
tanabe2000 0:505dd5510add 9 {
tanabe2000 7:e74a1688680d 10 serial.baud(115200);
tanabe2000 7:e74a1688680d 11 serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
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 3:30045028d27e 15 thread.start(callback(this, &MeasuringWheel::threadloop));
tanabe2000 2:f34a22b28ac5 16 // ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
tanabe2000 3:30045028d27e 17 //mainMicon.attach(callback(this, &MeasuringWheel::resetposition));//受信割り込み
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 7:e74a1688680d 32 subY = 0;
tanabe2000 7:e74a1688680d 33 subX = 0;
tanabe2000 7:e74a1688680d 34 wheel[0] = diameter[0]*PI/400.0*w1.getPulses();
tanabe2000 7:e74a1688680d 35 wheel[1] = diameter[1]*PI/400.0*w2.getPulses();
tanabe2000 7:e74a1688680d 36 wheel[2] = diameter[2]*PI/400.0*w3.getPulses();
tanabe2000 0:505dd5510add 37 w1.reset();
tanabe2000 0:505dd5510add 38 w2.reset();
tanabe2000 0:505dd5510add 39 w3.reset();
tanabe2000 0:505dd5510add 40 r = (wheel[0] + wheel[1] + wheel[2])/3.0;
tanabe2000 0:505dd5510add 41 wheel[0] -= r;
tanabe2000 0:505dd5510add 42 wheel[1] -= r;
tanabe2000 0:505dd5510add 43 wheel[2] -= r;
tanabe2000 3:30045028d27e 44 yawdegree = r1370.getAngle();
tanabe2000 7:e74a1688680d 45 if((yawdegree - beforeYaw) > 350)yawMode--;
tanabe2000 7:e74a1688680d 46 else if((yawdegree - beforeYaw) < -350)yawMode++;
tanabe2000 7:e74a1688680d 47 beforeYaw = yawdegree;
tanabe2000 7:e74a1688680d 48 nowYaw = 360*yawMode + yawdegree;
tanabe2000 7:e74a1688680d 49 yaw = -1.0*nowYaw;
tanabe2000 2:f34a22b28ac5 50 yaw *= PI/180;
tanabe2000 0:505dd5510add 51 for(int i = 0; i < 3; i++) {
tanabe2000 7:e74a1688680d 52 subY += wheel[i]*sin(radian[i]);
tanabe2000 3:30045028d27e 53 subX += wheel[i]*cos(radian[i]);
tanabe2000 0:505dd5510add 54 }
tanabe2000 2:f34a22b28ac5 55 X += subX*cos((float)yaw) - subY*sin((float)yaw);
tanabe2000 3:30045028d27e 56 Y += subX*sin((float)yaw) + subY*cos((float)yaw);
tanabe2000 2:f34a22b28ac5 57
tanabe2000 2:f34a22b28ac5 58 X_ = X + 32768;
tanabe2000 2:f34a22b28ac5 59 Y_ = Y + 32768;
tanabe2000 2:f34a22b28ac5 60 upBitX = (X_ >> 8) & 0xff;
tanabe2000 2:f34a22b28ac5 61 downBitX = X_ & 0xff;
tanabe2000 2:f34a22b28ac5 62 upBitY = (Y_ >> 8) & 0xff;
tanabe2000 7:e74a1688680d 63 downBitY = Y_ & 0xff;
tanabe2000 7:e74a1688680d 64 txdata[0] = upBitX;
tanabe2000 7:e74a1688680d 65 txdata[1] = downBitX;
tanabe2000 7:e74a1688680d 66 txdata[2] = upBitY;
tanabe2000 7:e74a1688680d 67 txdata[3] = downBitY;
tanabe2000 7:e74a1688680d 68 txdata[4] = r1370.upbit();
tanabe2000 7:e74a1688680d 69 txdata[5] = r1370.downbit();
tanabe2000 7:e74a1688680d 70 serial.sendData(txdata, BUFFER_SIZE);
tanabe2000 2:f34a22b28ac5 71
tanabe2000 2:f34a22b28ac5 72 }
tanabe2000 2:f34a22b28ac5 73
tanabe2000 3:30045028d27e 74 void MeasuringWheel::threadloop()
tanabe2000 3:30045028d27e 75 {
tanabe2000 7:e74a1688680d 76 while(true) {
tanabe2000 3:30045028d27e 77 computeXY();
tanabe2000 3:30045028d27e 78 }
tanabe2000 7:e74a1688680d 79 }
tanabe2000 2:f34a22b28ac5 80
tanabe2000 2:f34a22b28ac5 81 float MeasuringWheel::getjyroAngle()
tanabe2000 2:f34a22b28ac5 82 {
tanabe2000 3:30045028d27e 83 return yawdegree;
tanabe2000 7:e74a1688680d 84 // return nowYaw;
tanabe2000 0:505dd5510add 85 }
tanabe2000 0:505dd5510add 86
tanabe2000 0:505dd5510add 87 float MeasuringWheel::getOutX()
tanabe2000 0:505dd5510add 88 {
tanabe2000 0:505dd5510add 89 return X;
tanabe2000 0:505dd5510add 90 }
tanabe2000 0:505dd5510add 91
tanabe2000 0:505dd5510add 92 float MeasuringWheel::getOutY()
tanabe2000 0:505dd5510add 93 {
tanabe2000 0:505dd5510add 94 return Y;
tanabe2000 0:505dd5510add 95 }
tanabe2000 0:505dd5510add 96
tanabe2000 2:f34a22b28ac5 97 float MeasuringWheel::getWheel1()
tanabe2000 2:f34a22b28ac5 98 {
tanabe2000 2:f34a22b28ac5 99 return w1.getPulses();
tanabe2000 2:f34a22b28ac5 100 }
tanabe2000 2:f34a22b28ac5 101
tanabe2000 2:f34a22b28ac5 102 float MeasuringWheel::getWheel2()
tanabe2000 0:505dd5510add 103 {
tanabe2000 2:f34a22b28ac5 104 return w2.getPulses();
tanabe2000 2:f34a22b28ac5 105 }
tanabe2000 2:f34a22b28ac5 106
tanabe2000 2:f34a22b28ac5 107 float MeasuringWheel::getWheel3()
tanabe2000 2:f34a22b28ac5 108 {
tanabe2000 2:f34a22b28ac5 109 return w3.getPulses();
tanabe2000 1:3f01bf4d7e56 110 }
tanabe2000 0:505dd5510add 111
tanabe2000 3:30045028d27e 112
tanabe2000 1:3f01bf4d7e56 113
tanabe2000 3:30045028d27e 114 void MeasuringWheel::resetposition()
tanabe2000 2:f34a22b28ac5 115 {
tanabe2000 2:f34a22b28ac5 116 if(get == 'R') {
tanabe2000 2:f34a22b28ac5 117 X = 0;
tanabe2000 2:f34a22b28ac5 118 Y = 0;
tanabe2000 2:f34a22b28ac5 119 }
tanabe2000 2:f34a22b28ac5 120
tanabe2000 2:f34a22b28ac5 121
tanabe2000 2:f34a22b28ac5 122 }
tanabe2000 2:f34a22b28ac5 123