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 Motor_Out:public IOInterface { 
    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:
 
    Motor_Out()=default;
 
    Motor_Out(float kp, float ki, float kd):Kp(kp), Ki(ki), Kd(kd){}
 
    void getData(const Task::Action &a){
        setReady(0);
        L=a.getLWheelSpeed();
        R=a.getRWheelSpeed();
        setReady(1);
    }
 
    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 void change_goal(Task * t);
};
#endif