File content as of revision 0:65943c77d1dc:

#ifndef ROVER_H
#define ROVER_H
#include "mbed.h"

#include "controlDefineVariables.h"

#include "IONMcMotors.h"
#include "MPU6050.h"
#include "TOFs.h"
#include "Servo.h"
#include "Core.h"
#include <Eigen/Dense.h>
#include "platform/PlatformMutex.h"

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

class Rover
{
public:
    Rover();    
    
    void setRoverVelocity(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 setParameters(Eigen::VectorXd roverParameters);
    
    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& pitch_vel, float& roll, float dtImu);    
    
    void computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx);    
    
    float computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd);

    void initializeImu();
    
    void initializeTofs();
    
    void acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance);

    

private:

    IONMcMotors *ionMcFront;
    IONMcMotors *ionMcRetro;
    
    Servo *frontActuonix;
    Servo *retroActuonix;
    
    MPU6050 *mpu;
         
    TOFs *tofs;
    
    float r_frontWheel;
    float r_retroWheel;
    float pipeCurve_I;
    float pipeCurve_E;
    float pipeCurve_M;
    float retroFrontCentralDistance;
    float wheelsCntactPointDistanceFromPipeCenter;

    //Imu variables
   
    float accBias[3];
    float gyroBias[3];   
    int pitch_d;
    float pitch;
    float g_x_old;

    int forward_vel;
    
    int jointFront;
    int jointRetro;
    int enableStab;
    int enableCurv;    
    
    float time;
    float vels_a;
    float vels_m;
 
    int pipeDir;
        
    Eigen::Matrix<float, 4, 3> S;    
    Eigen::Matrix<float, 3, 4> S_inv;    
    
    float frontPos;
    float retroPos;

    //private functions
    float deg2rad(float deg);
    float centralJoint2actuonix(float jointAngle);
    void calibrateImu();
    void updateRoverKin(float pipe_radius, int pipeDir);
    
    void setState(int state);

    int get_forward_vel();
    //PlatformMutex eth_mutex;
    
    float roverFrontDistance;
    float roverRetroDistance;

};

#endif