complementary filter object

include/complementary_filter.h

@@ -0,0 +1,10 @@
+#include "vars.h"
+
+class ComplementaryFilter{
+    public:
+        ComplementaryFilter(float k);
+        float calculate(float f);
+    private:
+        int K;
+        float oldVal;
+};

include/drivecontroller.h

@@ -3,6 +3,7 @@
 #include <Arduino.h>
 #include "motor.h"
 #include "PID_v2.h"
+#include "complementary_filter.h"
 
 //PID Constants
 #define KP 1.5
@@ -41,6 +42,7 @@ class DriveController{
         Motor* m3;
         Motor* m4;
         PID* pid;
+        ComplementaryFilter* speedFilter;
         int pDir, pSpeed, pTilt, oldSpeed;
         float x, y, vx, vy, speed1, speed2, speed3, speed4, pidfactor, delta;
 

src/complementary_filter.cpp

@@ -0,0 +1,12 @@
+#include "complementary_filter.h"
+
+ComplementaryFilter::ComplementaryFilter(float k){
+    this->K = k;
+}
+
+float result;
+float ComplementaryFilter::calculate(float val){
+    result = val*K + oldVal*(1-K);
+    oldVal = val;
+    return result;
+}

src/drivecontroller.cpp

@@ -38,6 +38,9 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     pid->SetOutputLimits(-255,255);
     pid->SetMode(AUTOMATIC);
 
+    // Complementary filter for speed
+    speedFilter = new ComplementaryFilter(0.3);
+
     canUnlock = true;
     unlockTime = 0;
 
@@ -63,7 +66,7 @@ float DriveController::torad(float f){
 
 void DriveController::drive(int dir, int speed, int tilt){
 
-    speed = (speed * KSPD + oldSpeed * (1-KSPD))*GLOBAL_SPD_MULT;
+    speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT;
     tilt = tilt > 180 ? tilt - 360 : tilt;
 
     vx = ((speed * cosins[dir]));