File content as of revision 6:889949e44d1e:

#ifndef DIFFERENTIAL_DRIVE_KINEMATICS_H
#define DIFFERENTIAL_DRIVE_KINEMATICS_H

// STL
#include<sstream>
#include<string>

// EIGEN
#include <Eigen/Dense.h>

// MBED IO
#include "mbed.h"
#include "motordriver.h"
#include "HallEffectEncoder.h"
#include <Servo.h>
#include <SDFileSystem.h>
#include "LSM9DS1.h"

//TYPEDEFS 
typedef float Path[100][3] ;


//WIP own class
//LOW POSER SERVO VARIANT using mosfet to power up and down
class ServoLP{
public:
    ServoLP(PinName pin_servo, PinName pin_mosfet):
        servo(pin_servo),
        mosfet(pin_mosfet)
    {
        
    }    

    void power_and_set (float percent){
        mosfet = 1;
        servo = percent;
    }
    
    void shutdown (){
        mosfet = 0;
    }

private:
    Servo servo; 
    DigitalOut mosfet;   
    
};







class DifferentialDriveKinematics {
public:

    // CTOR
    DifferentialDriveKinematics(float x, float y, float theta, Motor left_motor, Motor right_motor, 
                                // PinName pin_enc_l, PinName pin_enc_r, float encoder_speed_period ,
                                HallEffectEncoder& enc_l_, HallEffectEncoder& enc_r_,
                                ServoLP& penservo_,
                                LSM9DS1& imu_,
                                float Ts, float controller_precision);

    
    
    /* ===Passive functions=== */
    void dynamicsWheels(float speed_left  , float speed_right  , float previous_states[3], float updated_states[3]);
    void dynamicsIMU   (float linear_speed, float angular_speed, float previous_states[3], float updated_states[3]);
    void updateStates  (float speed_left, float speed_right);
    
    
    void updateAngleStateIMU(float angular_vel, float dt);
    
    void updatePositionWheels(float speed_left, float speed_right, float dt);
    
    
    //IMU
    //float getLinearSpeedFromIMU();
    //float getAngularSpeedFromIMU();
    
    // Helpers
    float getDifferenceAngle(float angle1, float angle2);
    float getAngleFromTarget(float x_target, float y_target);
    bool  getWheelSpeedFromLinearAngularSpeed(float linear_speed, float angular_speed, float* wheel_speeds);
    
    //Speed Profiles
    float getPWMFromSpeedRight(float speed);
    float getPWMFromSpeedLeft(float speed);
    
    float getPWMFromSpeedRightPos(float speed);
    float getPWMFromSpeedLeftPos(float speed);
    float getPWMFromSpeedRightNeg(float speed);
    float getPWMFromSpeedLeftNeg(float speed);
    

    //KALMAN
    void kalmanUpdate(float observed_states[3], float predicted_states[3], FILE* fp, bool imu );
    void kalmanPredict(float speed_left, float speed_right, float predicted_states[3], bool imu );
    
    //Logger
    void init_logger(Serial* pc_);
    void log();
    std::string getLog(float angular_vel, float Vr, float Vl, float real_speed_left, float real_speed_right, float diff_angle, float target_angle);
    
    
    //===Active functions===

    bool followPathV4(Path path, int length_array, Serial* pc, FILE* fp);
    
    bool followPathV5(Path path, int length_array, Serial* pc, FILE* fp);
        
    //Accessors
    float getX();
    float getY();
    float getTheta();
    
    float getR();
    float getL();


private:
    /* ===IO=== */
    //Motors
    Motor m_r;
    Motor m_l;
    
    //Encoders
    HallEffectEncoder& enc_l;
    HallEffectEncoder& enc_r;
    
    /* PenServo */
    ServoLP& penservo;
    
    /* IMU */
    LSM9DS1 imu;
    
    /* Logger */
    Ticker log_ticker;

    //Kalman State
    float _x;
    float _y;
    float _theta;
    float Ts;
    
    //Body characteristics
    float R;    // Radius of the wheel
    float L;    // length between the wheels
    
    // Gains used to control the angle of the robot
    float Kp;   
    float Ki;
    float Kd;
    
    // Errors used to control the angle of the robot
    float previous_error;
    float steady_error;
    
    // uUed for the controller
    float precision;
        

    /* ===Kalman State=== */
    // Var State mat
    Eigen::Matrix3f Pk;
    
    //Transitions Matrices
    Eigen::Matrix3f Fk;
    Eigen::Matrix3f Hk; // Hk is I_3x3 for both measures
    Eigen::Matrix3f Qk;
    
    Eigen::Matrix3f Rk_odo; //Odometry meaure uncertainty
    Eigen::Matrix3f Rk_imu; //IMU      meaure uncertainty
    
    
    // CONSTANTS (WIP static since non-c++11 prohibit non static init) could be in a struct and not HARDCODED
    static const float A_L_Pos = 66.19920199;
    static const float B_L_Pos = -6.466423047830933;
    static const float A_R_Pos = 57.79291202;
    static const float B_R_Pos = -5.18994185761958;
    
    static const float A_L_Neg = 65.71910777;
    static const float B_L_Neg = 6.389965540983612;
    static const float A_R_Neg = 59.69554934;
    static const float B_R_Neg = 5.7779939016393485;
    
    
};



#endif