naoki tanabe / measuring_wheel

まだ使えません

Dependencies:   QEI OmniPosition

Dependents:  

measuring_wheel.cpp@0:505dd5510add, 2018-07-02 (annotated)

Committer:
tanabe2000
Date:
Mon Jul 02 08:38:07 2018 +0000
Revision:
0:505dd5510add
Child:
1:3f01bf4d7e56
measuring_v1

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