Diff: Robots/Rover/Rover.h

Revision:

4:3f22193053d0

Parent:

3:fc26045926d9

Child:

6:584653235830

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