Marco De Silva / ROVER

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?

UserRevisionLine numberNew 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