まだ使えません
Dependencies: QEI OmniPosition
measuring_wheel.cpp
- Committer:
- tanabe2000
- Date:
- 2018-07-04
- Revision:
- 1:3f01bf4d7e56
- Parent:
- 0:505dd5510add
- Child:
- 2:f34a22b28ac5
File content as of revision 1:3f01bf4d7e56:
#include "measuring_wheel.h" MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2): w1(channel1_1, channel1_2, NC, 500, QEI::X4_ENCODING), w2(channel2_1, channel2_2, NC, 500, QEI::X4_ENCODING), w3(channel3_1, channel3_2, NC, 500, QEI::X4_ENCODING), mainMicon(PC_10, PC_11, 115200), led(LED2) { radian[0] = 0; radian[1] = -2*PI/3; radian[2] = 2*PI/3; ticker.attach(callback(this, &MeasuringWheel::computeXY),0.01); ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1); // mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み } bool MeasuringWheel::wheelDiameter(float diameter1, float diameter2, float diameter3) { diameter[0] = diameter1; diameter[1] = diameter2; diameter[2] = diameter3; return 1; } void MeasuringWheel::computeXY() { wheel[0] = diameter[0]*PI/2000.0*w1.getPulses(); wheel[1] = diameter[1]*PI/2000.0*w2.getPulses(); wheel[2] = diameter[2]*PI/2000.0*w3.getPulses(); w1.reset(); w2.reset(); w3.reset(); r = (wheel[0] + wheel[1] + wheel[2])/3.0; wheel[0] -= r; wheel[1] -= r; wheel[2] -= r; for(int i = 0; i < 3; i++) { X += wheel[i]*cos(radian[i]); Y += wheel[i]*sin(radian[i]); } } float MeasuringWheel::getOutX() { return X; } float MeasuringWheel::getOutY() { return Y; } void MeasuringWheel::transmissionXY() { X_ = X + 32768; Y_ = Y + 32768; upBitX = (X_ >> 8) & 0xff; downBitX = X_ & 0xff; upBitY = (Y_ >> 8) & 0xff; downBitY = Y_ & 0xff; mainMicon.putc(72); mainMicon.putc(42); mainMicon.putc(upBitX); mainMicon.putc(downBitX); mainMicon.putc(upBitY); mainMicon.putc(downBitY); } //void MeasuringWheel::resetpisithon() //{ // get = mainMicon.getc(); // if(get != 'R') { // //X = 0; //// Y = 0; //// led = !led; // } // // //}