Diff: Robots/Rover/Rover.cpp

Revision:

4:3f22193053d0

Parent:

3:fc26045926d9

Child:

6:584653235830

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