PRISMA Lab / Mbed OS Hyfliers_Completo_testato

Clone per testare Mbed studio

Dependencies:   PwmIn IONMcMotor MPU6050 Eigen ROVER

Fork of Hyfliers_Completo_testato by Marco De Silva

Hyfliers_robot_eth_msgs.h@11:d93fbd8e02e9, 23 months ago (annotated)

Committer:
aledek
Date:
Wed Jul 27 09:37:11 2022 +0000
Revision:
11:d93fbd8e02e9
Parent:
6:584653235830 
first commit,

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anfontanelli 6:584653235830 1 //Communication messages for ethernet protocol
anfontanelli 6:584653235830 2
anfontanelli 6:584653235830 3 enum RobotCmd {
anfontanelli 6:584653235830 4 IDLE_STATE,
anfontanelli 6:584653235830 5 DISABLE_MOTORS,
anfontanelli 6:584653235830 6 ENABLE_ROVER,
anfontanelli 6:584653235830 7 ENABLE_ARM,
anfontanelli 6:584653235830 8 ENABLE_TOOL,
anfontanelli 6:584653235830 9 ARM_JOINT_CONTROL,
anfontanelli 6:584653235830 10 ARM_KINEMATIC_CONTROL_FIRST_ORDER,
anfontanelli 6:584653235830 11 ARM_KINEMATIC_CONTROL_SECOND_ORDER,
anfontanelli 6:584653235830 12 CALIBRATE_ROVER_IMU,
anfontanelli 6:584653235830 13 CALIBRATE_ROVER_LASERS,
anfontanelli 6:584653235830 14 CALIBRATE_ARM_LASERS,
anfontanelli 6:584653235830 15 GET_CURRENT_ROBOT_STATE,
anfontanelli 6:584653235830 16 RESTART_BOARD,
anfontanelli 6:584653235830 17 STOP_ALL
anfontanelli 6:584653235830 18 };
anfontanelli 6:584653235830 19
anfontanelli 6:584653235830 20
anfontanelli 6:584653235830 21 enum Status {
anfontanelli 6:584653235830 22 ETH_CONNECTED,
anfontanelli 6:584653235830 23 ETH_SRV_ACCEPTED,
anfontanelli 6:584653235830 24 ETH_LOST_CONNECTION,
anfontanelli 6:584653235830 25 ETH_WRONG_RECV_SIZE,
anfontanelli 6:584653235830 26 ETH_CHECKSUM_ERROR,
anfontanelli 6:584653235830 27 ETH_NEW_CNF,
anfontanelli 6:584653235830 28 ETH_NEW_CMD,
anfontanelli 6:584653235830 29 ETH_NEW_CMD_CURRENT_VAL_REQUEST,
anfontanelli 6:584653235830 30 ETH_ERROR_SENDING_DATA,
anfontanelli 6:584653235830 31 ETH_IDLE,
anfontanelli 6:584653235830 32 ROBOT_SAFETY_MODE
anfontanelli 6:584653235830 33 };
anfontanelli 6:584653235830 34
anfontanelli 6:584653235830 35
anfontanelli 6:584653235830 36 enum ControlMode {
anfontanelli 6:584653235830 37 IDLE_MODE,
anfontanelli 6:584653235830 38 GET_ROBOT_STATE_MODE,
anfontanelli 6:584653235830 39 JOINT_CONTROL_MODE,
anfontanelli 6:584653235830 40 KINEMATIC_CONTROL_FIRST_ORDER_MODE,
anfontanelli 6:584653235830 41 KINEMATIC_CONTROL_SECOND_ORDER_MODE,
anfontanelli 6:584653235830 42 ADMITTANCE_CONTROL_MODE,
anfontanelli 6:584653235830 43 HYBRID_CONTROL_MODE,
anfontanelli 6:584653235830 44 HYBRID_ADMITTANCE_CONTROL_MODE,
anfontanelli 6:584653235830 45 SAFETY_MODE
anfontanelli 6:584653235830 46 };
anfontanelli 6:584653235830 47
anfontanelli 6:584653235830 48 enum PacketId {
anfontanelli 6:584653235830 49 ID_config,
anfontanelli 6:584653235830 50 ID_receive_command_send_prec_val,
anfontanelli 6:584653235830 51 ID_receive_command_send_current_val
anfontanelli 6:584653235830 52 };
anfontanelli 6:584653235830 53
anfontanelli 6:584653235830 54
anfontanelli 6:584653235830 55 struct DebugVector
anfontanelli 6:584653235830 56 {
anfontanelli 6:584653235830 57 float d1;
anfontanelli 6:584653235830 58 float d2;
anfontanelli 6:584653235830 59 float d3;
anfontanelli 6:584653235830 60 float d4;
anfontanelli 6:584653235830 61 float d5;
anfontanelli 6:584653235830 62 float d6;
anfontanelli 6:584653235830 63 float d7;
anfontanelli 6:584653235830 64 float d8;
anfontanelli 6:584653235830 65 float d9;
anfontanelli 6:584653235830 66 float d10;
anfontanelli 6:584653235830 67 float d11;
anfontanelli 6:584653235830 68 float d12;
anfontanelli 6:584653235830 69 float d13;
anfontanelli 6:584653235830 70 float d14;
anfontanelli 6:584653235830 71 float d15;
anfontanelli 6:584653235830 72 float d16;
anfontanelli 6:584653235830 73 float d17;
anfontanelli 6:584653235830 74 float d18;
anfontanelli 6:584653235830 75 float d19;
anfontanelli 6:584653235830 76 float d20;
anfontanelli 6:584653235830 77 float d21;
anfontanelli 6:584653235830 78 float d22;
anfontanelli 6:584653235830 79 float d23;
anfontanelli 6:584653235830 80 float d24;
anfontanelli 6:584653235830 81
anfontanelli 6:584653235830 82 };
anfontanelli 6:584653235830 83
anfontanelli 6:584653235830 84 struct DataToTheBoard
anfontanelli 6:584653235830 85 {
anfontanelli 6:584653235830 86 float roverForwardVel;
anfontanelli 6:584653235830 87 float roverZVel; //speed around z axis (used to correct the rover orientation on the pipe)
anfontanelli 6:584653235830 88 float roverFrontJointAngle;
anfontanelli 6:584653235830 89 float roverRetroJointAngle;
anfontanelli 6:584653235830 90 //Arm position in base frame
anfontanelli 6:584653235830 91 float armX;
anfontanelli 6:584653235830 92 float armY;
anfontanelli 6:584653235830 93 float armZ;
anfontanelli 6:584653235830 94 //Arm oriantation in base frame
anfontanelli 6:584653235830 95 float armQ0;
anfontanelli 6:584653235830 96 float armQ1;
anfontanelli 6:584653235830 97 float armQ2;
anfontanelli 6:584653235830 98 float armQ3;
anfontanelli 6:584653235830 99
anfontanelli 6:584653235830 100 float drone_roll;
anfontanelli 6:584653235830 101 float drone_pitch;
anfontanelli 6:584653235830 102
anfontanelli 6:584653235830 103 float jointPos1;
anfontanelli 6:584653235830 104 float jointPos2;
anfontanelli 6:584653235830 105 float jointPos3;
anfontanelli 6:584653235830 106 float jointPos4;
anfontanelli 6:584653235830 107 float jointPos5;
anfontanelli 6:584653235830 108 float toolOrientAroundPipeAxis;
anfontanelli 6:584653235830 109 float roverLongitudinalPosition;
anfontanelli 6:584653235830 110 bool toolEmatClose;
anfontanelli 6:584653235830 111
anfontanelli 6:584653235830 112 };
anfontanelli 6:584653235830 113
anfontanelli 6:584653235830 114
anfontanelli 6:584653235830 115 struct ConfigDataToTheBoard
anfontanelli 6:584653235830 116 {
anfontanelli 6:584653235830 117 float wdTime;
anfontanelli 6:584653235830 118
anfontanelli 6:584653235830 119 bool debug;
anfontanelli 6:584653235830 120
anfontanelli 6:584653235830 121 float dtBoard;
anfontanelli 6:584653235830 122 float r_frontWheel;
anfontanelli 6:584653235830 123 float r_retroWheel;
anfontanelli 6:584653235830 124 float pipeCurve_I;
anfontanelli 6:584653235830 125 float pipeCurve_E;
anfontanelli 6:584653235830 126 float pipeCurve_M;
anfontanelli 6:584653235830 127 float retroFrontCentralDistance;
anfontanelli 6:584653235830 128 float wheelsCntactPointDistanceFromPipeCenter;
anfontanelli 6:584653235830 129 float pipeRadious;
anfontanelli 6:584653235830 130 float Kp_stabilization;
anfontanelli 6:584653235830 131 float Kd_stabilization;
anfontanelli 6:584653235830 132
anfontanelli 6:584653235830 133
anfontanelli 6:584653235830 134 float Kp_zero_torque;
anfontanelli 6:584653235830 135 float Ki_zero_torque;
anfontanelli 6:584653235830 136
anfontanelli 6:584653235830 137 float wheelsAcceleration;
anfontanelli 6:584653235830 138
anfontanelli 6:584653235830 139 float jacobianInversionWeight1;
anfontanelli 6:584653235830 140 float jacobianInversionWeight2;
anfontanelli 6:584653235830 141 float jacobianInversionWeight3;
anfontanelli 6:584653235830 142 float jacobianInversionWeight4;
anfontanelli 6:584653235830 143 float jacobianInversionWeight5;
anfontanelli 6:584653235830 144 float jacobianInversionWeight6;
anfontanelli 6:584653235830 145 float jacobianInversionWeight7;
anfontanelli 6:584653235830 146 float jacobianInversionWeight8;
anfontanelli 6:584653235830 147
anfontanelli 6:584653235830 148
anfontanelli 6:584653235830 149 // arm DH
anfontanelli 6:584653235830 150 float a3;
anfontanelli 6:584653235830 151 float a4;
anfontanelli 6:584653235830 152 float a5;
anfontanelli 6:584653235830 153 float a5i;
anfontanelli 6:584653235830 154 float a6;
anfontanelli 6:584653235830 155 float d7;
anfontanelli 6:584653235830 156 // drone distances
anfontanelli 6:584653235830 157 float x_0_r2;
anfontanelli 6:584653235830 158 float y_0_r2;
anfontanelli 6:584653235830 159 float z_0_r2;
anfontanelli 6:584653235830 160
anfontanelli 6:584653235830 161 float massr;
anfontanelli 6:584653235830 162 float mpxr ;
anfontanelli 6:584653235830 163 float mpyr ;
anfontanelli 6:584653235830 164 float mpzr ;
anfontanelli 6:584653235830 165 float mass1;
anfontanelli 6:584653235830 166 float mpx1 ;
anfontanelli 6:584653235830 167 float mpy1 ;
anfontanelli 6:584653235830 168 float mpz1 ;
anfontanelli 6:584653235830 169 float mass2;
anfontanelli 6:584653235830 170 float mpx2 ;
anfontanelli 6:584653235830 171 float mpy2 ;
anfontanelli 6:584653235830 172 float mpz2 ;
anfontanelli 6:584653235830 173 float mass3;
anfontanelli 6:584653235830 174 float mpx3 ;
anfontanelli 6:584653235830 175 float mpy3 ;
anfontanelli 6:584653235830 176 float mpz3 ;
anfontanelli 6:584653235830 177 float mass4;
anfontanelli 6:584653235830 178 float mpx4 ;
anfontanelli 6:584653235830 179 float mpy4 ;
anfontanelli 6:584653235830 180 float mpz4 ;
anfontanelli 6:584653235830 181 float mass5;
anfontanelli 6:584653235830 182 float mpx5 ;
anfontanelli 6:584653235830 183 float mpy5 ;
anfontanelli 6:584653235830 184 float mpz5 ;
anfontanelli 6:584653235830 185 float mass6;
anfontanelli 6:584653235830 186 float mpx6 ;
anfontanelli 6:584653235830 187 float mpy6 ;
anfontanelli 6:584653235830 188 float mpz6 ;
anfontanelli 6:584653235830 189
anfontanelli 6:584653235830 190 float mass_battery;
anfontanelli 6:584653235830 191 float mpx_battery;
anfontanelli 6:584653235830 192 float mpz_battery;
anfontanelli 6:584653235830 193
anfontanelli 6:584653235830 194 float kp_first_order;
anfontanelli 6:584653235830 195 float ko_first_order;
anfontanelli 6:584653235830 196 float kp_second_order;
anfontanelli 6:584653235830 197 float ko_second_order;
anfontanelli 6:584653235830 198 float dp_second_order;
anfontanelli 6:584653235830 199 float do_second_order;
anfontanelli 6:584653235830 200
anfontanelli 6:584653235830 201 float jointsMaxVel;
anfontanelli 6:584653235830 202 float jointsMaxAcc;
anfontanelli 6:584653235830 203 float jointsFilterOmega;
anfontanelli 6:584653235830 204 float jointsFilterZita;
anfontanelli 6:584653235830 205 float velAccSafetyFactor;
anfontanelli 6:584653235830 206
anfontanelli 6:584653235830 207 float maxCartPos_x;
anfontanelli 6:584653235830 208 float maxCartPos_y;
anfontanelli 6:584653235830 209 float maxCartPos_z;
anfontanelli 6:584653235830 210
anfontanelli 6:584653235830 211 float minCartPos_x;
anfontanelli 6:584653235830 212 float minCartPos_y;
anfontanelli 6:584653235830 213 float minCartPos_z;
anfontanelli 6:584653235830 214
anfontanelli 6:584653235830 215
anfontanelli 6:584653235830 216
anfontanelli 6:584653235830 217 };
anfontanelli 6:584653235830 218
anfontanelli 6:584653235830 219 struct DataFromTheBoard
anfontanelli 6:584653235830 220 {
anfontanelli 6:584653235830 221 float roverForwardVel;
anfontanelli 6:584653235830 222 float roverStabVel;
anfontanelli 6:584653235830 223 float roverZVel; //speed around z axis (used to correct the rover orientation on the pipe)
anfontanelli 6:584653235830 224 float roverPitchAngle;
anfontanelli 6:584653235830 225 float roverOdom;
anfontanelli 6:584653235830 226 float roverLaserFrontDx;
anfontanelli 6:584653235830 227 float roverLaserFrontSx;
anfontanelli 6:584653235830 228 float roverLaserRetroDx;
anfontanelli 6:584653235830 229 float roverLaserRetroSx;
anfontanelli 6:584653235830 230
anfontanelli 6:584653235830 231 //Arm position in base frame
anfontanelli 6:584653235830 232 float misTebX;
anfontanelli 6:584653235830 233 float misTebY;
anfontanelli 6:584653235830 234 float misTebZ;
anfontanelli 6:584653235830 235 //Arm oriantation in base frame
anfontanelli 6:584653235830 236 float misTebQ0;
anfontanelli 6:584653235830 237 float misTebQ1;
anfontanelli 6:584653235830 238 float misTebQ2;
anfontanelli 6:584653235830 239 float misTebQ3;
anfontanelli 6:584653235830 240
anfontanelli 6:584653235830 241 float cmdJointPos1;
anfontanelli 6:584653235830 242 float cmdJointPos2;
anfontanelli 6:584653235830 243 float cmdJointPos3;
anfontanelli 6:584653235830 244 float cmdJointPos4;
anfontanelli 6:584653235830 245 float cmdJointPos5;
anfontanelli 6:584653235830 246 float cmdJointPos6;
anfontanelli 6:584653235830 247
anfontanelli 6:584653235830 248 float estRoverJointPos1;
anfontanelli 6:584653235830 249 float estRoverJointPos2;
anfontanelli 6:584653235830 250
anfontanelli 6:584653235830 251 float misArmJointPos1;
anfontanelli 6:584653235830 252 float misArmJointPos2;
anfontanelli 6:584653235830 253 float misArmJointPos3;
anfontanelli 6:584653235830 254 float misArmJointPos4;
anfontanelli 6:584653235830 255 float misArmJointPos5;
anfontanelli 6:584653235830 256 float misArmJointPos6;
anfontanelli 6:584653235830 257
anfontanelli 6:584653235830 258 float droneFx;
anfontanelli 6:584653235830 259 float droneFy;
anfontanelli 6:584653235830 260 float droneFz;
anfontanelli 6:584653235830 261 float droneTx;
anfontanelli 6:584653235830 262 float droneTy;
anfontanelli 6:584653235830 263 float droneTz;
anfontanelli 6:584653235830 264
anfontanelli 6:584653235830 265 float toolOrientAroundPipeAxis;
anfontanelli 6:584653235830 266
anfontanelli 6:584653235830 267 float toolLaserFrontDx;
anfontanelli 6:584653235830 268 float toolLaserFrontSx;
anfontanelli 6:584653235830 269 float toolLaserRetroDx;
anfontanelli 6:584653235830 270 float toolLaserRetroSx;
anfontanelli 6:584653235830 271
anfontanelli 6:584653235830 272 };
anfontanelli 6:584653235830 273
anfontanelli 6:584653235830 274 struct ComandMsg
anfontanelli 6:584653235830 275 {
anfontanelli 6:584653235830 276 int pId;
anfontanelli 6:584653235830 277 RobotCmd Cmd;
anfontanelli 6:584653235830 278 DataToTheBoard Data;
anfontanelli 6:584653235830 279 uint8_t Checksum;
anfontanelli 6:584653235830 280 };
anfontanelli 6:584653235830 281
anfontanelli 6:584653235830 282 struct ConfigMsg
anfontanelli 6:584653235830 283 {
anfontanelli 6:584653235830 284 int pId;
anfontanelli 6:584653235830 285 ConfigDataToTheBoard Data;
anfontanelli 6:584653235830 286 uint8_t Checksum;
anfontanelli 6:584653235830 287 };
anfontanelli 6:584653235830 288
anfontanelli 6:584653235830 289
anfontanelli 6:584653235830 290 struct ResponseMsg
anfontanelli 6:584653235830 291 {
anfontanelli 6:584653235830 292 int StatusData;
anfontanelli 6:584653235830 293 int ControlModeData;
anfontanelli 6:584653235830 294 DataFromTheBoard Data;
anfontanelli 6:584653235830 295 DebugVector Dbv;
anfontanelli 6:584653235830 296
anfontanelli 6:584653235830 297 };
anfontanelli 6:584653235830 298
anfontanelli 6:584653235830 299