naoki tanabe / measuring_wheel

まだ使えません

Dependencies:   QEI OmniPosition

Dependents:  

measuring_wheel.cpp@1:3f01bf4d7e56, 2018-07-04 (annotated)

Committer:
tanabe2000
Date:
Wed Jul 04 09:32:06 2018 +0000
Revision:
1:3f01bf4d7e56
Parent:
0:505dd5510add
Child:
2:f34a22b28ac5
test

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 1:3f01bf4d7e56 8 led(LED2)
tanabe2000 0:505dd5510add 9
tanabe2000 0:505dd5510add 10 {
tanabe2000 0:505dd5510add 11 radian[0] = 0;
tanabe2000 0:505dd5510add 12 radian[1] = -2*PI/3;
tanabe2000 0:505dd5510add 13 radian[2] = 2*PI/3;
tanabe2000 1:3f01bf4d7e56 14 ticker.attach(callback(this, &MeasuringWheel::computeXY),0.01);
tanabe2000 1:3f01bf4d7e56 15 ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
tanabe2000 1:3f01bf4d7e56 16 // mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み
tanabe2000 0:505dd5510add 17 }
tanabe2000 0:505dd5510add 18
tanabe2000 0:505dd5510add 19
tanabe2000 0:505dd5510add 20 bool MeasuringWheel::wheelDiameter(float diameter1, float diameter2, float diameter3)
tanabe2000 0:505dd5510add 21 {
tanabe2000 0:505dd5510add 22 diameter[0] = diameter1;
tanabe2000 0:505dd5510add 23 diameter[1] = diameter2;
tanabe2000 0:505dd5510add 24 diameter[2] = diameter3;
tanabe2000 0:505dd5510add 25 return 1;
tanabe2000 0:505dd5510add 26 }
tanabe2000 0:505dd5510add 27
tanabe2000 0:505dd5510add 28 void MeasuringWheel::computeXY()
tanabe2000 0:505dd5510add 29 {
tanabe2000 0:505dd5510add 30
tanabe2000 0:505dd5510add 31 wheel[0] = diameter[0]*PI/2000.0*w1.getPulses();
tanabe2000 0:505dd5510add 32 wheel[1] = diameter[1]*PI/2000.0*w2.getPulses();
tanabe2000 0:505dd5510add 33 wheel[2] = diameter[2]*PI/2000.0*w3.getPulses();
tanabe2000 0:505dd5510add 34 w1.reset();
tanabe2000 0:505dd5510add 35 w2.reset();
tanabe2000 0:505dd5510add 36 w3.reset();
tanabe2000 0:505dd5510add 37 r = (wheel[0] + wheel[1] + wheel[2])/3.0;
tanabe2000 0:505dd5510add 38 wheel[0] -= r;
tanabe2000 0:505dd5510add 39 wheel[1] -= r;
tanabe2000 0:505dd5510add 40 wheel[2] -= r;
tanabe2000 0:505dd5510add 41 for(int i = 0; i < 3; i++) {
tanabe2000 0:505dd5510add 42 X += wheel[i]*cos(radian[i]);
tanabe2000 0:505dd5510add 43 Y += wheel[i]*sin(radian[i]);
tanabe2000 0:505dd5510add 44 }
tanabe2000 0:505dd5510add 45 }
tanabe2000 0:505dd5510add 46
tanabe2000 0:505dd5510add 47 float MeasuringWheel::getOutX()
tanabe2000 0:505dd5510add 48 {
tanabe2000 0:505dd5510add 49 return X;
tanabe2000 0:505dd5510add 50 }
tanabe2000 0:505dd5510add 51
tanabe2000 0:505dd5510add 52 float MeasuringWheel::getOutY()
tanabe2000 0:505dd5510add 53 {
tanabe2000 0:505dd5510add 54 return Y;
tanabe2000 0:505dd5510add 55 }
tanabe2000 0:505dd5510add 56
tanabe2000 1:3f01bf4d7e56 57 void MeasuringWheel::transmissionXY()
tanabe2000 0:505dd5510add 58 {
tanabe2000 1:3f01bf4d7e56 59 X_ = X + 32768;
tanabe2000 1:3f01bf4d7e56 60 Y_ = Y + 32768;
tanabe2000 0:505dd5510add 61 upBitX = (X_ >> 8) & 0xff;
tanabe2000 0:505dd5510add 62 downBitX = X_ & 0xff;
tanabe2000 0:505dd5510add 63 upBitY = (Y_ >> 8) & 0xff;
tanabe2000 0:505dd5510add 64 downBitY = Y_ & 0xff;
tanabe2000 0:505dd5510add 65 mainMicon.putc(72);
tanabe2000 0:505dd5510add 66 mainMicon.putc(42);
tanabe2000 0:505dd5510add 67 mainMicon.putc(upBitX);
tanabe2000 0:505dd5510add 68 mainMicon.putc(downBitX);
tanabe2000 0:505dd5510add 69 mainMicon.putc(upBitY);
tanabe2000 0:505dd5510add 70 mainMicon.putc(downBitY);
tanabe2000 1:3f01bf4d7e56 71 }
tanabe2000 0:505dd5510add 72
tanabe2000 1:3f01bf4d7e56 73 //void MeasuringWheel::resetpisithon()
tanabe2000 1:3f01bf4d7e56 74 //{
tanabe2000 1:3f01bf4d7e56 75 // get = mainMicon.getc();
tanabe2000 1:3f01bf4d7e56 76 // if(get != 'R') {
tanabe2000 1:3f01bf4d7e56 77 // //X = 0;
tanabe2000 1:3f01bf4d7e56 78 //// Y = 0;
tanabe2000 1:3f01bf4d7e56 79 //// led = !led;
tanabe2000 1:3f01bf4d7e56 80 // }
tanabe2000 1:3f01bf4d7e56 81 //
tanabe2000 1:3f01bf4d7e56 82 //
tanabe2000 1:3f01bf4d7e56 83 //}
tanabe2000 1:3f01bf4d7e56 84