PRISMA Lab / Mbed OS Hyfliers_Completo_testato

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PwmIn IONMcMotor MPU6050 Eigen ROVER

Fork of Hyfliers_Completo_testato by Marco De Silva

Robots/Rover/Rover_omni_h.txt@5:362759e42070, 2020-01-08 (annotated)

Committer:
anfontanelli
Date:
Wed Jan 08 11:12:51 2020 +0000
Revision:
5:362759e42070
Parent:
4:3f22193053d0 
rev1;

Who changed what in which revision?

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