Hyfliers_robot_eth_msgs.h

00001 //Communication messages for ethernet protocol
00002  
00003 enum RobotCmd {
00004     IDLE_STATE,
00005     DISABLE_MOTORS,
00006     ENABLE_ROVER,
00007     ENABLE_ARM,
00008     ENABLE_TOOL,
00009     ARM_JOINT_CONTROL,
00010     ARM_KINEMATIC_CONTROL_FIRST_ORDER,
00011     ARM_KINEMATIC_CONTROL_SECOND_ORDER,
00012     CALIBRATE_ROVER_IMU,
00013     CALIBRATE_ROVER_LASERS,
00014     CALIBRATE_ARM_LASERS,
00015     GET_CURRENT_ROBOT_STATE,
00016     RESTART_BOARD,
00017     STOP_ALL
00018 };
00019 
00020 
00021 enum Status {
00022     ETH_CONNECTED,
00023     ETH_SRV_ACCEPTED,
00024     ETH_LOST_CONNECTION,
00025     ETH_WRONG_RECV_SIZE,
00026     ETH_CHECKSUM_ERROR,
00027     ETH_NEW_CNF,
00028     ETH_NEW_CMD,
00029     ETH_NEW_CMD_CURRENT_VAL_REQUEST,
00030     ETH_ERROR_SENDING_DATA,
00031     ETH_IDLE,
00032     ROBOT_SAFETY_MODE
00033 };
00034 
00035 
00036 enum ControlMode {
00037     IDLE_MODE,
00038     GET_ROBOT_STATE_MODE,
00039     JOINT_CONTROL_MODE,
00040     KINEMATIC_CONTROL_FIRST_ORDER_MODE,
00041     KINEMATIC_CONTROL_SECOND_ORDER_MODE,
00042     ADMITTANCE_CONTROL_MODE,
00043     HYBRID_CONTROL_MODE,
00044     HYBRID_ADMITTANCE_CONTROL_MODE,
00045     SAFETY_MODE
00046 };
00047 
00048 enum PacketId {
00049     ID_config,
00050     ID_receive_command_send_prec_val,
00051     ID_receive_command_send_current_val
00052 };
00053 
00054 
00055 struct DebugVector 
00056 {
00057     float d1;
00058     float d2;
00059     float d3;
00060     float d4;
00061     float d5;
00062     float d6;
00063     float d7;
00064     float d8;
00065     float d9;
00066     float d10;
00067     float d11;
00068     float d12;
00069     float d13;
00070     float d14;
00071     float d15;
00072     float d16;
00073     float d17;
00074     float d18;
00075     float d19;
00076     float d20;
00077     float d21;
00078     float d22;
00079     float d23;
00080     float d24;
00081 
00082 }; 
00083 
00084 struct DataToTheBoard
00085 {
00086     float roverForwardVel;
00087     float roverZVel; //speed around z axis (used to correct the rover orientation on the pipe)
00088     float roverFrontJointAngle;
00089     float roverRetroJointAngle;
00090     //Arm position in base frame
00091     float armX;
00092     float armY;
00093     float armZ;
00094     //Arm oriantation in base frame
00095     float armQ0;
00096     float armQ1;
00097     float armQ2;
00098     float armQ3;
00099     
00100     float drone_roll;
00101     float drone_pitch;
00102     
00103     float jointPos1;
00104     float jointPos2;
00105     float jointPos3;
00106     float jointPos4;
00107     float jointPos5;
00108     float toolOrientAroundPipeAxis;
00109     float roverLongitudinalPosition;
00110     bool toolEmatClose;
00111 
00112 };
00113 
00114 
00115 struct ConfigDataToTheBoard
00116 {
00117     float wdTime;
00118 
00119     bool debug;
00120     
00121     float dtBoard;
00122     float r_frontWheel;
00123     float r_retroWheel;
00124     float pipeCurve_I;
00125     float pipeCurve_E;
00126     float pipeCurve_M;
00127     float retroFrontCentralDistance;
00128     float wheelsCntactPointDistanceFromPipeCenter;
00129     float pipeRadious;
00130     float Kp_stabilization;
00131     float Kd_stabilization;
00132     
00133         
00134     float Kp_zero_torque;
00135     float Ki_zero_torque;
00136 
00137     float wheelsAcceleration;
00138     
00139     float jacobianInversionWeight1;
00140     float jacobianInversionWeight2;
00141     float jacobianInversionWeight3;
00142     float jacobianInversionWeight4;
00143     float jacobianInversionWeight5;
00144     float jacobianInversionWeight6;
00145     float jacobianInversionWeight7;
00146     float jacobianInversionWeight8;
00147     
00148     
00149         // arm DH
00150     float a3;
00151     float a4;
00152     float a5;
00153     float a5i;
00154     float a6;
00155     float d7;
00156     // drone distances
00157     float x_0_r2;
00158     float y_0_r2;
00159     float z_0_r2;
00160     
00161     float massr; 
00162     float mpxr ;  
00163     float mpyr ; 
00164     float mpzr ; 
00165     float mass1; 
00166     float mpx1 ;  
00167     float mpy1 ; 
00168     float mpz1 ; 
00169     float mass2; 
00170     float mpx2 ;
00171     float mpy2 ;
00172     float mpz2 ;
00173     float mass3; 
00174     float mpx3 ;
00175     float mpy3 ;
00176     float mpz3 ;
00177     float mass4; 
00178     float mpx4 ;
00179     float mpy4 ;
00180     float mpz4 ;
00181     float mass5; 
00182     float mpx5 ;
00183     float mpy5 ;
00184     float mpz5 ;
00185     float mass6; 
00186     float mpx6 ;
00187     float mpy6 ;
00188     float mpz6 ;
00189     
00190     float mass_battery;
00191     float mpx_battery;
00192     float mpz_battery;
00193        
00194     float  kp_first_order;
00195     float  ko_first_order;
00196     float  kp_second_order;
00197     float  ko_second_order;
00198     float  dp_second_order;
00199     float  do_second_order;
00200     
00201     float  jointsMaxVel;
00202     float  jointsMaxAcc;
00203     float  jointsFilterOmega;
00204     float  jointsFilterZita;
00205     float  velAccSafetyFactor;
00206         
00207     float maxCartPos_x;
00208     float maxCartPos_y;
00209     float maxCartPos_z;
00210 
00211     float minCartPos_x;
00212     float minCartPos_y;
00213     float minCartPos_z;
00214 
00215     
00216 
00217 };
00218 
00219 struct DataFromTheBoard
00220 {
00221     float roverForwardVel;
00222     float roverStabVel;
00223     float roverZVel; //speed around z axis (used to correct the rover orientation on the pipe)
00224     float roverPitchAngle;
00225     float roverOdom;
00226     float roverLaserFrontDx;
00227     float roverLaserFrontSx;
00228     float roverLaserRetroDx;
00229     float roverLaserRetroSx;
00230     
00231    //Arm position in base frame
00232     float misTebX;
00233     float misTebY;
00234     float misTebZ;
00235     //Arm oriantation in base frame
00236     float misTebQ0;
00237     float misTebQ1;
00238     float misTebQ2;
00239     float misTebQ3;
00240     
00241     float cmdJointPos1;
00242     float cmdJointPos2;
00243     float cmdJointPos3;
00244     float cmdJointPos4;
00245     float cmdJointPos5;
00246     float cmdJointPos6;
00247     
00248     float estRoverJointPos1;
00249     float estRoverJointPos2;
00250     
00251     float misArmJointPos1;
00252     float misArmJointPos2;
00253     float misArmJointPos3;
00254     float misArmJointPos4;
00255     float misArmJointPos5;
00256     float misArmJointPos6;
00257     
00258     float droneFx;
00259     float droneFy;
00260     float droneFz;
00261     float droneTx;
00262     float droneTy;
00263     float droneTz;
00264     
00265     float toolOrientAroundPipeAxis;
00266     
00267     float toolLaserFrontDx;
00268     float toolLaserFrontSx;
00269     float toolLaserRetroDx;
00270     float toolLaserRetroSx;
00271   
00272 }; 
00273 
00274 struct ComandMsg 
00275 {
00276     int pId;
00277     RobotCmd Cmd;
00278     DataToTheBoard Data;
00279     uint8_t Checksum;
00280 };
00281 
00282 struct ConfigMsg 
00283 {   
00284     int pId;
00285     ConfigDataToTheBoard Data;
00286     uint8_t Checksum;
00287 };
00288 
00289 
00290 struct ResponseMsg 
00291 {
00292     int StatusData;
00293     int ControlModeData;
00294     DataFromTheBoard Data;
00295     DebugVector Dbv;
00296 
00297 }; 
00298 
00299