まだ使えません
Dependencies: QEI OmniPosition
Revision 2:f34a22b28ac5, committed 2018-07-31
- Comitter:
- tanabe2000
- Date:
- Tue Jul 31 08:30:51 2018 +0000
- Parent:
- 1:3f01bf4d7e56
- Commit message:
- ver2.1
Changed in this revision
diff -r 3f01bf4d7e56 -r f34a22b28ac5 OmniPosition.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/OmniPosition.lib Tue Jul 31 08:30:51 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/OmniPosition/#47676abdf529
diff -r 3f01bf4d7e56 -r f34a22b28ac5 QEI.lib --- a/QEI.lib Wed Jul 04 09:32:06 2018 +0000 +++ b/QEI.lib Tue Jul 31 08:30:51 2018 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#40523b009a03 +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca
diff -r 3f01bf4d7e56 -r f34a22b28ac5 measuring_wheel.cpp --- a/measuring_wheel.cpp Wed Jul 04 09:32:06 2018 +0000 +++ b/measuring_wheel.cpp Tue Jul 31 08:30:51 2018 +0000 @@ -5,15 +5,17 @@ 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), + r1370(PC_6, PC_7), led(LED2) { - radian[0] = 0; - radian[1] = -2*PI/3; - radian[2] = 2*PI/3; + radian[0] = 0.0; + radian[1] = -2.0*PI/3.0; + radian[2] = 2.0*PI/3.0; ticker.attach(callback(this, &MeasuringWheel::computeXY),0.01); - ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1); -// mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み +// ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1); + mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み +// angleTiker.attach(callback(this, &MeasuringWheel::rawAngle),0.1); } @@ -38,10 +40,43 @@ wheel[0] -= r; wheel[1] -= r; wheel[2] -= r; +// r1370.update(); + yaw = r1370.getAngle(); + yaw *= PI/180; for(int i = 0; i < 3; i++) { - X += wheel[i]*cos(radian[i]); - Y += wheel[i]*sin(radian[i]); + subX = wheel[i]*cos(radian[i]); + subY = wheel[i]*sin(radian[i]); } + X += subX*cos((float)yaw) - subY*sin((float)yaw); + Y += subX*sin((float)yaw) - subY*cos((float)yaw); + X+=subX; + Y+=subY; + + + 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::rawAngle() +//{ +// r1370.update(); +// yaw = r1370.getRate(); +//} + +float MeasuringWheel::getjyroAngle() +{ + return yaw; } float MeasuringWheel::getOutX() @@ -54,31 +89,37 @@ return Y; } -void MeasuringWheel::transmissionXY() +float MeasuringWheel::getWheel1() +{ + return w1.getPulses(); +} + +float MeasuringWheel::getWheel2() { - 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); + return w2.getPulses(); +} + +float MeasuringWheel::getWheel3() +{ + return w3.getPulses(); } -//void MeasuringWheel::resetpisithon() +//void MeasuringWheel::transmissionXY() //{ -// get = mainMicon.getc(); -// if(get != 'R') { -// //X = 0; -//// Y = 0; -//// led = !led; -// } -// -// //} +void MeasuringWheel::resetpisithon() +{ + get = mainMicon.getc(); + if(get == 'R') { + X = 0; + Y = 0; +// Cdif = yaw + 540 - r1370.getRate(); +// Cdif %= 360; +// Cdif -= 180; + led = !led; + } + + +} +
diff -r 3f01bf4d7e56 -r f34a22b28ac5 measuring_wheel.h --- a/measuring_wheel.h Wed Jul 04 09:32:06 2018 +0000 +++ b/measuring_wheel.h Tue Jul 31 08:30:51 2018 +0000 @@ -3,6 +3,8 @@ #include "mbed.h" #include "QEI.h" +//#include "r1307.h" +#include "OmniPosition.h" #define PI 3.141592653589793 class MeasuringWheel @@ -16,27 +18,33 @@ float getOutX(); float getOutY(); void transmissionXY(); - + float getWheel1(),getWheel2(),getWheel3(); + float getjyroAngle(); + OmniPosition r1370; private : float diameter[3]; float radian[3]; - float wheel[3],r,X,Y; + float wheel[3],r,X,Y,subX,subY; void computeXY(); + QEI w1; QEI w2; QEI w3; +// R1307 r1370; + float Cdif, yaw; Ticker ticker; - Ticker ticker1; uint16_t X_, Y_,ofsetX, ofsetY; char upBitX, upBitY; char downBitX,downBitY; Serial mainMicon; char get; DigitalOut led; -// void resetpisithon(); + void resetpisithon(); +// void rawAngle(); + };