crawl.h

#ifndef INCLUDED_Crawl_h
#define INCLUDED_Crawl_h
#include <Arduino.h>

class FirstOrderFilter {
 protected:
  float dt;
  float T;
  float y;
 public:
  FirstOrderFilter();
  void setDt(float dt);
  void setT(float T);

  float getOutput();
  float calculate(float x);
};

class LaggedDerivative : public FirstOrderFilter {

  float y;
 public:
  float calculate(float x);
  float getOutput();
};
class Integral {
 public:
  Integral();
  void setDt(float dt);
  void setOutput(float y);
  void setLimit(float limit_low, float limit_high);
  void calculate(float x);
  float getOutput();


  float y;
  float limit_low;
  float limit_high;
  float dt;
};
class CrlRobot {
 public:
  void init();
  void realtimeLoop();
  void setDt(float dt);
  void setKalman(bool enable_kalman);
  float getThetaX();
  float getThetaY();
  float getThetaZ();
  float getHeadVelocity();

  float getAccX();
  float getAccY();
  float getAccZ();
  float getThetaDotX();
  float getThetaDotY();
  float getThetaDotZ();
  float getEncoderLeft();
  float getEncoderRight();
  float getOdometryLeft();
  float getOdometryRight();
  void setMotorLeft(float motor_left);
  void setMotorRight(float motor_right);
  void resetEncoderLeft();
  void resetEncoderRight();
  void updateState();


  float dt;
  int dt_us;
  float theta;

  float head_velocity;

  float acc_x;
  float acc_y;
  float acc_z;
  float theta_dot_x;
  float theta_dot_y;
  float theta_dot_z;
  float offset_gx;
  float offset_gy;
  float offset_gz;
  float motor_left;
  float motor_right;
  float encoder_left;
  float encoder_right;
  unsigned long t1;
  unsigned long t2;
  unsigned long tt;
  float kEtoMM;
  bool enable_kalman;
  float rate_theta;
  FirstOrderFilter fof_acc_x;
  FirstOrderFilter fof_acc_y;
  FirstOrderFilter fof_acc_z;
  LaggedDerivative ld_odometry;

  void calcTheta();
  void calcThetaKalmanFilter();
  void makeTiming();
  void calcState();

  void calcHeadVelocity();
  void initGyroOffset();
  void initTheta();
};

extern CrlRobot crl;

#endif