Diff: Rover.h

Revision:

0:65943c77d1dc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rover.h	Mon Oct 04 12:52:17 2021 +0000
@@ -0,0 +1,110 @@
+#ifndef ROVER_H
+#define ROVER_H
+#include "mbed.h"
+
+#include "controlDefineVariables.h"
+
+#include "IONMcMotors.h"
+#include "MPU6050.h"
+#include "TOFs.h"
+#include "Servo.h"
+#include "Core.h"
+#include <Eigen/Dense.h>
+#include "platform/PlatformMutex.h"
+
+typedef Eigen::Matrix<float, 4, 4> Matrix4f;
+typedef Eigen::Matrix<float, 3, 3> Matrix3f;
+
+class Rover
+{
+public:
+    Rover();    
+    
+    void setRoverVelocity(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, float &torM4);
+    void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4);
+    
+    void setParameters(Eigen::VectorXd roverParameters);
+    
+    void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx);
+
+    void setCentralJointsAngle(float act1, float act2);
+    void calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu);    
+    
+    void computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx);    
+    
+    float computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd);
+
+    void initializeImu();
+    
+    void initializeTofs();
+    
+    void acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance);
+
+    
+
+private:
+
+    IONMcMotors *ionMcFront;
+    IONMcMotors *ionMcRetro;
+    
+    Servo *frontActuonix;
+    Servo *retroActuonix;
+    
+    MPU6050 *mpu;
+         
+    TOFs *tofs;
+    
+    float r_frontWheel;
+    float r_retroWheel;
+    float pipeCurve_I;
+    float pipeCurve_E;
+    float pipeCurve_M;
+    float retroFrontCentralDistance;
+    float wheelsCntactPointDistanceFromPipeCenter;
+
+    //Imu variables
+   
+    float accBias[3];
+    float gyroBias[3];   
+    int pitch_d;
+    float pitch;
+    float g_x_old;
+
+    int forward_vel;
+    
+    int jointFront;
+    int jointRetro;
+    int enableStab;
+    int enableCurv;    
+    
+    float time;
+    float vels_a;
+    float vels_m;
+ 
+    int pipeDir;
+        
+    Eigen::Matrix<float, 4, 3> S;    
+    Eigen::Matrix<float, 3, 4> S_inv;    
+    
+    float frontPos;
+    float retroPos;
+
+    //private functions
+    float deg2rad(float deg);
+    float centralJoint2actuonix(float jointAngle);
+    void calibrateImu();
+    void updateRoverKin(float pipe_radius, int pipeDir);
+    
+    void setState(int state);
+
+    int get_forward_vel();
+    //PlatformMutex eth_mutex;
+    
+    float roverFrontDistance;
+    float roverRetroDistance;
+
+};
+
+#endif
\ No newline at end of file