Diff: Robots/Rover/Rover_omni_cpp.txt

Revision:

4:3f22193053d0

Parent:

3:fc26045926d9

--- /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();    
+    
+}    
+
+