File content as of revision 4:3f22193053d0:

#include "Rover.h"

Rover::Rover(){
    
    ionMcBoudRate = 460800;
    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_frontWheel = 0.03;
    r_retroWheel = 0.03;
    
    pipeCurve_I = 0.165;
    pipeCurve_E = 0.2925;
    pipeCurve_M = 0.228;
    
    pipeDir = 0;
        
    retroFrontCentralDistance = 0.25;
    wheelsCntactPointDistanceFromPipeCenter = 0.08; 
    
    eth_time_out = 2.0;
    
    S = Eigen::Matrix<float, 4, 3>::Zero();       

    
    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); 
    
    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::Vector4f wheelsSpeed;
    
    updateRoverKin(pipe_radius, pipeDir);
    
    cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
    wheelsSpeed = S*cartesianSpeed;  
    ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
    ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
    ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
    ionMcRetro->setSpeed(2,wheelsSpeed[3],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::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, speedM3);
    
    wheelsSpeed << speedM1, speedM2, speedM3, speedM4;
    
    cartesianSpeed = S_inv*wheelsSpeed;   
    
    forward_speed = cartesianSpeed[0];
    stabilization_speed = cartesianSpeed[1];
    asset_correction_speed = cartesianSpeed[2];
    
}

void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){
    
    Eigen::Vector3f cartesianSpeed;
    Eigen::Vector4f wheelsSpeed;
    
    float speedM1 = 0.0;
    float speedM2 = 0.0;
    float speedM3 = 0.0;  
    float speedM4 = 0.0;  
    
    getMotorsSpeed(speedM1, speedM2, speedM3, speedM4);
    
    front_dx = speedM1;
    front_sx = speedM2;
    retro_dx = speedM3;
    retro_sx = speedM4;    
    
}

void Rover::updateRoverKin(float pipe_radius, int pipeDir){
    float rf = r_frontWheel;
    float rr = r_retroWheel;
    float l = retroFrontCentralDistance;
    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;        
        
    }
        //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;
          
          
    Eigen::FullPivLU<Matrix4f> Core_S(S*S.transpose());
    S_inv = S.transpose()*Core_S.inverse();      
          
    }

void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){
    

    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, float &speedM4){
    
    speedM1 = ionMcFront->getMotorSpeed(1);
    speedM2 = ionMcFront->getMotorSpeed(2);
    speedM3 = ionMcRetro->getMotorSpeed(1);
    speedM4 = ionMcRetro->getMotorSpeed(2);
    
}

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);

    frontActuonix->write(1.0 - LmFront/maxLenght);
    retroActuonix->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();    
    
}