a
old/Rover_omni_h.txt@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 | #include "IONMcMotors.h" |
marcodesilva | 0:65943c77d1dc | 5 | #include "Servo.h" |
marcodesilva | 0:65943c77d1dc | 6 | #include "Core.h" |
marcodesilva | 0:65943c77d1dc | 7 | #include "MPU6050.h" |
marcodesilva | 0:65943c77d1dc | 8 | #include "platform/PlatformMutex.h" |
marcodesilva | 0:65943c77d1dc | 9 | |
marcodesilva | 0:65943c77d1dc | 10 | #include "TOFs.h" |
marcodesilva | 0:65943c77d1dc | 11 | |
marcodesilva | 0:65943c77d1dc | 12 | #include "Eth_tcp.h" |
marcodesilva | 0:65943c77d1dc | 13 | |
marcodesilva | 0:65943c77d1dc | 14 | |
marcodesilva | 0:65943c77d1dc | 15 | class Rover |
marcodesilva | 0:65943c77d1dc | 16 | { |
marcodesilva | 0:65943c77d1dc | 17 | public: |
marcodesilva | 0:65943c77d1dc | 18 | Rover(); |
marcodesilva | 0:65943c77d1dc | 19 | |
marcodesilva | 0:65943c77d1dc | 20 | void setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration); |
marcodesilva | 0:65943c77d1dc | 21 | void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius); |
marcodesilva | 0:65943c77d1dc | 22 | void getMotorsTorque(float &torM1, float &torM2, float &torM3); |
marcodesilva | 0:65943c77d1dc | 23 | void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3); |
marcodesilva | 0:65943c77d1dc | 24 | |
marcodesilva | 0:65943c77d1dc | 25 | void getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro); |
marcodesilva | 0:65943c77d1dc | 26 | |
marcodesilva | 0:65943c77d1dc | 27 | |
marcodesilva | 0:65943c77d1dc | 28 | void setCentralJointsAngle(float act1, float act2); |
marcodesilva | 0:65943c77d1dc | 29 | void calcImuAngles(float& pitch, float& roll, float dtImu); |
marcodesilva | 0:65943c77d1dc | 30 | |
marcodesilva | 0:65943c77d1dc | 31 | int get_forward_vel(); |
marcodesilva | 0:65943c77d1dc | 32 | int get_pitch(); |
marcodesilva | 0:65943c77d1dc | 33 | int get_jointFront(); |
marcodesilva | 0:65943c77d1dc | 34 | int get_jointRetro(); |
marcodesilva | 0:65943c77d1dc | 35 | |
marcodesilva | 0:65943c77d1dc | 36 | void ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) ; |
marcodesilva | 0:65943c77d1dc | 37 | |
marcodesilva | 0:65943c77d1dc | 38 | int getEthState(void); |
marcodesilva | 0:65943c77d1dc | 39 | |
marcodesilva | 0:65943c77d1dc | 40 | |
marcodesilva | 0:65943c77d1dc | 41 | void startEthComunication(); |
marcodesilva | 0:65943c77d1dc | 42 | |
marcodesilva | 0:65943c77d1dc | 43 | void initializeImu(); |
marcodesilva | 0:65943c77d1dc | 44 | |
marcodesilva | 0:65943c77d1dc | 45 | void initializeTofs(); |
marcodesilva | 0:65943c77d1dc | 46 | void acquireTofs(float &frontDistance, float &retroDistance); |
marcodesilva | 0:65943c77d1dc | 47 | |
marcodesilva | 0:65943c77d1dc | 48 | void computeCentralJointsFromTofs(); |
marcodesilva | 0:65943c77d1dc | 49 | |
marcodesilva | 0:65943c77d1dc | 50 | |
marcodesilva | 0:65943c77d1dc | 51 | |
marcodesilva | 0:65943c77d1dc | 52 | private: |
marcodesilva | 0:65943c77d1dc | 53 | |
marcodesilva | 0:65943c77d1dc | 54 | IONMcMotors *ionMcMeccanum; |
marcodesilva | 0:65943c77d1dc | 55 | IONMcMotors *ionMcOmni; |
marcodesilva | 0:65943c77d1dc | 56 | |
marcodesilva | 0:65943c77d1dc | 57 | Servo *meccanumActuonix; |
marcodesilva | 0:65943c77d1dc | 58 | Servo *omniActuonix; |
marcodesilva | 0:65943c77d1dc | 59 | |
marcodesilva | 0:65943c77d1dc | 60 | MPU6050 *mpu; |
marcodesilva | 0:65943c77d1dc | 61 | |
marcodesilva | 0:65943c77d1dc | 62 | Eth_tcp *eth_tcp; |
marcodesilva | 0:65943c77d1dc | 63 | |
marcodesilva | 0:65943c77d1dc | 64 | Thread EthThread; |
marcodesilva | 0:65943c77d1dc | 65 | |
marcodesilva | 0:65943c77d1dc | 66 | distanceSensors *distance_sensors; |
marcodesilva | 0:65943c77d1dc | 67 | |
marcodesilva | 0:65943c77d1dc | 68 | int ionMcBoudRate; |
marcodesilva | 0:65943c77d1dc | 69 | |
marcodesilva | 0:65943c77d1dc | 70 | int meccanumMotorGearBoxRatio; |
marcodesilva | 0:65943c77d1dc | 71 | int omniMotorGearBoxRatio; |
marcodesilva | 0:65943c77d1dc | 72 | |
marcodesilva | 0:65943c77d1dc | 73 | int meccanumBoardAddress; |
marcodesilva | 0:65943c77d1dc | 74 | int omniBoardAddress; |
marcodesilva | 0:65943c77d1dc | 75 | |
marcodesilva | 0:65943c77d1dc | 76 | float meccanumTransmissionRatio; |
marcodesilva | 0:65943c77d1dc | 77 | float omniTransmissionRatio; |
marcodesilva | 0:65943c77d1dc | 78 | |
marcodesilva | 0:65943c77d1dc | 79 | int meccanumEncoderPulse; |
marcodesilva | 0:65943c77d1dc | 80 | int omniEncoderPulse; |
marcodesilva | 0:65943c77d1dc | 81 | |
marcodesilva | 0:65943c77d1dc | 82 | float meccanumKt; |
marcodesilva | 0:65943c77d1dc | 83 | float omniKt; |
marcodesilva | 0:65943c77d1dc | 84 | |
marcodesilva | 0:65943c77d1dc | 85 | float r_meccanumWheel; |
marcodesilva | 0:65943c77d1dc | 86 | float r_omniWheel; |
marcodesilva | 0:65943c77d1dc | 87 | |
marcodesilva | 0:65943c77d1dc | 88 | float wheelsCntactPointDistanceFromPipeCenter; |
marcodesilva | 0:65943c77d1dc | 89 | float omniMeccanumCentralDistance; |
marcodesilva | 0:65943c77d1dc | 90 | |
marcodesilva | 0:65943c77d1dc | 91 | //Imu variables |
marcodesilva | 0:65943c77d1dc | 92 | int16_t ax, ay, az; |
marcodesilva | 0:65943c77d1dc | 93 | int16_t gx, gy, gz; |
marcodesilva | 0:65943c77d1dc | 94 | |
marcodesilva | 0:65943c77d1dc | 95 | float a_x,a_y,a_z; |
marcodesilva | 0:65943c77d1dc | 96 | float g_x, g_y, g_z; |
marcodesilva | 0:65943c77d1dc | 97 | |
marcodesilva | 0:65943c77d1dc | 98 | int16_t FS_a; |
marcodesilva | 0:65943c77d1dc | 99 | float FS_g; |
marcodesilva | 0:65943c77d1dc | 100 | |
marcodesilva | 0:65943c77d1dc | 101 | PlatformMutex eth_mutex; |
marcodesilva | 0:65943c77d1dc | 102 | |
marcodesilva | 0:65943c77d1dc | 103 | |
marcodesilva | 0:65943c77d1dc | 104 | float accBias[3]; |
marcodesilva | 0:65943c77d1dc | 105 | float gyroBias[3]; |
marcodesilva | 0:65943c77d1dc | 106 | |
marcodesilva | 0:65943c77d1dc | 107 | |
marcodesilva | 0:65943c77d1dc | 108 | |
marcodesilva | 0:65943c77d1dc | 109 | float pitchAcc; |
marcodesilva | 0:65943c77d1dc | 110 | float rollAcc; |
marcodesilva | 0:65943c77d1dc | 111 | float pitchAcc_p; |
marcodesilva | 0:65943c77d1dc | 112 | float rollAcc_p; |
marcodesilva | 0:65943c77d1dc | 113 | float pitch_integrated; |
marcodesilva | 0:65943c77d1dc | 114 | float roll_integrated; |
marcodesilva | 0:65943c77d1dc | 115 | |
marcodesilva | 0:65943c77d1dc | 116 | |
marcodesilva | 0:65943c77d1dc | 117 | int forward_vel; |
marcodesilva | 0:65943c77d1dc | 118 | int pitch_d; |
marcodesilva | 0:65943c77d1dc | 119 | |
marcodesilva | 0:65943c77d1dc | 120 | int jointFront; |
marcodesilva | 0:65943c77d1dc | 121 | int jointRetro; |
marcodesilva | 0:65943c77d1dc | 122 | int enableStab; |
marcodesilva | 0:65943c77d1dc | 123 | int enableCurv; |
marcodesilva | 0:65943c77d1dc | 124 | |
marcodesilva | 0:65943c77d1dc | 125 | float time; |
marcodesilva | 0:65943c77d1dc | 126 | float pitch; |
marcodesilva | 0:65943c77d1dc | 127 | float vels_a; |
marcodesilva | 0:65943c77d1dc | 128 | float vels_m; |
marcodesilva | 0:65943c77d1dc | 129 | |
marcodesilva | 0:65943c77d1dc | 130 | float pipeCurve_I; |
marcodesilva | 0:65943c77d1dc | 131 | float pipeCurve_E; |
marcodesilva | 0:65943c77d1dc | 132 | float pipeCurve_M; |
marcodesilva | 0:65943c77d1dc | 133 | int pipeDir; |
marcodesilva | 0:65943c77d1dc | 134 | |
marcodesilva | 0:65943c77d1dc | 135 | double eth_time_out; //ms |
marcodesilva | 0:65943c77d1dc | 136 | |
marcodesilva | 0:65943c77d1dc | 137 | int eth_state; |
marcodesilva | 0:65943c77d1dc | 138 | double eth_time; |
marcodesilva | 0:65943c77d1dc | 139 | double eth_time_sample_received; |
marcodesilva | 0:65943c77d1dc | 140 | |
marcodesilva | 0:65943c77d1dc | 141 | Eigen::Matrix3f S; |
marcodesilva | 0:65943c77d1dc | 142 | Eigen::Matrix3f S_inv; |
marcodesilva | 0:65943c77d1dc | 143 | //private functions |
marcodesilva | 0:65943c77d1dc | 144 | float deg2rad(float deg); |
marcodesilva | 0:65943c77d1dc | 145 | float centralJoint2actuonix(float jointAngle); |
marcodesilva | 0:65943c77d1dc | 146 | void calibrateImu(); |
marcodesilva | 0:65943c77d1dc | 147 | void updateRoverKin(float pipe_radius, int pipeDir); |
marcodesilva | 0:65943c77d1dc | 148 | |
marcodesilva | 0:65943c77d1dc | 149 | void setState(int state); |
marcodesilva | 0:65943c77d1dc | 150 | |
marcodesilva | 0:65943c77d1dc | 151 | |
marcodesilva | 0:65943c77d1dc | 152 | Timer comunicationTimer; |
marcodesilva | 0:65943c77d1dc | 153 | |
marcodesilva | 0:65943c77d1dc | 154 | char cmd; |
marcodesilva | 0:65943c77d1dc | 155 | MyBuffer <int> vec_to_send; |
marcodesilva | 0:65943c77d1dc | 156 | MyBuffer <int> recv_vec; |
marcodesilva | 0:65943c77d1dc | 157 | double t_p; |
marcodesilva | 0:65943c77d1dc | 158 | double t; |
marcodesilva | 0:65943c77d1dc | 159 | int n_of_int; |
marcodesilva | 0:65943c77d1dc | 160 | bool eth_status; |
marcodesilva | 0:65943c77d1dc | 161 | |
marcodesilva | 0:65943c77d1dc | 162 | bool sock_open; |
marcodesilva | 0:65943c77d1dc | 163 | |
marcodesilva | 0:65943c77d1dc | 164 | |
marcodesilva | 0:65943c77d1dc | 165 | |
marcodesilva | 0:65943c77d1dc | 166 | float frontPos; |
marcodesilva | 0:65943c77d1dc | 167 | float retroPos; |
marcodesilva | 0:65943c77d1dc | 168 | |
marcodesilva | 0:65943c77d1dc | 169 | |
marcodesilva | 0:65943c77d1dc | 170 | }; |
marcodesilva | 0:65943c77d1dc | 171 | |
marcodesilva | 0:65943c77d1dc | 172 | #endif |