main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "define.h" //ステータス用
00004 
00005 Ticker timer;
00006 Timer T;
00007 
00008 QEI Enc1(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00009 QEI Enc2(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00010 DigitalIn sw(p26);
00011 Serial Saber(p13,p14);
00012 Serial pc(USBTX,USBRX);
00013 
00014 int encount1 = 0, encount2 = 0;
00015 float  encount_rot = 0;
00016 float spd1 = 0;
00017 
00018 void timer_warikomi()
00019 {
00020     static float pre_sp1 = 0;
00021     float pull_ratio = 1.0;
00022     float multi = 4.0;
00023     float ppr = 1.0;
00024     static int cmd = 15;
00025     encount1 = Enc1.getPulses();
00026     encount2 = Enc2.getPulses();
00027     if (encount1 > encount2) encount_rot = encount1;
00028     else encount_rot = encount2;
00029     
00030     float rot_sp1 = encount_rot/multi/ppr*pull_ratio; 
00031     spd1 = (rot_sp1 - pre_sp1)/INT_TIME/(48*4);
00032         
00033         if(sw == ON) cmd = 0;
00034         
00035         Saber.putc(SB_ADRS);
00036         Saber.putc(1);
00037         Saber.putc(cmd);
00038         Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111);
00039         
00040         Saber.putc(SB_ADRS);
00041         Saber.putc(4);
00042         Saber.putc(cmd);
00043         Saber.putc((SB_ADRS + 4 + cmd) & 0b01111111);
00044         pre_sp1 = rot_sp1;
00045 }
00046 
00047 int main() {
00048     Saber.baud(115200);
00049     pc.baud(9600);
00050     timer.attach(timer_warikomi,INT_TIME);
00051     
00052     while(1) {
00053     pc.printf("%d\t%d\t%f\t%f\n", encount1 , encount2, encount_rot, spd1);
00054     }
00055 }