File content as of revision 123:55e5e9acc541:

#ifndef PLANB_H
#define PLANB_H

#include "mbed.h"
#include "Odometry2.h"
#include "Motor.h"

class aserv_planB
{
public:
    aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR);
    void update(float dt);
    void control_speed();
    void setGoal(float x, float y, float theta);
    void setGoal(float x, float y);
    void stop(void);
    bool isArrived(void) {return arrived;}
    float carre(float x) {return x*x;}
    float Kp_angle, Kd_angle, Ki_angle;
    float Kp_distance, Ki_distance, Kd_distance;

private:
    Odometry2 &m_odometry;
    Motor &m_motorL;
    Motor &m_motorR;
    
    float cmd;
    float cmd_g, cmd_d;
    float limite;
    float lim_min, lim_max;
    
    float somme_erreur_theta, somme_erreur_distance;
    float delta_erreur_theta, delta_erreur_distance;
    float erreur_precedente_theta, erreur_precedente_distance;
    float distanceGoal, distance;
    float thetaGoal;
    
    float m_goalX, m_goalY, m_goalPhi;
    
    bool arrived, squip;
    int N;
    
    char state;
    //char etat_angle;
};


#endif