Revision 4:3f22193053d0, committed 2020-01-08

Comitter:

anfontanelli

Date:

Wed Jan 08 11:05:36 2020 +0000

Parent:

3:fc26045926d9

Child:

5:362759e42070

Commit message:

release bad

Changed in this revision

--- a/Robots/Rover/Rover.cpp	Wed Nov 06 10:57:51 2019 +0000
+++ b/Robots/Rover/Rover.cpp	Wed Jan 08 11:05:36 2020 +0000
@@ -3,19 +3,19 @@
 Rover::Rover(){
     
     ionMcBoudRate = 460800;
-    meccanumBoardAddress = 128;
-    meccanumMotorGearBoxRatio = 103;
-    meccanumEncoderPulse = 512;
-    meccanumKt = 0.00667;
-    meccanumTransmissionRatio = 1.0;
-    omniTransmissionRatio = 1.0;
-    omniBoardAddress = 129;
-    omniMotorGearBoxRatio = 103;
-    omniEncoderPulse = 512;
-    omniKt = 0.00667;
+    frontBoardAddress = 128;
+    frontMotorGearBoxRatio = 103;
+    frontEncoderPulse = 512;
+    frontKt = 0.00667;
+    frontTransmissionRatio = 1.0;
+    retroTransmissionRatio = 1.0;
+    retroBoardAddress = 129;
+    retroMotorGearBoxRatio = 103;
+    retroEncoderPulse = 512;
+    retroKt = 0.00667;
     
-    r_meccanumWheel = 0.03;
-    r_omniWheel = 0.019;
+    r_frontWheel = 0.03;
+    r_retroWheel = 0.03;
     
     pipeCurve_I = 0.165;
     pipeCurve_E = 0.2925;
@@ -23,18 +23,18 @@
     
     pipeDir = 0;
         
-    omniMeccanumCentralDistance = 0.25;
+    retroFrontCentralDistance = 0.25;
     wheelsCntactPointDistanceFromPipeCenter = 0.08; 
     
     eth_time_out = 2.0;
     
-    S = Eigen::Matrix3f::Zero();       
+    S = Eigen::Matrix<float, 4, 3>::Zero();       
 
     
-    ionMcMeccanum = new IONMcMotors(meccanumBoardAddress,ionMcBoudRate, PG_14, PG_9, meccanumMotorGearBoxRatio, meccanumEncoderPulse, meccanumKt, meccanumKt);
-    ionMcOmni = new IONMcMotors(omniBoardAddress,ionMcBoudRate, PB_9, PB_8, omniMotorGearBoxRatio, omniEncoderPulse, omniKt, omniKt);
-    meccanumActuonix = new Servo(PE_11);
-    omniActuonix = new Servo(PE_9);
+    ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PG_14, PG_9, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt);
+    ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PB_9, PB_8, retroMotorGearBoxRatio, retroEncoderPulse, retroKt, retroKt);
+    frontActuonix = new Servo(PE_11);
+    retroActuonix = new Servo(PE_9);
     mpu = new MPU6050(PF_15, PF_14);
     eth_tcp = new Eth_tcp(3154, 500); 
     
@@ -72,7 +72,6 @@
      acquireTofs(frontDistance, retroDistance);
      
      
-     
      float frontVel = 0.05*frontDistance;
      float retroVel = -0.04*retroDistance;
      
@@ -132,16 +131,16 @@
 void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
     
     Eigen::Vector3f cartesianSpeed;
-    Eigen::Vector3f wheelsSpeed;
+    Eigen::Vector4f wheelsSpeed;
     
     updateRoverKin(pipe_radius, pipeDir);
     
     cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
     wheelsSpeed = S*cartesianSpeed;  
-    ionMcMeccanum->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
-    ionMcMeccanum->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
-    ionMcOmni->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
-    
+    ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
+    ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
+    ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
+    ionMcRetro->setSpeed(2,wheelsSpeed[3],maxWheelAcceleration);
 }
 
 
@@ -179,17 +178,18 @@
 void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){
     
     Eigen::Vector3f cartesianSpeed;
-    Eigen::Vector3f wheelsSpeed;
+    Eigen::Vector4f wheelsSpeed;
     
     float speedM1 = 0.0;
     float speedM2 = 0.0;
     float speedM3 = 0.0;  
-          
+    float speedM4 = 0.0;   
+            
     updateRoverKin(pipe_radius, pipeDir);
     
-    getMotorsSpeed(speedM1, speedM2, speedM3);
+    getMotorsSpeed(speedM1, speedM2, speedM3, speedM3);
     
-    wheelsSpeed << speedM1, speedM2, speedM3;
+    wheelsSpeed << speedM1, speedM2, speedM3, speedM4;
     
     cartesianSpeed = S_inv*wheelsSpeed;   
     
@@ -199,28 +199,29 @@
     
 }
 
-void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){
+void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){
     
     Eigen::Vector3f cartesianSpeed;
-    Eigen::Vector3f wheelsSpeed;
+    Eigen::Vector4f wheelsSpeed;
     
     float speedM1 = 0.0;
     float speedM2 = 0.0;
     float speedM3 = 0.0;  
-              
-    getMotorsSpeed(speedM1, speedM2, speedM3);
+    float speedM4 = 0.0;  
+    
+    getMotorsSpeed(speedM1, speedM2, speedM3, speedM4);
     
-    speed_wheel_dx = speedM1;
-    speed_wheel_sx = speedM2;
-    speed_wheels_retro = speedM3;
-    
+    front_dx = speedM1;
+    front_sx = speedM2;
+    retro_dx = speedM3;
+    retro_sx = speedM4;    
     
 }
 
 void Rover::updateRoverKin(float pipe_radius, int pipeDir){
-    float rm = r_meccanumWheel;
-    float ro = r_omniWheel;
-    float l = omniMeccanumCentralDistance;
+    float rf = r_frontWheel;
+    float rr = r_retroWheel;
+    float l = retroFrontCentralDistance;
     float L = wheelsCntactPointDistanceFromPipeCenter;
     float pr = pipe_radius;
     float R_dx = 0.0;
@@ -240,30 +241,33 @@
         R_m = pipeCurve_M;        
         
     }
-        
-    S <<  -1.0*R_dx/(rm*R_m), pr/rm, -L/rm,
-         1.0*R_sx/(rm*R_m), pr/rm, -L/rm,
-          0,    pr/ro,  l/ro;
+        //AGGIORNARE la S
+    S <<  -1.0*R_dx/(rf*R_m), pr/rf, -(L+l)/rf,
+           1.0*R_sx/(rf*R_m), pr/rf, -(L+l)/rf,
+          -1.0*R_dx/(rr*R_m), -pr/rr, -(L+l)/rr,
+           1.0*R_sx/(rr*R_m), -pr/rr, -(L+l)/rr;
           
-    S_inv <<   -rm/2, rm/2, 0,
-               l*rm/(2*pr*(L+l)),  l*rm/(2*pr*(L+l)), L*ro/(pr*(L+l)),
-               -rm/(2*(L+l)), -rm/(2*(L+l)), ro/(L+l); 
+          
+    Eigen::FullPivLU<Matrix4f> Core_S(S*S.transpose());
+    S_inv = S.transpose()*Core_S.inverse();      
+          
     }
 
-void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){
+void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){
     
 
-    torM1 = ionMcMeccanum->getMotorTorque(1);
-    torM2 = ionMcMeccanum->getMotorTorque(2);
-    torM3 = ionMcOmni->getMotorTorque(1);
-    
+    torM1 = ionMcFront->getMotorTorque(1);
+    torM2 = ionMcFront->getMotorTorque(2);
+    torM3 = ionMcRetro->getMotorTorque(1);
+    torM4 = ionMcRetro->getMotorTorque(2);    
 }
 
-void  Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3){
+void  Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4){
     
-    speedM1 = ionMcMeccanum->getMotorSpeed(1);
-    speedM2 = ionMcMeccanum->getMotorSpeed(2);
-    speedM3 = ionMcOmni->getMotorSpeed(1);
+    speedM1 = ionMcFront->getMotorSpeed(1);
+    speedM2 = ionMcFront->getMotorSpeed(2);
+    speedM3 = ionMcRetro->getMotorSpeed(1);
+    speedM4 = ionMcRetro->getMotorSpeed(2);
     
 }
 
@@ -276,8 +280,8 @@
     float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront);
     float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro);
 
-    meccanumActuonix->write(1.0 - LmFront/maxLenght);
-    omniActuonix->write(1.0 - LmRetro/maxLenght);
+    frontActuonix->write(1.0 - LmFront/maxLenght);
+    retroActuonix->write(1.0 - LmRetro/maxLenght);
     
     if (jointFront > 3.14*5/180){
         pipeDir = -1;

--- a/Robots/Rover/Rover.h	Wed Nov 06 10:57:51 2019 +0000
+++ b/Robots/Rover/Rover.h	Wed Jan 08 11:05:36 2020 +0000
@@ -4,6 +4,7 @@
 #include "IONMcMotors.h"
 #include "Servo.h"
 #include "Core.h"
+#include <Eigen/Dense.h>
 #include "MPU6050.h"
 #include "platform/PlatformMutex.h"
 
@@ -11,6 +12,9 @@
 
 #include "Eth_tcp.h"
 
+typedef Eigen::Matrix<float, 4, 4> Matrix4f;
+
+
 
 class Rover
 {
@@ -19,10 +23,10 @@
     
     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 getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4);
+    void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4);
     
-    void getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro);
+    void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx);
 
 
     void setCentralJointsAngle(float act1, float act2);
@@ -51,11 +55,11 @@
 
 private:
 
-    IONMcMotors *ionMcMeccanum;
-    IONMcMotors *ionMcOmni;
+    IONMcMotors *ionMcFront;
+    IONMcMotors *ionMcRetro;
     
-    Servo *meccanumActuonix;
-    Servo *omniActuonix;
+    Servo *frontActuonix;
+    Servo *retroActuonix;
     
     MPU6050 *mpu;
     
@@ -67,26 +71,26 @@
 
     int ionMcBoudRate;
     
-    int meccanumMotorGearBoxRatio;
-    int omniMotorGearBoxRatio;
+    int frontMotorGearBoxRatio;
+    int retroMotorGearBoxRatio;
     
-    int meccanumBoardAddress;
-    int omniBoardAddress;
+    int frontBoardAddress;
+    int retroBoardAddress;
     
-    float meccanumTransmissionRatio;
-    float omniTransmissionRatio;
+    float frontTransmissionRatio;
+    float retroTransmissionRatio;
     
-    int meccanumEncoderPulse;
-    int omniEncoderPulse;
+    int frontEncoderPulse;
+    int retroEncoderPulse;
     
-    float meccanumKt;
-    float omniKt;
+    float frontKt;
+    float retroKt;
     
-    float r_meccanumWheel;
-    float r_omniWheel;
+    float r_frontWheel;
+    float r_retroWheel;
 
     float wheelsCntactPointDistanceFromPipeCenter;
-    float omniMeccanumCentralDistance;
+    float retroFrontCentralDistance;
     
     //Imu variables
     int16_t ax, ay, az;
@@ -103,8 +107,6 @@
     
     float accBias[3];
     float gyroBias[3];   
-    
-    
 
     float pitchAcc;
     float rollAcc;
@@ -113,7 +115,7 @@
     float pitch_integrated;
     float roll_integrated;
     
-        
+
     int forward_vel;
     int pitch_d;
     
@@ -138,8 +140,8 @@
     double eth_time;    
     double eth_time_sample_received;
     
-    Eigen::Matrix3f S;    
-    Eigen::Matrix3f S_inv;    
+    Eigen::Matrix<float, 4, 3> S;    
+    Eigen::Matrix<float, 3, 4> S_inv;    
     //private functions
     float deg2rad(float deg);
     float centralJoint2actuonix(float jointAngle);
@@ -147,8 +149,7 @@
     void updateRoverKin(float pipe_radius, int pipeDir);
     
     void setState(int state);
-    
-    
+
     Timer comunicationTimer;  
     
     char cmd;
@@ -160,9 +161,7 @@
     bool eth_status;
     
     bool sock_open;
-    
-
-    
+ 
     float frontPos;
     float retroPos;
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Rover/Rover_omni_cpp.txt	Wed Jan 08 11:05:36 2020 +0000
@@ -0,0 +1,487 @@
+#include "Rover.h"
+
+Rover::Rover(){
+    
+    ionMcBoudRate = 460800;
+    meccanumBoardAddress = 128;
+    meccanumMotorGearBoxRatio = 103;
+    meccanumEncoderPulse = 512;
+    meccanumKt = 0.00667;
+    meccanumTransmissionRatio = 1.0;
+    omniTransmissionRatio = 1.0;
+    omniBoardAddress = 129;
+    omniMotorGearBoxRatio = 103;
+    omniEncoderPulse = 512;
+    omniKt = 0.00667;
+    
+    r_meccanumWheel = 0.03;
+    r_omniWheel = 0.019;
+    
+    pipeCurve_I = 0.165;
+    pipeCurve_E = 0.2925;
+    pipeCurve_M = 0.228;
+    
+    pipeDir = 0;
+        
+    omniMeccanumCentralDistance = 0.25;
+    wheelsCntactPointDistanceFromPipeCenter = 0.08; 
+    
+    eth_time_out = 2.0;
+    
+    S = Eigen::Matrix3f::Zero();       
+
+    
+    ionMcMeccanum = new IONMcMotors(meccanumBoardAddress,ionMcBoudRate, PG_14, PG_9, meccanumMotorGearBoxRatio, meccanumEncoderPulse, meccanumKt, meccanumKt);
+    ionMcOmni = new IONMcMotors(omniBoardAddress,ionMcBoudRate, PB_9, PB_8, omniMotorGearBoxRatio, omniEncoderPulse, omniKt, omniKt);
+    meccanumActuonix = new Servo(PE_11);
+    omniActuonix = new Servo(PE_9);
+    mpu = new MPU6050(PF_15, PF_14);
+    eth_tcp = new Eth_tcp(3154, 500); 
+    
+    distance_sensors = new distanceSensors(); 
+    
+    
+    eth_state = -1;
+    
+    eth_status = false;
+    
+    
+}
+
+void Rover::initializeTofs(){
+    distance_sensors->TOFs_init();
+    distance_sensors->TOFs_offset(); 
+}
+    
+void Rover::acquireTofs(float &frontDistance, float &retroDistance){
+     
+     distance_sensors->TOFs_acquireFiltr();
+     frontDistance = distance_sensors->getFrontDistance();
+     retroDistance = distance_sensors->getRetroDistance();
+     
+
+
+}
+
+
+void Rover::computeCentralJointsFromTofs(){
+     
+     float frontDistance;
+     float retroDistance;   
+     
+     acquireTofs(frontDistance, retroDistance);
+     
+     
+     
+     float frontVel = 0.05*frontDistance;
+     float retroVel = -0.04*retroDistance;
+     
+
+     if(enableCurv==1){
+         frontPos = frontPos + frontVel*0.005;
+         retroPos = retroPos + retroVel*0.005;
+     }else{
+         frontPos = 0.0;
+         retroPos = 0.0;
+         }
+
+     if(frontPos > 0.5) frontPos=0.5;
+     if(frontPos < -0.5) frontPos=-0.5;
+     if(retroPos > 0.5) retroPos=0.5;
+     if(retroPos < -0.5) retroPos=-0.5;
+
+     setCentralJointsAngle(frontPos,retroPos);
+     
+}
+
+
+void Rover::initializeImu(){
+    
+    printf("Initialize IMU \r\n");
+    //IMU
+    FS_a = 8192;    //per avere letture in g, sarebbe 32768/4         perchè 4g è il fondo scala
+    FS_g = 131.072;    //per avere letture in gradi/s, sarebbe 32768/250 perchè 250 è il fondo scala
+    
+    
+    accBias[0] = 0.0;
+    accBias[1] = 0.0;
+    accBias[2] = 0.0;
+    gyroBias[0] = 0.0;   
+    gyroBias[1] = 0.0;  
+    gyroBias[2] = 0.0;         
+    
+    
+    pitchAcc = 0.0;
+    rollAcc = 0.0;
+    
+    pitch_integrated = 0.0;
+    roll_integrated = 0.0; 
+    
+    mpu->initialize();
+    bool mpu6050TestResult = mpu->testConnection();
+    if(mpu6050TestResult) {
+        printf("MPU6050 test passed \r\n");
+    } else {
+        printf("MPU6050 test failed \r\n");
+    }    
+    
+    
+    calibrateImu();
+}
+
+void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
+    
+    Eigen::Vector3f cartesianSpeed;
+    Eigen::Vector3f wheelsSpeed;
+    
+    updateRoverKin(pipe_radius, pipeDir);
+    
+    cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
+    wheelsSpeed = S*cartesianSpeed;  
+    ionMcMeccanum->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
+    ionMcMeccanum->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
+    ionMcOmni->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
+    
+}
+
+
+int Rover::get_forward_vel(){ 
+    int fv;
+    eth_mutex.lock();
+    fv = forward_vel;
+    eth_mutex.unlock();
+    return fv;
+
+}
+int Rover::get_pitch(){ 
+    int pit;
+    eth_mutex.lock();
+    pit = pitch_d;
+    eth_mutex.unlock();
+    return pit;
+}
+
+int Rover::get_jointFront(){
+    int jf;
+    eth_mutex.lock();
+    jf = jointFront;
+    eth_mutex.unlock();
+    return jf;
+}
+int Rover::get_jointRetro(){
+    int jr;
+    eth_mutex.lock();
+    jr = jointRetro;
+    eth_mutex.unlock();
+    return jr;
+}
+
+void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){
+    
+    Eigen::Vector3f cartesianSpeed;
+    Eigen::Vector3f wheelsSpeed;
+    
+    float speedM1 = 0.0;
+    float speedM2 = 0.0;
+    float speedM3 = 0.0;  
+          
+    updateRoverKin(pipe_radius, pipeDir);
+    
+    getMotorsSpeed(speedM1, speedM2, speedM3);
+    
+    wheelsSpeed << speedM1, speedM2, speedM3;
+    
+    cartesianSpeed = S_inv*wheelsSpeed;   
+    
+    forward_speed = cartesianSpeed[0];
+    stabilization_speed = cartesianSpeed[1];
+    asset_correction_speed = cartesianSpeed[2];
+    
+}
+
+void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){
+    
+    Eigen::Vector3f cartesianSpeed;
+    Eigen::Vector3f wheelsSpeed;
+    
+    float speedM1 = 0.0;
+    float speedM2 = 0.0;
+    float speedM3 = 0.0;  
+              
+    getMotorsSpeed(speedM1, speedM2, speedM3);
+    
+    speed_wheel_dx = speedM1;
+    speed_wheel_sx = speedM2;
+    speed_wheels_retro = speedM3;
+    
+    
+}
+
+void Rover::updateRoverKin(float pipe_radius, int pipeDir){
+    float rm = r_meccanumWheel;
+    float ro = r_omniWheel;
+    float l = omniMeccanumCentralDistance;
+    float L = wheelsCntactPointDistanceFromPipeCenter;
+    float pr = pipe_radius;
+    float R_dx = 0.0;
+    float R_sx = 0.0;
+    float R_m = 0.0;    
+    if(pipeDir ==0){
+        R_dx = 1.0;
+        R_sx = 1.0;
+        R_m = 1.0;
+    }else if(pipeDir == 1){
+        R_dx = pipeCurve_I;
+        R_sx = pipeCurve_E;
+        R_m = pipeCurve_M;
+    }else if(pipeDir == -1){
+        R_dx = pipeCurve_E;
+        R_sx = pipeCurve_I;
+        R_m = pipeCurve_M;        
+        
+    }
+        
+    S <<  -1.0*R_dx/(rm*R_m), pr/rm, -L/rm,
+         1.0*R_sx/(rm*R_m), pr/rm, -L/rm,
+          0,    pr/ro,  l/ro;
+          
+    S_inv <<   -rm/2, rm/2, 0,
+               l*rm/(2*pr*(L+l)),  l*rm/(2*pr*(L+l)), L*ro/(pr*(L+l)),
+               -rm/(2*(L+l)), -rm/(2*(L+l)), ro/(L+l); 
+    }
+
+void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){
+    
+
+    torM1 = ionMcMeccanum->getMotorTorque(1);
+    torM2 = ionMcMeccanum->getMotorTorque(2);
+    torM3 = ionMcOmni->getMotorTorque(1);
+    
+}
+
+void  Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3){
+    
+    speedM1 = ionMcMeccanum->getMotorSpeed(1);
+    speedM2 = ionMcMeccanum->getMotorSpeed(2);
+    speedM3 = ionMcOmni->getMotorSpeed(1);
+    
+}
+
+void Rover::setCentralJointsAngle(float jointFront, float jointRetro){
+    
+    float LmRestPosFront = (9.0+0.5)/1000; //9
+    float LmRestPosRetro = (9.0-0.5)/1000; //9
+    float maxLenght = 0.02;
+    
+    float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront);
+    float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro);
+
+    meccanumActuonix->write(1.0 - LmFront/maxLenght);
+    omniActuonix->write(1.0 - LmRetro/maxLenght);
+    
+    if (jointFront > 3.14*5/180){
+        pipeDir = -1;
+    }else if (jointFront < -3.14*5/180){    
+        pipeDir = 1;
+    }else{
+        pipeDir = 0;
+    }
+        
+    
+}
+
+float Rover::centralJoint2actuonix(float jointAngle){
+    
+    float alpha = deg2rad(34.4);
+    float L1 = 23.022/1000;
+    float L2 = 62.037/1000;
+    float L3 = 51.013/1000;
+    float L4 = 4.939/1000;     
+    
+    float l1 = L1*sin(alpha + jointAngle);
+    float l2 = L1*cos(alpha + jointAngle); 
+    float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) );
+    return L3 - h;     
+        
+}
+
+
+float Rover::deg2rad(float deg) {
+    return deg * M_PI / 180.0;
+}
+
+
+
+void Rover::calcImuAngles(float& pitch, float& roll, float dtImu)
+{ 
+
+         mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz);       //acquisisco i dati dal sensore
+         a_x=float(ax)/FS_a - accBias[0];                                  //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
+         a_y=float(ay)/FS_a - accBias[1];
+         a_z=float(az)/FS_a;
+         g_x=float(gx)/FS_g - gyroBias[0];
+         g_y=float(gy)/FS_g - gyroBias[1];
+         g_z=float(gz)/FS_g - gyroBias[2];        
+         
+            
+        pitch_integrated = g_x * dtImu;  // Angle around the X-axis     // Integrate the gyro data(deg/s) over time to get angle, usato g_x perchè movimento attorno all'asse x
+        roll_integrated  =  g_y * dtImu;  // Angle around the Y-axis    //uso g_y perchè ho il rollio ruotando atttorno all'asse y
+      
+        pitchAcc = atan2f(a_y, sqrt(a_z*a_z + a_x*a_x))*180/M_PI;         //considerare formule paper, calcolo i contrubiti dovuti dall'accelerometro
+        rollAcc  = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI;        //
+        
+        if ( abs( pitchAcc - pitchAcc_p ) > 10){
+            pitchAcc = pitch;
+            }
+            
+        if ( abs( rollAcc - rollAcc_p ) > 10){
+            rollAcc = roll;
+            }   
+  
+        // Apply Complementary Filter 
+        pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc;
+        roll  = 0.95f*( roll + roll_integrated)  + 0.05f*(rollAcc);
+        
+        pitchAcc_p = pitchAcc;
+        rollAcc_p = rollAcc;
+   
+    
+}
+
+void Rover::calibrateImu()
+{ 
+ const int CALIBRATION = 500;
+         for(int i=0;i<50;i++)
+   {
+         mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  
+   }
+     
+    
+   printf(" START CALIBRATION \r\n");
+   
+     
+   for(int i=0;i<CALIBRATION;i++)
+   {
+         mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz);       //acquisisco i dati dal sensore   
+         a_x=(float)ax/FS_a;                                  //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
+         a_y=(float)ay/FS_a;
+       //a_z=(float)az/FS_a;
+         g_x=(float)gx/FS_g;
+         g_y=(float)gy/FS_g;;
+         g_z=(float)gz/FS_g;;
+        
+         accBias[0] += a_x;
+         accBias[1] += a_y;
+         gyroBias[0] += g_x; 
+         gyroBias[1] += g_y; 
+         gyroBias[2] += g_z;   
+   
+   }
+   
+        //BIAS   
+         accBias[0]  = accBias[0]/CALIBRATION;
+         accBias[1]  = accBias[1]/CALIBRATION;
+         gyroBias[0] = gyroBias[0]/CALIBRATION; 
+         gyroBias[1] = gyroBias[1]/CALIBRATION; 
+         gyroBias[2] = gyroBias[2]/CALIBRATION;   
+   
+   
+   printf(" END CALIBRATION \r\n");
+   
+   //printf("%f\t %f\t %f\t %f\t %f\t \r\n", accBias[0], accBias[1], gyroBias[0],gyroBias[1],gyroBias[2]);   //stampo le misure
+   
+    
+}
+
+
+
+void Rover::startEthComunication(){
+    printf("Avvio la comunicazione ethernet \r\n");
+    comunicationTimer.start(); 
+    eth_tcp->connect();
+    setState(1);
+
+}
+ 
+void Rover::ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) {
+                       
+        if( eth_tcp->is_connected()){
+            setState(2);
+            printf("ETH: connected!\r\n");
+            sock_open = true;
+        }else{
+            setState(1);
+        }
+
+        eth_status = eth_tcp->recv_pkt(cmd, recv_vec, n_of_int);
+        
+        eth_time = comunicationTimer.read();
+        //printf("%f\r\n", (double)(eth_time - eth_time_sample_received));
+        
+                 
+        if(eth_status){  
+            eth_time_sample_received = comunicationTimer.read();  
+            if(cmd == 's'){            
+                if(n_of_int == 4){
+                    eth_mutex.lock();
+                    forward_vel = recv_vec.get();
+                    pitch_d = recv_vec.get();
+                    //jointFront = recv_vec.get();
+                    //jointRetro = recv_vec.get();
+                    enableStab = recv_vec.get();
+                    enableCurv= recv_vec.get();
+                    eth_mutex.unlock();
+                }
+                
+                eth_mutex.lock();
+                vec_to_send.put(_time*1000);
+                vec_to_send.put(_pitch*1000);
+                vec_to_send.put(_vels_a*1000);
+                vec_to_send.put(_vels_m*1000);
+                vec_to_send.put(_velf_a*1000);
+                vec_to_send.put(_velf_m*1000);       
+                           
+                eth_mutex.unlock();
+                eth_tcp->send_vec_of_int(vec_to_send);
+                
+                vec_to_send.clear();
+   
+                cmd = ' ';
+                
+            } 
+        }  
+        
+        if((double)(eth_time - eth_time_sample_received)>eth_time_out && sock_open == true){
+            eth_tcp->reset_connection();
+            sock_open = false;
+            eth_mutex.lock();
+            forward_vel = 0.0;
+            pitch_d = 0.0;
+            //jointFront = 0.0;
+            //jointRetro = 0.0;
+            enableCurv = 0;
+            eth_mutex.unlock();
+        }
+
+
+}
+
+int Rover::getEthState(){
+    int state;
+    
+    eth_mutex.lock();
+    state = eth_state;
+    eth_mutex.unlock();
+
+    return state;
+}
+
+void Rover::setState(int state){
+
+    eth_mutex.lock();
+    eth_state = state;
+    eth_mutex.unlock();    
+    
+}    
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Rover/Rover_omni_h.txt	Wed Jan 08 11:05:36 2020 +0000
@@ -0,0 +1,172 @@
+#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
\ No newline at end of file

--- a/main.cpp	Wed Nov 06 10:57:51 2019 +0000
+++ b/main.cpp	Wed Jan 08 11:05:36 2020 +0000
@@ -6,7 +6,7 @@
 //#include "MX.h"
 //#include "UARTSerial_half.h"
 
-#define DEBUG false
+#define DEBUG true
 #define READ true 
 
  
@@ -34,9 +34,10 @@
     float vels_m = 0.0;    
     float vela_m = 0.0;
     
-    float vel_dx = 0.0;
-    float vel_sx = 0.0;    
-    float vel_r = 0.0;
+    float front_vel_dx = 0.0;
+    float front_vel_sx = 0.0;    
+    float retro_vel_dx = 0.0;
+    float retro_vel_sx = 0.0;
     float forward_vel = 0.0; 
     float pitch;
     
@@ -121,7 +122,7 @@
              
              timeReadPrec = time;             
              //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); 
-             rover.getRoverWheelsVelocity(vel_dx, vel_sx, vel_r); 
+             rover.getRoverWheelsVelocity(front_vel_dx, front_vel_sx, retro_vel_dx, retro_vel_sx); 
              
  
          }
@@ -130,7 +131,7 @@
          if(DEBUG && time - timePrintPrec > dtPrint){
              timePrintPrec = time;
               
-             //printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance);
+             printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance);
                
              //printf(" Ts: %4.2f \t Vfa: %4.4f \t Vfm %4.4f \t Vsa: %4.4f \t Vsm: %4.4f \t A: %4.4f \r\n",dtPassedStab*1000, velf_a, velf_m, vels_a, vels_m, amplitude);
 
@@ -148,7 +149,7 @@
              
 
          }
-          rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, vel_r);
+          rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, velf_a);
           //rover.ethComunicationUpdate(schedulerTimer.read(), pitch, velf_m, vels_m, dtPassedStab*1000, vel_r);