Revision 3:c751c9e54791, committed 2021-04-16

Comitter:

kikuchi8810

Date:

Fri Apr 16 09:37:28 2021 +0000

Parent:

1:cb2c4d733c1b

Commit message:

test

Changed in this revision

diff -r cb2c4d733c1b -r c751c9e54791 Controller.cpp
--- a/Controller.cpp	Fri Jan 15 08:50:35 2021 +0000
+++ b/Controller.cpp	Fri Apr 16 09:37:28 2021 +0000
@@ -7,6 +7,7 @@
 Controller::Controller(PinName tx,PinName rx,int baudrate):XBee3(tx,rx)
 {
     XBee3.baud(baudrate);
+    XBee3.attach(this,&Controller::update,Serial::RxIrq);
     ButtonState = 0;
     LJoyX = 128;
     LJoyY = 128;
@@ -61,11 +62,11 @@
     }
 }
 
-void Controller::DRbegin()
+/*void Controller::DRbegin()
 {
     XBee3.attach(this,&Controller::update,Serial::RxIrq);
 }
-
+*/
 int8_t Controller::readButton(int buttonNum) const
 {
     int8_t ret = 0;

diff -r cb2c4d733c1b -r c751c9e54791 Controller.h
--- a/Controller.h	Fri Jan 15 08:50:35 2021 +0000
+++ b/Controller.h	Fri Apr 16 09:37:28 2021 +0000
@@ -29,7 +29,7 @@
     //Controller(RawSerial* serial_xbee);
 
     void update();
-    void DRbegin();
+    //void DRbegin();
 
     /**
      * コントローラのボタン入力状態を返す．

diff -r cb2c4d733c1b -r c751c9e54791 Filter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter.cpp	Fri Apr 16 09:37:28 2021 +0000
@@ -0,0 +1,82 @@
+#include "Filter.h"
+#include "mbed.h"
+
+
+Filter::Filter(double xint_time)
+{
+    int_time = xint_time;
+    set_t = false;
+}
+
+void Filter::setLowPassPara(double T, double init_data)
+{
+    T_LPF = T;
+    preOutput = init_data;
+    set_t = true;
+}
+
+double Filter::LowPassFilter(double input)
+{
+    //static double preOutput = input;
+    if(!set_t) {
+        // Serial.print("abc");
+        return input;
+    } else {
+        double Output = (int_time * input + T_LPF * preOutput)/(T_LPF + int_time);
+        preOutput = Output;
+        return Output;
+    }
+}
+
+void Filter::setSecondOrderPara(double xOmega, double xDzeta, double init_data)
+{
+    omega = xOmega;
+    dzeta = xDzeta;
+    prev_output2 = prev_output1 = init_data;
+
+    set_secorder = true;
+}
+
+void Filter::initPrevData(double init_data)
+{
+    prev_output2 = prev_output1 = init_data;
+}
+
+double Filter::SecondOrderLag(double input)
+{
+    if(!set_secorder) {
+        return input;
+    } else {
+        double output = (2*prev_output1*(1+dzeta*omega*int_time) - prev_output2 + pow(omega, 2.0)*pow(int_time, 2.0)*input)/(1 + 2*dzeta*omega*int_time + pow(omega, 2.0)*pow(int_time, 2.0));
+        prev_output2 = prev_output1;
+        prev_output1 = output;
+        return output;
+    }
+}
+
+void Filter::setNotchPara(double Omega, double int_data)
+{
+    // 落としたい角周波数[rad/s]をOm_nに入れる
+    Om_n = Omega;
+    sq_Om = pow(Om_n, 2.0); // Om_nの2乗
+    sq_dt = pow(int_time, 2.0); // dtの2乗
+
+    n_preOutput[0] = int_data;
+    n_preOutput[1] = int_data;
+
+    n_preInput[0] = int_data;
+    n_preInput[1] = int_data;
+}
+
+double Filter::NotchFilter(double input)
+{
+    double Output = (2*(1 + Om_n * int_time) * n_preOutput[0]-n_preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * n_preInput[0] + n_preInput[1]) / (1 + 2 * Om_n * int_time + sq_Om * sq_dt);
+
+    n_preInput[1] = n_preInput[0];
+    n_preInput[0] = input;
+
+    n_preOutput[1] = n_preOutput[0];
+    n_preOutput[0] = Output;
+
+    return Output;
+}
\ No newline at end of file

diff -r cb2c4d733c1b -r c751c9e54791 Filter.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter.h	Fri Apr 16 09:37:28 2021 +0000
@@ -0,0 +1,41 @@
+#ifndef FILTER_h
+#define FILTER_h
+
+#include "mbed.h"
+
+class Filter
+{
+public:
+    double T_LPF;
+    double Om_n;
+    double sq_dt;
+    double sq_Om;
+    
+    double omega;
+    double dzeta;
+
+    Filter(double);
+    void setLowPassPara(double T, double init_data);
+    double LowPassFilter(double input);
+    
+    void setSecondOrderPara(double xOmega, double xDzeta, double init_data);
+    void initPrevData(double init_data);
+    double SecondOrderLag(double input);
+    
+    void setNotchPara(double Omega, double init_data);
+    double NotchFilter(double input);
+    
+private:
+    double int_time;
+    double preOutput;
+    bool set_t;
+    
+    double prev_output1, prev_output2;
+    bool set_secorder;
+    
+    double n_preOutput[2];
+    double n_preInput[2];
+
+};
+
+#endif
\ No newline at end of file

diff -r cb2c4d733c1b -r c751c9e54791 main.cpp
--- a/main.cpp	Fri Jan 15 08:50:35 2021 +0000
+++ b/main.cpp	Fri Apr 16 09:37:28 2021 +0000
@@ -1,17 +1,28 @@
 #include "mbed.h"
 #include "Controller.h"
+#include "Filter.h"
+
+#define INT_TIME 0.01
+Filter joyRX_filter(INT_TIME);
+Filter joyRY_filter(INT_TIME);
+Filter joyLX_filter(INT_TIME);
+Filter joyLY_filter(INT_TIME);
 
 Controller Con(p28,p27,115200);
 Serial pc(USBTX,USBRX,115200);
 
 int main()
 {
-    Con.DRbegin();
+    joyRX_filter.setLowPassPara(0.16,0.0);
+    joyRY_filter.setLowPassPara(0.16,0.0);
+    joyLX_filter.setLowPassPara(0.16,0.0);
+    joyLY_filter.setLowPassPara(0.16,0.0);
+    
     while(1) {
-        double joyRX = Con.readJoyRXbyte();
-        double joyRY = Con.readJoyRYbyte();
-        double joyLX = Con.readJoyLXbyte();
-        double joyLY = Con.readJoyLYbyte();
+        double joyRX = joyRX_filter.LowPassFilter(Con.readJoyRXbyte());
+        double joyRY = joyRY_filter.LowPassFilter(Con.readJoyRYbyte());
+        double joyLX = joyLX_filter.LowPassFilter(Con.readJoyLXbyte());
+        double joyLY = joyLY_filter.LowPassFilter(Con.readJoyLYbyte());
         unsigned int buttonstate = Con.getButtonState();
         if(buttonstate & BUTTON_MARU) pc.printf("%d\n",Con.getButtonState());
         else if(buttonstate & BUTTON_L2) pc.printf("%3.0lf %3.0lf %3.0lf %3.0lf\n",joyRX,joyRY,joyLX,joyLY);