carte_strategie_2019

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Committer:
kyxstark
Date:
Sat Jun 01 02:50:52 2019 +0000
Revision:
91:42ae63e5daf5
Parent:
42:657b6a573e11
reprise erreur asserv correction bug;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sitkah 35:742dc6b200b0 1 #include "Evitement.h"
Sitkah 35:742dc6b200b0 2
Sitkah 35:742dc6b200b0 3 #include <stdlib.h>
Sitkah 35:742dc6b200b0 4 #include <string.h>
Sitkah 35:742dc6b200b0 5 #include <math.h>
Sitkah 35:742dc6b200b0 6
Sitkah 42:657b6a573e11 7 /////////////////////////////DEBUT D'UNE EBAUCHE DE CARTOGRAPHIE DU TERRAIN EN MODE DAMMIER AFIN DE PRATIQUER UN A* EN EVITEMENT/////////////////////////////////////////////////
Sitkah 37:fca332b64b42 8
Sitkah 37:fca332b64b42 9
Sitkah 36:6dd30780bd8e 10 /////////////////////ROBOTS/////////////////////////////////////////////
Sitkah 36:6dd30780bd8e 11 #ifdef ROBOT_BIG
Sitkah 36:6dd30780bd8e 12 S_obstacle robot{
Sitkah 36:6dd30780bd8e 13 0,
Sitkah 36:6dd30780bd8e 14 0,
Sitkah 36:6dd30780bd8e 15 6,
Sitkah 36:6dd30780bd8e 16 6,
Sitkah 36:6dd30780bd8e 17 0,
Sitkah 36:6dd30780bd8e 18 0,
Sitkah 36:6dd30780bd8e 19 };
Sitkah 36:6dd30780bd8e 20 #else
Sitkah 36:6dd30780bd8e 21 S_obstacle robot{
Sitkah 36:6dd30780bd8e 22 210,
Sitkah 36:6dd30780bd8e 23 285,
Sitkah 36:6dd30780bd8e 24 4,
Sitkah 36:6dd30780bd8e 25 4,
Sitkah 36:6dd30780bd8e 26 0,
Sitkah 36:6dd30780bd8e 27 0,
Sitkah 36:6dd30780bd8e 28 };
Sitkah 36:6dd30780bd8e 29 #endif
Sitkah 35:742dc6b200b0 30 /////////////////////LISTE DES OBSTACLES/////////////////////////////////
Sitkah 35:742dc6b200b0 31 S_obstacle bloc1{
Sitkah 35:742dc6b200b0 32 500, //x
Sitkah 35:742dc6b200b0 33 800, //y
Sitkah 35:742dc6b200b0 34 2, //largeur axe X
Sitkah 35:742dc6b200b0 35 2, //longueur axe Y
Sitkah 35:742dc6b200b0 36 1, //danger
Sitkah 35:742dc6b200b0 37 0, //rayonnement distance à laquelle il y a un possible danger
Sitkah 35:742dc6b200b0 38 };
Sitkah 35:742dc6b200b0 39
Sitkah 35:742dc6b200b0 40 S_obstacle bloc2{
Sitkah 35:742dc6b200b0 41 500, //x
Sitkah 35:742dc6b200b0 42 2150, //y
Sitkah 35:742dc6b200b0 43 2, //largeur axe X
Sitkah 35:742dc6b200b0 44 2, //longueur axe Y
Sitkah 35:742dc6b200b0 45 1, //danger
Sitkah 35:742dc6b200b0 46 0, //rayonnement distance à laquelle il y a un possible danger
Sitkah 35:742dc6b200b0 47 };
Sitkah 35:742dc6b200b0 48
Sitkah 35:742dc6b200b0 49 S_obstacle bloc3{
Sitkah 35:742dc6b200b0 50 1100, //x
Sitkah 35:742dc6b200b0 51 200, //y
Sitkah 35:742dc6b200b0 52 2, //largeur axe X
Sitkah 35:742dc6b200b0 53 2, //longueur axe Y
Sitkah 35:742dc6b200b0 54 10, //danger
Sitkah 35:742dc6b200b0 55 0, //rayonnement distance à laquelle il y a un possible danger
Sitkah 35:742dc6b200b0 56 };
Sitkah 35:742dc6b200b0 57
Sitkah 35:742dc6b200b0 58 S_obstacle bloc4{
Sitkah 35:742dc6b200b0 59 1100, //x
Sitkah 35:742dc6b200b0 60 2600, //y
Sitkah 35:742dc6b200b0 61 2, //largeur axe X
Sitkah 35:742dc6b200b0 62 2, //longueur axe Y
Sitkah 35:742dc6b200b0 63 1, //danger
Sitkah 35:742dc6b200b0 64 0, //rayonnement distance à laquelle il y a un possible danger
Sitkah 35:742dc6b200b0 65 };
Sitkah 35:742dc6b200b0 66
Sitkah 35:742dc6b200b0 67 S_obstacle bloc5{
Sitkah 35:742dc6b200b0 68 1400, //x
Sitkah 35:742dc6b200b0 69 1000, //y
Sitkah 35:742dc6b200b0 70 2, //largeur axe X
Sitkah 35:742dc6b200b0 71 2, //longueur axe Y
Sitkah 35:742dc6b200b0 72 1, //danger
Sitkah 35:742dc6b200b0 73 0, //rayonnement distance à laquelle il y a un possible danger
Sitkah 35:742dc6b200b0 74 };
Sitkah 35:742dc6b200b0 75
Sitkah 35:742dc6b200b0 76 S_obstacle bloc6{
Sitkah 35:742dc6b200b0 77 1400, //x
Sitkah 35:742dc6b200b0 78 1800, //y
Sitkah 35:742dc6b200b0 79 2, //largeur axe X
Sitkah 35:742dc6b200b0 80 2, //longueur axe Y
Sitkah 35:742dc6b200b0 81 1, //danger
Sitkah 35:742dc6b200b0 82 0, //rayonnement distance à laquelle il y a un possible danger
Sitkah 35:742dc6b200b0 83 };
Sitkah 35:742dc6b200b0 84
Sitkah 36:6dd30780bd8e 85 S_obstacle liste_objets[6]={bloc1,bloc2,bloc3,bloc4,bloc5,bloc6};
Sitkah 35:742dc6b200b0 86
Sitkah 36:6dd30780bd8e 87
Sitkah 35:742dc6b200b0 88
Sitkah 36:6dd30780bd8e 89 void Terrain_init(char **Table){
Sitkah 35:742dc6b200b0 90 int x,y;
Sitkah 35:742dc6b200b0 91 for(x=0;x<TAILLE_TABLE_X/TAILLE_CELLULE;x++){
Sitkah 35:742dc6b200b0 92 for(y=0;y<TAILLE_TABLE_Y/TAILLE_CELLULE;y++){
Sitkah 36:6dd30780bd8e 93 Table[x][y]=1;
Sitkah 35:742dc6b200b0 94 }
Sitkah 35:742dc6b200b0 95 }
Sitkah 35:742dc6b200b0 96 }
Sitkah 36:6dd30780bd8e 97
Sitkah 36:6dd30780bd8e 98 void Terrain_actualisation(char **Table, struct S_obstacle *liste_objet){
Sitkah 36:6dd30780bd8e 99 int x=0,y=0,i=0;
Sitkah 36:6dd30780bd8e 100 x=liste_objet[i].x/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 101 y=liste_objet[i].y/TAILLE_CELLULE;
Sitkah 35:742dc6b200b0 102
Sitkah 35:742dc6b200b0 103 for(i=0; i<nb_objets;i++){
Sitkah 35:742dc6b200b0 104 while(x<(x+liste_objet[i].largeur)){
Sitkah 36:6dd30780bd8e 105
Sitkah 36:6dd30780bd8e 106 y=liste_objet[i].y/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 107
Sitkah 35:742dc6b200b0 108 while(y<(y+liste_objet[i].longueur)){
Sitkah 35:742dc6b200b0 109 Table[x][y]=liste_objet[i].danger;
Sitkah 35:742dc6b200b0 110 y++;
Sitkah 35:742dc6b200b0 111 }
Sitkah 35:742dc6b200b0 112 x++;
Sitkah 35:742dc6b200b0 113 }
Sitkah 36:6dd30780bd8e 114 }
Sitkah 36:6dd30780bd8e 115 }
Sitkah 36:6dd30780bd8e 116
Sitkah 36:6dd30780bd8e 117 void Actualisation_robot(short x,short y, short theta){
Sitkah 36:6dd30780bd8e 118 robot.x=x;
Sitkah 36:6dd30780bd8e 119 robot.y=y;
Sitkah 36:6dd30780bd8e 120 }
Sitkah 36:6dd30780bd8e 121
Sitkah 36:6dd30780bd8e 122
Sitkah 36:6dd30780bd8e 123 void Actualisation_position(char **Table){
Sitkah 36:6dd30780bd8e 124 short x,y;
Sitkah 36:6dd30780bd8e 125 #ifdef ROBOT_SMALL
Sitkah 36:6dd30780bd8e 126 x=(robot.x-115)/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 127 y=(robot.y-115)/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 128 #else
Sitkah 36:6dd30780bd8e 129 x=(robot.x-185)/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 130 y=(robot.y-185)/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 131 #endif
Sitkah 36:6dd30780bd8e 132 while(x<(x+robot.largeur)){
Sitkah 36:6dd30780bd8e 133 y=robot.y/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 134
Sitkah 36:6dd30780bd8e 135 while(y<(y+robot.longueur)){
Sitkah 36:6dd30780bd8e 136 Table[x][y]=robot.danger;
Sitkah 36:6dd30780bd8e 137 y++;
Sitkah 36:6dd30780bd8e 138 }
Sitkah 36:6dd30780bd8e 139 x++;
Sitkah 36:6dd30780bd8e 140 }
Sitkah 36:6dd30780bd8e 141 }
Sitkah 36:6dd30780bd8e 142
Sitkah 36:6dd30780bd8e 143 void Actualisation_objets(char **Table,S_obstacle bloc, short x_bloc, short y_bloc){ //déplacement des cubes
Sitkah 36:6dd30780bd8e 144 int x=0,y=0;
Sitkah 36:6dd30780bd8e 145 x=bloc.x/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 146 y=bloc.y/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 147
Sitkah 36:6dd30780bd8e 148 while(x<(x+bloc.largeur)){
Sitkah 36:6dd30780bd8e 149
Sitkah 36:6dd30780bd8e 150 y=bloc.y/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 151
Sitkah 36:6dd30780bd8e 152 while(y<(y+bloc.longueur)){
Sitkah 36:6dd30780bd8e 153 Table[x][y]=1;
Sitkah 36:6dd30780bd8e 154 y++;
Sitkah 36:6dd30780bd8e 155 }
Sitkah 36:6dd30780bd8e 156 x++;
Sitkah 35:742dc6b200b0 157 }
Sitkah 35:742dc6b200b0 158
Sitkah 36:6dd30780bd8e 159 bloc.x=x_bloc;
Sitkah 36:6dd30780bd8e 160 bloc.y=y_bloc;
Sitkah 36:6dd30780bd8e 161 x=bloc.x/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 162 y=bloc.y/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 163
Sitkah 36:6dd30780bd8e 164 while(x<(x+bloc.largeur)){
Sitkah 35:742dc6b200b0 165
Sitkah 36:6dd30780bd8e 166 y=bloc.y/TAILLE_CELLULE;
Sitkah 36:6dd30780bd8e 167
Sitkah 36:6dd30780bd8e 168 while(y<(y+bloc.longueur)){
Sitkah 36:6dd30780bd8e 169 Table[x][y]=1;
Sitkah 36:6dd30780bd8e 170 y++;
Sitkah 36:6dd30780bd8e 171 }
Sitkah 36:6dd30780bd8e 172 x++;
Sitkah 36:6dd30780bd8e 173 }
Sitkah 36:6dd30780bd8e 174 }
Sitkah 37:fca332b64b42 175 void Localisation_robot_adv(char **Table){
Sitkah 37:fca332b64b42 176 }
Sitkah 36:6dd30780bd8e 177
Sitkah 37:fca332b64b42 178 /*void Automate_evitement(char **Table, S_obstacle robot, ){
Sitkah 37:fca332b64b42 179
Sitkah 37:fca332b64b42 180 switch(EvitEtat){
Sitkah 37:fca332b64b42 181 case ETAT_INIT_EVITEMENT:
Sitkah 37:fca332b64b42 182 break;
Sitkah 37:fca332b64b42 183
Sitkah 37:fca332b64b42 184 case ETAT_ESTIMATION_POSITION :
Sitkah 37:fca332b64b42 185 break;
Sitkah 37:fca332b64b42 186
Sitkah 37:fca332b64b42 187 case ETAT_ESTIMATION_POSITION_ROTATION_ACK:
Sitkah 37:fca332b64b42 188 break;
Sitkah 37:fca332b64b42 189
Sitkah 37:fca332b64b42 190 case ETAT_ESTIMATION_POSITION_ROTATION_ACK_END:
Sitkah 37:fca332b64b42 191 break;
Sitkah 37:fca332b64b42 192
Sitkah 37:fca332b64b42 193 case ETAT_FIN_EVITEMENT:
Sitkah 37:fca332b64b42 194 break;
Sitkah 37:fca332b64b42 195 }
Sitkah 37:fca332b64b42 196 }*/