File content as of revision 4:3f22193053d0:

#ifndef ROVER_H
#define ROVER_H
#include "mbed.h"
#include "IONMcMotors.h"
#include "Servo.h"
#include "Core.h"
#include <Eigen/Dense.h>
#include "MPU6050.h"
#include "platform/PlatformMutex.h"

#include "TOFs.h"

#include "Eth_tcp.h"

typedef Eigen::Matrix<float, 4, 4> Matrix4f;



class Rover
{
public:
    Rover();    
    
    void setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration);
    void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius);
    void getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4);
    void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4);
    
    void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx);


    void setCentralJointsAngle(float act1, float act2);
    void calcImuAngles(float& pitch, float& roll, float dtImu);
    
    int get_forward_vel();
    int get_pitch();
    int get_jointFront();
    int get_jointRetro();
    
    void ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) ;
    
    int getEthState(void);

    
    void startEthComunication();
    
    void initializeImu();
    
    void initializeTofs();
    void acquireTofs(float &frontDistance, float &retroDistance);
    
    void computeCentralJointsFromTofs();



private:

    IONMcMotors *ionMcFront;
    IONMcMotors *ionMcRetro;
    
    Servo *frontActuonix;
    Servo *retroActuonix;
    
    MPU6050 *mpu;
    
    Eth_tcp *eth_tcp;
    
    Thread EthThread;   
    
    distanceSensors *distance_sensors;

    int ionMcBoudRate;
    
    int frontMotorGearBoxRatio;
    int retroMotorGearBoxRatio;
    
    int frontBoardAddress;
    int retroBoardAddress;
    
    float frontTransmissionRatio;
    float retroTransmissionRatio;
    
    int frontEncoderPulse;
    int retroEncoderPulse;
    
    float frontKt;
    float retroKt;
    
    float r_frontWheel;
    float r_retroWheel;

    float wheelsCntactPointDistanceFromPipeCenter;
    float retroFrontCentralDistance;
    
    //Imu variables
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
    
    float a_x,a_y,a_z;
    float g_x, g_y, g_z;
    
    int16_t FS_a;   
    float FS_g;    
    
    PlatformMutex eth_mutex;
    
    
    float accBias[3];
    float gyroBias[3];   

    float pitchAcc;
    float rollAcc;
    float pitchAcc_p;
    float rollAcc_p;
    float pitch_integrated;
    float roll_integrated;
    

    int forward_vel;
    int pitch_d;
    
    int jointFront;
    int jointRetro;
    int enableStab;
    int enableCurv;    
    
    float time;
    float pitch;
    float vels_a;
    float vels_m;
    
    float pipeCurve_I;
    float pipeCurve_E;
    float pipeCurve_M;    
    int pipeDir;
    
    double eth_time_out; //ms
    
    int eth_state;
    double eth_time;    
    double eth_time_sample_received;
    
    Eigen::Matrix<float, 4, 3> S;    
    Eigen::Matrix<float, 3, 4> S_inv;    
    //private functions
    float deg2rad(float deg);
    float centralJoint2actuonix(float jointAngle);
    void calibrateImu();
    void updateRoverKin(float pipe_radius, int pipeDir);
    
    void setState(int state);

    Timer comunicationTimer;  
    
    char cmd;
    MyBuffer <int> vec_to_send;
    MyBuffer <int> recv_vec;
    double t_p;
    double t;
    int n_of_int;
    bool eth_status;
    
    bool sock_open;
 
    float frontPos;
    float retroPos;

    
};

#endif