main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "Filter.h"
00004 #include "RoboClaw.h"
00005 #define ADRS 129 // RoboClawのアドレス
00006 #define REF1 (48*4) // 目標回転数．一秒あたりの回転数をかけてご使用ください．
00007 #define REF2 (48*4) // 上記同様．
00008 #define INT_TIME 0.02
00009 
00010 Ticker timer;
00011 Timer T;
00012 
00013 DigitalIn sw(p29); // スイッチを使うピンに合わせてご使用ください．
00014 //DigitalIn sw2(p30);
00015 //DigitalIn sw3(p24);
00016 RoboClaw MD(115200, p9, p10);
00017 RawSerial pc(USBTX,USBRX,115200);
00018 //DigitalIn SENS1(p18);//geregehand
00019 //DigitalIn SENS2(p17);
00020 /*
00021 QEI enc1(NC,NC,NC,48,&T,QEI::4X_ENCODING);
00022 QEI enc2(NC,NC,NC,48,&T,QEI::4X_ENCODING);
00023 */
00024 Filter LPF1(INT_TIME);
00025 Filter LPF2(INT_TIME);
00026 /*
00027 int cmd = 0;
00028 int rot3 = 0;
00029 int rot4 = 0;
00030 */
00031 int ref_qpps1 = 0,ref_qpps2 = 0;
00032 int qpps1 = 0,qpps2 = 0;
00033 
00034 
00035 void robo_send(int adrs, int speed1, int speed2){
00036     MD.SpeedM1(adrs,speed1);
00037     MD.SpeedM2(adrs,speed2);
00038 }
00039 
00040 void timer_warikomi(){
00041     bool sw_buff = sw.read();
00042     static bool pre_sw = sw.read(), t_f = 0;
00043     
00044     if(sw_buff && !pre_sw) t_f = !t_f;
00045     else t_f = t_f;
00046     pre_sw = sw_buff;
00047     
00048     if(t_f) {
00049         ref_qpps1 = REF1;
00050         ref_qpps2 = REF2;
00051     }
00052     else {
00053         ref_qpps1 = 0;
00054         ref_qpps2 = 0;
00055     }
00056     
00057     qpps1 = LPF1.LowPassFilter(ref_qpps1);
00058     qpps2 = LPF2.LowPassFilter(ref_qpps2);
00059     
00060     robo_send(ADRS,qpps1,qpps2);
00061 }
00062 
00063 int main() {
00064     
00065     LPF1.setLowPassPara(2.0, 0);
00066     LPF2.setLowPassPara(2.0, 0);
00067     
00068     //SENS1.mode( PullUp );//内蔵PullUp使用.今回は関係なし．
00069     timer.attach(timer_warikomi,INT_TIME);
00070     while(true) {
00071     }
00072 }