taka yamanouchi
/
mbed_test_enc
shashutu_test
Diff: main.cpp
- Revision:
- 0:753587765f3d
diff -r 000000000000 -r 753587765f3d main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Dec 27 01:48:59 2018 +0000 @@ -0,0 +1,55 @@ +#include "mbed.h" +#include "QEI.h" +#include "define.h" //ステータス用 + +Ticker timer; +Timer T; + +QEI Enc1(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING); +QEI Enc2(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING); +DigitalIn sw(p26); +Serial Saber(p13,p14); +Serial pc(USBTX,USBRX); + +int encount1 = 0, encount2 = 0; +float encount_rot = 0; +float spd1 = 0; + +void timer_warikomi() +{ + static float pre_sp1 = 0; + float pull_ratio = 1.0; + float multi = 4.0; + float ppr = 1.0; + static int cmd = 15; + encount1 = Enc1.getPulses(); + encount2 = Enc2.getPulses(); + if (encount1 > encount2) encount_rot = encount1; + else encount_rot = encount2; + + float rot_sp1 = encount_rot/multi/ppr*pull_ratio; + spd1 = (rot_sp1 - pre_sp1)/INT_TIME/(48*4); + + if(sw == ON) cmd = 0; + + Saber.putc(SB_ADRS); + Saber.putc(1); + Saber.putc(cmd); + Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111); + + Saber.putc(SB_ADRS); + Saber.putc(4); + Saber.putc(cmd); + Saber.putc((SB_ADRS + 4 + cmd) & 0b01111111); + pre_sp1 = rot_sp1; +} + +int main() { + Saber.baud(115200); + pc.baud(9600); + timer.attach(timer_warikomi,INT_TIME); + + while(1) { + pc.printf("%d\t%d\t%f\t%f\n", encount1 , encount2, encount_rot, spd1); + } +}