Telemetre.cpp

00001 # include "Telemetre.h"
00002 
00003 
00004 T_MODULE listeModules[NOMBRE_OBJETS];
00005 /*********************************************************************************************************/
00006 /* FUNCTION NAME: SendTelemetreID                                                                        */
00007 /* DESCRIPTION  : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
00008 /*********************************************************************************************************/
00009 void SendTelemetreID (unsigned short id)
00010 {
00011     CANMessage msgTx=CANMessage();
00012     msgTx.id=id;
00013     msgTx.len=0;
00014     can1.write(msgTx);
00015     wait_us(200);
00016 }
00017 
00018 /*********************************************************************************************************/
00019 /* FUNCTION NAME: EspaceLibre                                                                            */
00020 /* DESCRIPTION  : Calcule si la place et libre pour poser un module                                      */
00021 /*********************************************************************************************************/
00022 bool EspaceLibre (signed short xModule, signed short  yModule, signed short xRobot, signed short yRobot){
00023     return true;
00024     
00025 }
00026 
00027 /*********************************************************************************************************/
00028 /* FUNCTION NAME: BonnePlace                                                                             */
00029 /* DESCRIPTION  : Calcule si le module est bien placé                                                    */
00030 /*********************************************************************************************************/
00031 bool BonnePlace(signed short xModule, signed short  yModule, signed short xRobot, signed short yRobot){
00032     bool bienPlace = false;
00033     double distance = abs(xModule - xRobot)*abs(yModule - yRobot) + abs(yModule - yRobot)*abs(yModule - yRobot);
00034     
00035     distance = sqrt(distance);
00036     
00037 
00038     if ((TELEMETRE_PROFONDEUR - 10 < TELEMETRE_PROFONDEUR )||( TELEMETRE_PROFONDEUR + 10 > TELEMETRE_PROFONDEUR)){
00039         bienPlace = true;
00040         }
00041     return bienPlace;
00042 }
00043 
00044 void initModules(void){
00045 
00046     listeModules[0].x = 0;
00047     listeModules[0].y = 1100;
00048     
00049     listeModules[1].x = 200;
00050     listeModules[1].y = 950;
00051         
00052     listeModules[2].x = 600;
00053     listeModules[2].y = 200;
00054         
00055     listeModules[3].x = 600;
00056     listeModules[3].y = 1000;
00057         
00058     listeModules[4].x = 1100;
00059     listeModules[4].y = 500;
00060         
00061     listeModules[5].x = 1350;
00062     listeModules[5].y = 0;
00063         
00064     listeModules[6].x = 1400;
00065     listeModules[6].y = 900;
00066         
00067     listeModules[7].x = 1850;
00068     listeModules[7].y = 800;
00069         
00070     listeModules[8].x = 0;
00071     listeModules[8].y = 1850;
00072         
00073     listeModules[9].x = 200;
00074     listeModules[9].y = 2050;
00075         
00076     listeModules[10].x = 600;
00077     listeModules[10].y = 2000;
00078         
00079     listeModules[11].x = 600;
00080     listeModules[11].y = 2800;
00081     
00082     listeModules[12].x = 1100;
00083     listeModules[12].y = 2500;
00084     
00085     listeModules[13].x = 1350;
00086     listeModules[13].y = 0;
00087     
00088     listeModules[14].x = 1400;
00089     listeModules[14].y = 2100;
00090     
00091     listeModules[15].x = 1850;
00092     listeModules[15].y = 2200;
00093 }
00094 
00095 signed short returnX(int indiceTab){
00096     return listeModules[indiceTab].x;
00097     }
00098     
00099 signed short returnY(int indiceTab){
00100     return listeModules[indiceTab].y;
00101     }
00102 
00103 void processData(int objectType, signed short x, signed short y, int nb_module){
00104     switch(objectType){
00105         case CYLINDRE:
00106             //
00107         break;
00108         
00109         case MODULE:
00110             //
00111             /*if( (x == listeModules.module1.x) && (y == listeModules.module1.y) ){
00112                     printf("module 1");
00113                     listeModules.module1.timeout ++;
00114                 }       */     
00115         break;
00116         
00117         case BALLE:
00118             //
00119         break;
00120         }
00121     
00122     }