まだ使えません
Dependencies: QEI OmniPosition
measuring_wheel.cpp
- Committer:
- tanabe2000
- Date:
- 2018-07-02
- Revision:
- 0:505dd5510add
- Child:
- 1:3f01bf4d7e56
File content as of revision 0:505dd5510add:
#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) { radian[0] = 0; radian[1] = -2*PI/3; radian[2] = 2*PI/3; ticker.attach(callback(this, &MeasuringWheel::computeXY),0.001); } 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; } TransmissionXY::TransmissionXY(PinName serialTX,PinName serialRX): mainMicon(serialTX, serialRX, 115200), led(LED2) { // mainMicon.attach(callback(this, &TransmissionXY::resetpisithon)); } void TransmissionXY::resetpisithon() { get = mainMicon.getc(); if(get == 'R') { ofsetX = X_; ofsetY = Y_; led = !led; } X_ -= ofsetX; Y_ -= ofsetY; } bool TransmissionXY::sendMeasuring(MeasuringWheel &request) { X_ = request.getOutX()+32768; Y_ = request.getOutY()+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); return 1; }