Diff: Robots/Rover/Rover_omni_h.txt

Revision:

4:3f22193053d0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Rover/Rover_omni_h.txt	Wed Jan 08 11:05:36 2020 +0000
@@ -0,0 +1,172 @@
+#ifndef ROVER_H
+#define ROVER_H
+#include "mbed.h"
+#include "IONMcMotors.h"
+#include "Servo.h"
+#include "Core.h"
+#include "MPU6050.h"
+#include "platform/PlatformMutex.h"
+
+#include "TOFs.h"
+
+#include "Eth_tcp.h"
+
+
+class Rover
+{
+public:
+    Rover();    
+    
+    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 getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro);
+
+
+    void setCentralJointsAngle(float act1, float act2);
+    void calcImuAngles(float& pitch, float& roll, float dtImu);
+    
+    int get_forward_vel();
+    int get_pitch();
+    int get_jointFront();
+    int get_jointRetro();
+    
+    void ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) ;
+    
+    int getEthState(void);
+
+    
+    void startEthComunication();
+    
+    void initializeImu();
+    
+    void initializeTofs();
+    void acquireTofs(float &frontDistance, float &retroDistance);
+    
+    void computeCentralJointsFromTofs();
+
+
+
+private:
+
+    IONMcMotors *ionMcMeccanum;
+    IONMcMotors *ionMcOmni;
+    
+    Servo *meccanumActuonix;
+    Servo *omniActuonix;
+    
+    MPU6050 *mpu;
+    
+    Eth_tcp *eth_tcp;
+    
+    Thread EthThread;   
+    
+    distanceSensors *distance_sensors;
+
+    int ionMcBoudRate;
+    
+    int meccanumMotorGearBoxRatio;
+    int omniMotorGearBoxRatio;
+    
+    int meccanumBoardAddress;
+    int omniBoardAddress;
+    
+    float meccanumTransmissionRatio;
+    float omniTransmissionRatio;
+    
+    int meccanumEncoderPulse;
+    int omniEncoderPulse;
+    
+    float meccanumKt;
+    float omniKt;
+    
+    float r_meccanumWheel;
+    float r_omniWheel;
+
+    float wheelsCntactPointDistanceFromPipeCenter;
+    float omniMeccanumCentralDistance;
+    
+    //Imu variables
+    int16_t ax, ay, az;
+    int16_t gx, gy, gz;
+    
+    float a_x,a_y,a_z;
+    float g_x, g_y, g_z;
+    
+    int16_t FS_a;   
+    float FS_g;    
+    
+    PlatformMutex eth_mutex;
+    
+    
+    float accBias[3];
+    float gyroBias[3];   
+    
+    
+
+    float pitchAcc;
+    float rollAcc;
+    float pitchAcc_p;
+    float rollAcc_p;
+    float pitch_integrated;
+    float roll_integrated;
+    
+        
+    int forward_vel;
+    int pitch_d;
+    
+    int jointFront;
+    int jointRetro;
+    int enableStab;
+    int enableCurv;    
+    
+    float time;
+    float pitch;
+    float vels_a;
+    float vels_m;
+    
+    float pipeCurve_I;
+    float pipeCurve_E;
+    float pipeCurve_M;    
+    int pipeDir;
+    
+    double eth_time_out; //ms
+    
+    int eth_state;
+    double eth_time;    
+    double eth_time_sample_received;
+    
+    Eigen::Matrix3f S;    
+    Eigen::Matrix3f S_inv;    
+    //private functions
+    float deg2rad(float deg);
+    float centralJoint2actuonix(float jointAngle);
+    void calibrateImu();
+    void updateRoverKin(float pipe_radius, int pipeDir);
+    
+    void setState(int state);
+    
+    
+    Timer comunicationTimer;  
+    
+    char cmd;
+    MyBuffer <int> vec_to_send;
+    MyBuffer <int> recv_vec;
+    double t_p;
+    double t;
+    int n_of_int;
+    bool eth_status;
+    
+    bool sock_open;
+    
+
+    
+    float frontPos;
+    float retroPos;
+
+    
+};
+
+#endif
\ No newline at end of file