taka yamanouchi / Mbed 2 deprecated mbed_test_enc

shashutu_test

Dependencies:   mbed QEI2

main.cpp@0:753587765f3d, 2018-12-27 (annotated)

Committer:
sink
Date:
Thu Dec 27 01:48:59 2018 +0000
Revision:
0:753587765f3d
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sink 0:753587765f3d 1 #include "mbed.h"
sink 0:753587765f3d 2 #include "QEI.h"
sink 0:753587765f3d 3 #include "define.h" //ステータス用
sink 0:753587765f3d 4
sink 0:753587765f3d 5 Ticker timer;
sink 0:753587765f3d 6 Timer T;
sink 0:753587765f3d 7
sink 0:753587765f3d 8 QEI Enc1(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
sink 0:753587765f3d 9 QEI Enc2(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
sink 0:753587765f3d 10 DigitalIn sw(p26);
sink 0:753587765f3d 11 Serial Saber(p13,p14);
sink 0:753587765f3d 12 Serial pc(USBTX,USBRX);
sink 0:753587765f3d 13
sink 0:753587765f3d 14 int encount1 = 0, encount2 = 0;
sink 0:753587765f3d 15 float encount_rot = 0;
sink 0:753587765f3d 16 float spd1 = 0;
sink 0:753587765f3d 17
sink 0:753587765f3d 18 void timer_warikomi()
sink 0:753587765f3d 19 {
sink 0:753587765f3d 20 static float pre_sp1 = 0;
sink 0:753587765f3d 21 float pull_ratio = 1.0;
sink 0:753587765f3d 22 float multi = 4.0;
sink 0:753587765f3d 23 float ppr = 1.0;
sink 0:753587765f3d 24 static int cmd = 15;
sink 0:753587765f3d 25 encount1 = Enc1.getPulses();
sink 0:753587765f3d 26 encount2 = Enc2.getPulses();
sink 0:753587765f3d 27 if (encount1 > encount2) encount_rot = encount1;
sink 0:753587765f3d 28 else encount_rot = encount2;
sink 0:753587765f3d 29
sink 0:753587765f3d 30 float rot_sp1 = encount_rot/multi/ppr*pull_ratio;
sink 0:753587765f3d 31 spd1 = (rot_sp1 - pre_sp1)/INT_TIME/(48*4);
sink 0:753587765f3d 32
sink 0:753587765f3d 33 if(sw == ON) cmd = 0;
sink 0:753587765f3d 34
sink 0:753587765f3d 35 Saber.putc(SB_ADRS);
sink 0:753587765f3d 36 Saber.putc(1);
sink 0:753587765f3d 37 Saber.putc(cmd);
sink 0:753587765f3d 38 Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111);
sink 0:753587765f3d 39
sink 0:753587765f3d 40 Saber.putc(SB_ADRS);
sink 0:753587765f3d 41 Saber.putc(4);
sink 0:753587765f3d 42 Saber.putc(cmd);
sink 0:753587765f3d 43 Saber.putc((SB_ADRS + 4 + cmd) & 0b01111111);
sink 0:753587765f3d 44 pre_sp1 = rot_sp1;
sink 0:753587765f3d 45 }
sink 0:753587765f3d 46
sink 0:753587765f3d 47 int main() {
sink 0:753587765f3d 48 Saber.baud(115200);
sink 0:753587765f3d 49 pc.baud(9600);
sink 0:753587765f3d 50 timer.attach(timer_warikomi,INT_TIME);
sink 0:753587765f3d 51
sink 0:753587765f3d 52 while(1) {
sink 0:753587765f3d 53 pc.printf("%d\t%d\t%f\t%f\n", encount1 , encount2, encount_rot, spd1);
sink 0:753587765f3d 54 }
sink 0:753587765f3d 55 }