Diff: main.cpp

Revision:

0:06626756e7f1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 12 06:37:11 2019 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "Filter.h"
+#include "RoboClaw.h"
+#define ADRS 129 // RoboClawのアドレス
+#define REF1 (48*4) // 目標回転数．一秒あたりの回転数をかけてご使用ください．
+#define REF2 (48*4) // 上記同様．
+#define INT_TIME 0.02
+
+Ticker timer;
+Timer T;
+
+DigitalIn sw(p29); // スイッチを使うピンに合わせてご使用ください．
+//DigitalIn sw2(p30);
+//DigitalIn sw3(p24);
+RoboClaw MD(115200, p9, p10);
+RawSerial pc(USBTX,USBRX,115200);
+//DigitalIn SENS1(p18);//geregehand
+//DigitalIn SENS2(p17);
+/*
+QEI enc1(NC,NC,NC,48,&T,QEI::4X_ENCODING);
+QEI enc2(NC,NC,NC,48,&T,QEI::4X_ENCODING);
+*/
+Filter LPF1(INT_TIME);
+Filter LPF2(INT_TIME);
+/*
+int cmd = 0;
+int rot3 = 0;
+int rot4 = 0;
+*/
+int ref_qpps1 = 0,ref_qpps2 = 0;
+int qpps1 = 0,qpps2 = 0;
+
+
+void robo_send(int adrs, int speed1, int speed2){
+    MD.SpeedM1(adrs,speed1);
+    MD.SpeedM2(adrs,speed2);
+}
+
+void timer_warikomi(){
+    bool sw_buff = sw.read();
+    static bool pre_sw = sw.read(), t_f = 0;
+    
+    if(sw_buff && !pre_sw) t_f = !t_f;
+    else t_f = t_f;
+    pre_sw = sw_buff;
+    
+    if(t_f) {
+        ref_qpps1 = REF1;
+        ref_qpps2 = REF2;
+    }
+    else {
+        ref_qpps1 = 0;
+        ref_qpps2 = 0;
+    }
+    
+    qpps1 = LPF1.LowPassFilter(ref_qpps1);
+    qpps2 = LPF2.LowPassFilter(ref_qpps2);
+    
+    robo_send(ADRS,qpps1,qpps2);
+}
+
+int main() {
+    
+    LPF1.setLowPassPara(2.0, 0);
+    LPF2.setLowPassPara(2.0, 0);
+    
+    //SENS1.mode( PullUp );//内蔵PullUp使用.今回は関係なし．
+    timer.attach(timer_warikomi,INT_TIME);
+    while(true) {
+    }
+}