Soma Takahashi / measuring_wheel

2019NHK_teamA_auto_measuring wheel

Dependencies:   QEI R1370MeasuringWheel

measuring_wheel.cpp@3:30045028d27e, 2018-08-27 (annotated)

Committer:
tanabe2000
Date:
Mon Aug 27 04:45:39 2018 +0000
Revision:
3:30045028d27e
Parent:
2:f34a22b28ac5
Child:
7:e74a1688680d
Child:
8:8ea251946b2a
ver2

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 3:30045028d27e 7 mainMicon(PC_12,PD_2, 115200),
tanabe2000 3:30045028d27e 8 r1370(PC_10, PC_11),
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 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 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 3:30045028d27e 44 yawdegree = r1370.getAngle();
tanabe2000 3:30045028d27e 45 yaw = yawdegree;
tanabe2000 2:f34a22b28ac5 46 yaw *= PI/180;
tanabe2000 3:30045028d27e 47 // yaw *= PI/180;
tanabe2000 0:505dd5510add 48 for(int i = 0; i < 3; i++) {
tanabe2000 3:30045028d27e 49 subX += wheel[i]*cos(radian[i]);
tanabe2000 3:30045028d27e 50 subY += wheel[i]*sin(radian[i]);
tanabe2000 0:505dd5510add 51 }
tanabe2000 2:f34a22b28ac5 52 X += subX*cos((float)yaw) - subY*sin((float)yaw);
tanabe2000 3:30045028d27e 53 Y += subX*sin((float)yaw) + subY*cos((float)yaw);
tanabe2000 2:f34a22b28ac5 54 X+=subX;
tanabe2000 2:f34a22b28ac5 55 Y+=subY;
tanabe2000 2:f34a22b28ac5 56
tanabe2000 2:f34a22b28ac5 57 X_ = X + 32768;
tanabe2000 2:f34a22b28ac5 58 Y_ = Y + 32768;
tanabe2000 2:f34a22b28ac5 59 upBitX = (X_ >> 8) & 0xff;
tanabe2000 2:f34a22b28ac5 60 downBitX = X_ & 0xff;
tanabe2000 2:f34a22b28ac5 61 upBitY = (Y_ >> 8) & 0xff;
tanabe2000 3:30045028d27e 62 downBitY = Y_ & 0xff;
tanabe2000 3:30045028d27e 63
tanabe2000 2:f34a22b28ac5 64 mainMicon.putc(72);
tanabe2000 2:f34a22b28ac5 65 mainMicon.putc(42);
tanabe2000 2:f34a22b28ac5 66 mainMicon.putc(upBitX);
tanabe2000 2:f34a22b28ac5 67 mainMicon.putc(downBitX);
tanabe2000 2:f34a22b28ac5 68 mainMicon.putc(upBitY);
tanabe2000 2:f34a22b28ac5 69 mainMicon.putc(downBitY);
tanabe2000 3:30045028d27e 70 mainMicon.putc(r1370.upbit());
tanabe2000 3:30045028d27e 71 mainMicon.putc(r1370.downbit());
tanabe2000 3:30045028d27e 72 mainMicon.putc((unsigned char)(upBitX + downBitY + upBitY + downBitY + r1370.upbit() + r1370.downbit()));
tanabe2000 2:f34a22b28ac5 73
tanabe2000 2:f34a22b28ac5 74 }
tanabe2000 2:f34a22b28ac5 75
tanabe2000 3:30045028d27e 76 void MeasuringWheel::threadloop()
tanabe2000 3:30045028d27e 77 {
tanabe2000 3:30045028d27e 78 while(true){
tanabe2000 3:30045028d27e 79 computeXY();
tanabe2000 3:30045028d27e 80 }
tanabe2000 3:30045028d27e 81 }
tanabe2000 3:30045028d27e 82
tanabe2000 2:f34a22b28ac5 83 //void MeasuringWheel::rawAngle()
tanabe2000 2:f34a22b28ac5 84 //{
tanabe2000 2:f34a22b28ac5 85 // r1370.update();
tanabe2000 2:f34a22b28ac5 86 // yaw = r1370.getRate();
tanabe2000 2:f34a22b28ac5 87 //}
tanabe2000 2:f34a22b28ac5 88
tanabe2000 2:f34a22b28ac5 89 float MeasuringWheel::getjyroAngle()
tanabe2000 2:f34a22b28ac5 90 {
tanabe2000 3:30045028d27e 91
tanabe2000 3:30045028d27e 92 return yawdegree;
tanabe2000 0:505dd5510add 93 }
tanabe2000 0:505dd5510add 94
tanabe2000 0:505dd5510add 95 float MeasuringWheel::getOutX()
tanabe2000 0:505dd5510add 96 {
tanabe2000 0:505dd5510add 97 return X;
tanabe2000 0:505dd5510add 98 }
tanabe2000 0:505dd5510add 99
tanabe2000 0:505dd5510add 100 float MeasuringWheel::getOutY()
tanabe2000 0:505dd5510add 101 {
tanabe2000 0:505dd5510add 102 return Y;
tanabe2000 0:505dd5510add 103 }
tanabe2000 0:505dd5510add 104
tanabe2000 2:f34a22b28ac5 105 float MeasuringWheel::getWheel1()
tanabe2000 2:f34a22b28ac5 106 {
tanabe2000 2:f34a22b28ac5 107 return w1.getPulses();
tanabe2000 2:f34a22b28ac5 108 }
tanabe2000 2:f34a22b28ac5 109
tanabe2000 2:f34a22b28ac5 110 float MeasuringWheel::getWheel2()
tanabe2000 0:505dd5510add 111 {
tanabe2000 2:f34a22b28ac5 112 return w2.getPulses();
tanabe2000 2:f34a22b28ac5 113 }
tanabe2000 2:f34a22b28ac5 114
tanabe2000 2:f34a22b28ac5 115 float MeasuringWheel::getWheel3()
tanabe2000 2:f34a22b28ac5 116 {
tanabe2000 2:f34a22b28ac5 117 return w3.getPulses();
tanabe2000 1:3f01bf4d7e56 118 }
tanabe2000 0:505dd5510add 119
tanabe2000 3:30045028d27e 120
tanabe2000 1:3f01bf4d7e56 121
tanabe2000 3:30045028d27e 122 void MeasuringWheel::resetposition()
tanabe2000 2:f34a22b28ac5 123 {
tanabe2000 2:f34a22b28ac5 124 get = mainMicon.getc();
tanabe2000 2:f34a22b28ac5 125 if(get == 'R') {
tanabe2000 2:f34a22b28ac5 126 X = 0;
tanabe2000 2:f34a22b28ac5 127 Y = 0;
tanabe2000 2:f34a22b28ac5 128 led = !led;
tanabe2000 2:f34a22b28ac5 129 }
tanabe2000 2:f34a22b28ac5 130
tanabe2000 2:f34a22b28ac5 131
tanabe2000 2:f34a22b28ac5 132 }
tanabe2000 2:f34a22b28ac5 133