naoki tanabe / measuring_wheel

まだ使えません

Dependencies:   QEI OmniPosition

Dependents:  

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?

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 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