control_interface.h

#ifndef CONTROL_IF_H
#define CONTROL_IF_H
 
#include "worldbuilder.h"
#include "task.h"
 
class Configurator; 
 
// class IOInterface{
//  protected:
//  bool ready=false;
//  public:
//  IOInterface()=default;
 
//  bool isReady(){
//      return ready;
//  }
 
//  void setReady(bool b){
//      ready=b;
//  }
 
// };
 
// class LIDAR_In:public IOInterface{ 
// public:
//  bool debugOn=0;
//  int iteration=0;
//  bool stop=0;
//  CoordinateContainer data2fp;
// };
 
class MotorInterface { 
    protected:
    float L=0, R=0, L_gain=1.0f, R_gain=1.0f, Kp=0.45, Ki=0.25, Kd=0.2; //from empirical, Kp should be 1.2
    float prev_error=0;
    float integral=0;
    public:
 
    MotorInterface()=default;
 
    MotorInterface(float kp, float ki, float kd):Kp(kp), Ki(ki), Kd(kd){}
 
    void getData(const Task::Action &a){
        L=a.getLWheelSpeed();
        R=a.getRWheelSpeed();
    }
 
    float get_L(){
        float f=L*L_gain;
        return f;
    }
 
    float get_R(){
        float f=R*R_gain;
        return f;
    }
 
    void adjust_gain( float angle_D, b2Transform observed, float * y_D=NULL);
 
    void PID(float e);
 
    float outer_loop(float e);
 
 
    void reset(){
        L_gain=1.0f;
        R_gain=1.0f;
        reset_error();
    }
 
    void reset_error(){
        integral=0;
        prev_error=0;
    }
 
};  
 
 
 
 
struct GoalChanger{
    virtual Task change_goal(const Task & t)=0;
};
#endif