a
Rover.h@0:65943c77d1dc, 2021-10-04 (annotated)
- Committer:
- marcodesilva
- Date:
- Mon Oct 04 12:52:17 2021 +0000
- Revision:
- 0:65943c77d1dc
a;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
marcodesilva | 0:65943c77d1dc | 1 | #ifndef ROVER_H |
marcodesilva | 0:65943c77d1dc | 2 | #define ROVER_H |
marcodesilva | 0:65943c77d1dc | 3 | #include "mbed.h" |
marcodesilva | 0:65943c77d1dc | 4 | |
marcodesilva | 0:65943c77d1dc | 5 | #include "controlDefineVariables.h" |
marcodesilva | 0:65943c77d1dc | 6 | |
marcodesilva | 0:65943c77d1dc | 7 | #include "IONMcMotors.h" |
marcodesilva | 0:65943c77d1dc | 8 | #include "MPU6050.h" |
marcodesilva | 0:65943c77d1dc | 9 | #include "TOFs.h" |
marcodesilva | 0:65943c77d1dc | 10 | #include "Servo.h" |
marcodesilva | 0:65943c77d1dc | 11 | #include "Core.h" |
marcodesilva | 0:65943c77d1dc | 12 | #include <Eigen/Dense.h> |
marcodesilva | 0:65943c77d1dc | 13 | #include "platform/PlatformMutex.h" |
marcodesilva | 0:65943c77d1dc | 14 | |
marcodesilva | 0:65943c77d1dc | 15 | typedef Eigen::Matrix<float, 4, 4> Matrix4f; |
marcodesilva | 0:65943c77d1dc | 16 | typedef Eigen::Matrix<float, 3, 3> Matrix3f; |
marcodesilva | 0:65943c77d1dc | 17 | |
marcodesilva | 0:65943c77d1dc | 18 | class Rover |
marcodesilva | 0:65943c77d1dc | 19 | { |
marcodesilva | 0:65943c77d1dc | 20 | public: |
marcodesilva | 0:65943c77d1dc | 21 | Rover(); |
marcodesilva | 0:65943c77d1dc | 22 | |
marcodesilva | 0:65943c77d1dc | 23 | void setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration); |
marcodesilva | 0:65943c77d1dc | 24 | void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius); |
marcodesilva | 0:65943c77d1dc | 25 | void getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4); |
marcodesilva | 0:65943c77d1dc | 26 | void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4); |
marcodesilva | 0:65943c77d1dc | 27 | |
marcodesilva | 0:65943c77d1dc | 28 | void setParameters(Eigen::VectorXd roverParameters); |
marcodesilva | 0:65943c77d1dc | 29 | |
marcodesilva | 0:65943c77d1dc | 30 | void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx); |
marcodesilva | 0:65943c77d1dc | 31 | |
marcodesilva | 0:65943c77d1dc | 32 | void setCentralJointsAngle(float act1, float act2); |
marcodesilva | 0:65943c77d1dc | 33 | void calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu); |
marcodesilva | 0:65943c77d1dc | 34 | |
marcodesilva | 0:65943c77d1dc | 35 | void computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx); |
marcodesilva | 0:65943c77d1dc | 36 | |
marcodesilva | 0:65943c77d1dc | 37 | float computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd); |
marcodesilva | 0:65943c77d1dc | 38 | |
marcodesilva | 0:65943c77d1dc | 39 | void initializeImu(); |
marcodesilva | 0:65943c77d1dc | 40 | |
marcodesilva | 0:65943c77d1dc | 41 | void initializeTofs(); |
marcodesilva | 0:65943c77d1dc | 42 | |
marcodesilva | 0:65943c77d1dc | 43 | void acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance); |
marcodesilva | 0:65943c77d1dc | 44 | |
marcodesilva | 0:65943c77d1dc | 45 | |
marcodesilva | 0:65943c77d1dc | 46 | |
marcodesilva | 0:65943c77d1dc | 47 | private: |
marcodesilva | 0:65943c77d1dc | 48 | |
marcodesilva | 0:65943c77d1dc | 49 | IONMcMotors *ionMcFront; |
marcodesilva | 0:65943c77d1dc | 50 | IONMcMotors *ionMcRetro; |
marcodesilva | 0:65943c77d1dc | 51 | |
marcodesilva | 0:65943c77d1dc | 52 | Servo *frontActuonix; |
marcodesilva | 0:65943c77d1dc | 53 | Servo *retroActuonix; |
marcodesilva | 0:65943c77d1dc | 54 | |
marcodesilva | 0:65943c77d1dc | 55 | MPU6050 *mpu; |
marcodesilva | 0:65943c77d1dc | 56 | |
marcodesilva | 0:65943c77d1dc | 57 | TOFs *tofs; |
marcodesilva | 0:65943c77d1dc | 58 | |
marcodesilva | 0:65943c77d1dc | 59 | float r_frontWheel; |
marcodesilva | 0:65943c77d1dc | 60 | float r_retroWheel; |
marcodesilva | 0:65943c77d1dc | 61 | float pipeCurve_I; |
marcodesilva | 0:65943c77d1dc | 62 | float pipeCurve_E; |
marcodesilva | 0:65943c77d1dc | 63 | float pipeCurve_M; |
marcodesilva | 0:65943c77d1dc | 64 | float retroFrontCentralDistance; |
marcodesilva | 0:65943c77d1dc | 65 | float wheelsCntactPointDistanceFromPipeCenter; |
marcodesilva | 0:65943c77d1dc | 66 | |
marcodesilva | 0:65943c77d1dc | 67 | //Imu variables |
marcodesilva | 0:65943c77d1dc | 68 | |
marcodesilva | 0:65943c77d1dc | 69 | float accBias[3]; |
marcodesilva | 0:65943c77d1dc | 70 | float gyroBias[3]; |
marcodesilva | 0:65943c77d1dc | 71 | int pitch_d; |
marcodesilva | 0:65943c77d1dc | 72 | float pitch; |
marcodesilva | 0:65943c77d1dc | 73 | float g_x_old; |
marcodesilva | 0:65943c77d1dc | 74 | |
marcodesilva | 0:65943c77d1dc | 75 | int forward_vel; |
marcodesilva | 0:65943c77d1dc | 76 | |
marcodesilva | 0:65943c77d1dc | 77 | int jointFront; |
marcodesilva | 0:65943c77d1dc | 78 | int jointRetro; |
marcodesilva | 0:65943c77d1dc | 79 | int enableStab; |
marcodesilva | 0:65943c77d1dc | 80 | int enableCurv; |
marcodesilva | 0:65943c77d1dc | 81 | |
marcodesilva | 0:65943c77d1dc | 82 | float time; |
marcodesilva | 0:65943c77d1dc | 83 | float vels_a; |
marcodesilva | 0:65943c77d1dc | 84 | float vels_m; |
marcodesilva | 0:65943c77d1dc | 85 | |
marcodesilva | 0:65943c77d1dc | 86 | int pipeDir; |
marcodesilva | 0:65943c77d1dc | 87 | |
marcodesilva | 0:65943c77d1dc | 88 | Eigen::Matrix<float, 4, 3> S; |
marcodesilva | 0:65943c77d1dc | 89 | Eigen::Matrix<float, 3, 4> S_inv; |
marcodesilva | 0:65943c77d1dc | 90 | |
marcodesilva | 0:65943c77d1dc | 91 | float frontPos; |
marcodesilva | 0:65943c77d1dc | 92 | float retroPos; |
marcodesilva | 0:65943c77d1dc | 93 | |
marcodesilva | 0:65943c77d1dc | 94 | //private functions |
marcodesilva | 0:65943c77d1dc | 95 | float deg2rad(float deg); |
marcodesilva | 0:65943c77d1dc | 96 | float centralJoint2actuonix(float jointAngle); |
marcodesilva | 0:65943c77d1dc | 97 | void calibrateImu(); |
marcodesilva | 0:65943c77d1dc | 98 | void updateRoverKin(float pipe_radius, int pipeDir); |
marcodesilva | 0:65943c77d1dc | 99 | |
marcodesilva | 0:65943c77d1dc | 100 | void setState(int state); |
marcodesilva | 0:65943c77d1dc | 101 | |
marcodesilva | 0:65943c77d1dc | 102 | int get_forward_vel(); |
marcodesilva | 0:65943c77d1dc | 103 | //PlatformMutex eth_mutex; |
marcodesilva | 0:65943c77d1dc | 104 | |
marcodesilva | 0:65943c77d1dc | 105 | float roverFrontDistance; |
marcodesilva | 0:65943c77d1dc | 106 | float roverRetroDistance; |
marcodesilva | 0:65943c77d1dc | 107 | |
marcodesilva | 0:65943c77d1dc | 108 | }; |
marcodesilva | 0:65943c77d1dc | 109 | |
marcodesilva | 0:65943c77d1dc | 110 | #endif |