Diff: Robots/Rover/Rover.cpp

Revision:

6:584653235830

Parent:

4:3f22193053d0

diff -r 362759e42070 -r 584653235830 Robots/Rover/Rover.cpp
--- a/Robots/Rover/Rover.cpp	Wed Jan 08 11:12:51 2020 +0000
+++ b/Robots/Rover/Rover.cpp	Tue Sep 14 11:18:35 2021 +0000
@@ -1,84 +1,84 @@
 #include "Rover.h"
+#include <iostream>
 
 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);
+    ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PB_9, PB_8, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt);
+    ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PG_14, PG_9, 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;
-    
+    tofs = new TOFs(); 
+
+    enableCurv = 1;
+    g_x_old = 0.0;
     
 }
 
 void Rover::initializeTofs(){
-    distance_sensors->TOFs_init();
-    distance_sensors->TOFs_offset(); 
-}
+
+    tofs->TOFs_init();
+    ThisThread::sleep_for(1000);
+    //tofs->TOFs_offset(); 
+
     
-void Rover::acquireTofs(float &frontDistance, float &retroDistance){
-     
-     distance_sensors->TOFs_acquireFiltr();
-     frontDistance = distance_sensors->getFrontDistance();
-     retroDistance = distance_sensors->getRetroDistance();
-     
+}
 
-
+void Rover::setParameters(Eigen::VectorXd roverParameters){
+    r_frontWheel = roverParameters(0);
+    r_retroWheel = roverParameters(1);
+    pipeCurve_I = roverParameters(2);
+    pipeCurve_E = roverParameters(3);
+    pipeCurve_M = roverParameters(4);
+    retroFrontCentralDistance = roverParameters(5);
+    wheelsCntactPointDistanceFromPipeCenter = roverParameters(6);
+    
 }
 
 
-void Rover::computeCentralJointsFromTofs(){
+float Rover::computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd){
+     
+     vels_a = -Kp*M_PI*(pitch_d-pitch)/180 -Kd*M_PI*(0-pitch_vel)/180;
+    return vels_a;
+    
+}
+
+    
+void Rover::acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance){
+     
+     tofs->TOFs_acquireFiltr();
+     roverLaserFrontDx = tofs->getLaserFrontDx();
+     roverLaserFrontSx = tofs->getLaserFrontSx();
+     roverLaserRetroDx = tofs->getLaserRetroDx();
+     roverLaserRetroSx = tofs->getLaserRetroSx();
+     
+     roverFrontDistance = tofs->getFrontDistance();
+     roverRetroDistance = tofs->getRetroDistance();
+
+  
+}
+
+void Rover::computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx){
      
      float frontDistance;
-     float retroDistance;   
+     float retroDistance;
      
-     acquireTofs(frontDistance, retroDistance);
+     acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance);
      
-     
+     frontDistance = roverFrontDistance;
+     retroDistance = roverRetroDistance;
      float frontVel = 0.05*frontDistance;
      float retroVel = -0.04*retroDistance;
      
-
      if(enableCurv==1){
-         frontPos = frontPos + frontVel*0.005;
-         retroPos = retroPos + retroVel*0.005;
+         frontPos = frontPos + frontVel*0.02;
+         retroPos = retroPos + retroVel*0.02;
      }else{
          frontPos = 0.0;
          retroPos = 0.0;
@@ -98,24 +98,14 @@
     
     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) {
@@ -128,7 +118,7 @@
     calibrateImu();
 }
 
-void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
+void Rover::setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
     
     Eigen::Vector3f cartesianSpeed;
     Eigen::Vector4f wheelsSpeed;
@@ -136,44 +126,19 @@
     updateRoverKin(pipe_radius, pipeDir);
     
     cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
-    wheelsSpeed = S*cartesianSpeed;  
+    wheelsSpeed = S*cartesianSpeed;
+    //std::cout << "des: " << wheelsSpeed.transpose() << std::endl;
     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){
     
@@ -187,10 +152,13 @@
             
     updateRoverKin(pipe_radius, pipeDir);
     
-    getMotorsSpeed(speedM1, speedM2, speedM3, speedM3);
+    getMotorsSpeed(speedM1, speedM2, speedM3, speedM4);
     
     wheelsSpeed << speedM1, speedM2, speedM3, speedM4;
     
+    //td::cout << "mis: " << wheelsSpeed.transpose() << std::endl;
+
+    
     cartesianSpeed = S_inv*wheelsSpeed;   
     
     forward_speed = cartesianSpeed[0];
@@ -242,14 +210,14 @@
         
     }
         //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 <<  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();      
+    Eigen::FullPivLU<Matrix3f> Core_S(S.transpose()*S);
+    S_inv = Core_S.inverse()*S.transpose();      
           
     }
 
@@ -273,7 +241,7 @@
 
 void Rover::setCentralJointsAngle(float jointFront, float jointRetro){
     
-    float LmRestPosFront = (9.0+0.5)/1000; //9
+    float LmRestPosFront = (9.0)/1000; //9
     float LmRestPosRetro = (9.0-0.5)/1000; //9
     float maxLenght = 0.02;
     
@@ -290,6 +258,8 @@
     }else{
         pipeDir = 0;
     }
+    
+    pipeDir = 0;/////////////////////////////////////////////////////////////
         
     
 }
@@ -316,57 +286,87 @@
 
 
 
-void Rover::calcImuAngles(float& pitch, float& roll, float dtImu)
+void Rover::calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu)
 { 
+    int16_t ax = 0;
+    int16_t ay = 0;
+    int16_t az = 0;
+    int16_t gx = 0;
+    int16_t gy = 0;
+    int16_t gz = 0;
+    float pitchAcc = 0.0;
+    float rollAcc = 0.0;
+    float pitchAcc_p = 0.0;
+    float rollAcc_p = 0.0;
+    float pitch_integrated = 0.0;
+    float roll_integrated = 0.0;
+    mpu->getMotion6(ax, ay, az, gx, gy, gz);       //acquisisco i dati dal sensore
+    float a_x=float(ax)/FS_a - accBias[0];                                  //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
+    float a_y=float(ay)/FS_a - accBias[1];
+    float a_z=float(az)/FS_a;
+    float g_x=float(gx)/FS_g - gyroBias[0];
+    float g_y=float(gy)/FS_g - gyroBias[1];
+    float g_z=float(gz)/FS_g - gyroBias[2]; 
+    if(abs(g_x) > 100.0){
+        pitch_vel = 0;
+    }else{
+        pitch_vel =  g_x;         
+    }
+    /*g_x_old = g_x;*/
+    //pitch_vel =  g_x;  
+    
+        
+    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;
 
-         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);  
-   }
+
+    //Imu variables
+    int16_t ax = 0;
+    int16_t ay = 0;
+    int16_t az = 0;
+    int16_t gx = 0;
+    int16_t gy = 0;
+    int16_t gz = 0;
+    float a_x,a_y,a_z;
+    float g_x, g_y, g_z;
+    
+    const int CALIBRATION = 500;
+    
+    for(int i=0;i<50;i++)
+    {
+         mpu->getMotion6(ax, ay, az, gx, gy, gz);  
+    }
      
     
-   printf(" START CALIBRATION \r\n");
-   
+    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   
+    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;
@@ -379,113 +379,19 @@
          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);
+    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::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();    
-    
-}    
-
-