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 "MPU6050.h"
#include "platform/PlatformMutex.h"

#include "TOFs.h"

#include "Eth_tcp.h"


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);
    void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3);
    
    void getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro);


    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 *ionMcMeccanum;
    IONMcMotors *ionMcOmni;
    
    Servo *meccanumActuonix;
    Servo *omniActuonix;
    
    MPU6050 *mpu;
    
    Eth_tcp *eth_tcp;
    
    Thread EthThread;   
    
    distanceSensors *distance_sensors;

    int ionMcBoudRate;
    
    int meccanumMotorGearBoxRatio;
    int omniMotorGearBoxRatio;
    
    int meccanumBoardAddress;
    int omniBoardAddress;
    
    float meccanumTransmissionRatio;
    float omniTransmissionRatio;
    
    int meccanumEncoderPulse;
    int omniEncoderPulse;
    
    float meccanumKt;
    float omniKt;
    
    float r_meccanumWheel;
    float r_omniWheel;

    float wheelsCntactPointDistanceFromPipeCenter;
    float omniMeccanumCentralDistance;
    
    //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::Matrix3f S;    
    Eigen::Matrix3f 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