Tobias Berger / Mbed 2 deprecated Robocode

Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/Mapping.cpp@129:0f60bf9640bb, 2017-05-15 (annotated)

Committer:
cittecla
Date:
Mon May 15 11:48:05 2017 +0000
Revision:
129:0f60bf9640bb
Parent:
128:6bde4483ce7b
Child:
130:670a954495bf
found and fixed a few errors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 12:91c2e07d2b5b 1 /**
cittecla 12:91c2e07d2b5b 2 * Mapping function library
cittecla 12:91c2e07d2b5b 3 * Handels Mapping of the arena and LEGO-stones
cittecla 12:91c2e07d2b5b 4 **/
cittecla 12:91c2e07d2b5b 5
cittecla 38:3526c36e4c73 6 #include "Mapping.h"
cittecla 38:3526c36e4c73 7
cittecla 51:4a18b47fd659 8 #define list_time_value 60
cittecla 51:4a18b47fd659 9 #define list_target_value 1
cittecla 51:4a18b47fd659 10 #define list_boundry_value 255
cittecla 51:4a18b47fd659 11 #define servo_angle 50
cittecla 51:4a18b47fd659 12
cittecla 51:4a18b47fd659 13
cittecla 47:728502df3cb6 14 uint8_t obstacle_list[row][col] = { 0 };
cittecla 49:652438112348 15 uint8_t target_list[row][col] = {0};
cittecla 51:4a18b47fd659 16
cittecla 47:728502df3cb6 17 uint8_t a = list_time_value; //Substitution um Matrix übersichtlich zu halten
cittecla 49:652438112348 18 uint8_t superpos[7][7] = {{0,0,a,a,a,0,0},
cittecla 47:728502df3cb6 19 {0,a,a,a,a,a,0},
cittecla 47:728502df3cb6 20 {a,a,a,a,a,a,a},
cittecla 47:728502df3cb6 21 {a,a,a,a,a,a,a},
cittecla 47:728502df3cb6 22 {a,a,a,a,a,a,a},
cittecla 47:728502df3cb6 23 {0,a,a,a,a,a,0},
cittecla 47:728502df3cb6 24 {0,0,a,a,a,0,0}
cittecla 47:728502df3cb6 25 };
cittecla 47:728502df3cb6 26
cittecla 46:8b52c7b34d34 27 position old_pos = {0};
cittecla 49:652438112348 28 float old_heading = 0;
cittecla 46:8b52c7b34d34 29
cittecla 49:652438112348 30 //******************************************************************************
cittecla 13:57f5a7876d2f 31 void mapping()
cittecla 13:57f5a7876d2f 32 {
cittecla 60:b57577b0072f 33 printf("mapping...\r\n");
cittecla 62:c2fcf3b349e9 34
cittecla 50:2e2bf0815fd9 35 position current_pos = get_current_pos();
cittecla 50:2e2bf0815fd9 36 float current_heading = get_current_heading();
cittecla 50:2e2bf0815fd9 37 if(old_heading != current_heading || old_pos.x != current_pos.x || old_pos.y != current_pos.y) {
cittecla 47:728502df3cb6 38 //check_sensor(left);
cittecla 49:652438112348 39 float distance0 = getDistanceIR(0);
cittecla 49:652438112348 40 if(distance0 < 0.75f && distance0 > 0.1f) {
cittecla 51:4a18b47fd659 41 set_servo_position(0, servo_angle);
cittecla 49:652438112348 42 float distance = getDistanceIR(1);
cittecla 49:652438112348 43 int object = 0;
cittecla 57:1a395b6928ee 44 if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance0 || distance + 0.05f > distance0)) {
cittecla 49:652438112348 45 object = 0;
cittecla 49:652438112348 46 } else {
cittecla 49:652438112348 47 object = 1;
cittecla 49:652438112348 48 }
cittecla 52:56399c2f13cd 49 position mapping_pos = position_calculation(distance0, servo_angle, 0.12, -0.12, current_heading, current_pos);
cittecla 49:652438112348 50 draw_to_map(mapping_pos, object);
cittecla 47:728502df3cb6 51 }
cittecla 47:728502df3cb6 52 //check_sensor(right);
cittecla 51:4a18b47fd659 53 float distance4 = getDistanceIR(4);
cittecla 49:652438112348 54 if(distance4 < 0.75f && distance4 > 0.1f) {
cittecla 129:0f60bf9640bb 55 set_servo_position(2, -servo_angle);
cittecla 51:4a18b47fd659 56 float distance = getDistanceIR(5);
cittecla 49:652438112348 57 int object = 0;
cittecla 57:1a395b6928ee 58 if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance4 || distance + 0.05f > distance4)) {
cittecla 49:652438112348 59 object = 0;
cittecla 49:652438112348 60 } else {
cittecla 49:652438112348 61 object = 1;
cittecla 49:652438112348 62 }
cittecla 52:56399c2f13cd 63 position mapping_pos = position_calculation(distance4, -servo_angle, -0.12, -0.12, current_heading, current_pos);
cittecla 49:652438112348 64 draw_to_map(mapping_pos, object);
cittecla 49:652438112348 65 }
cittecla 49:652438112348 66 //check_sensor(center);
cittecla 51:4a18b47fd659 67 float distance2 = getDistanceIR(2);
cittecla 49:652438112348 68 if(distance2 < 0.75f && distance2 > 0.1f) {
cittecla 51:4a18b47fd659 69 float distance = getDistanceIR(3);
cittecla 49:652438112348 70 int object = 0;
cittecla 57:1a395b6928ee 71 if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance2 || distance + 0.05f > distance2)) {
cittecla 49:652438112348 72 object = 0;
cittecla 49:652438112348 73 } else {
cittecla 49:652438112348 74 object = 1;
cittecla 49:652438112348 75 }
cittecla 52:56399c2f13cd 76 position mapping_pos = position_calculation(distance2, 0, -0.12, -0.12, current_heading, current_pos);
cittecla 49:652438112348 77 draw_to_map(mapping_pos, object);
cittecla 47:728502df3cb6 78 }
cittecla 47:728502df3cb6 79
cittecla 50:2e2bf0815fd9 80 old_pos = current_pos;
cittecla 50:2e2bf0815fd9 81 old_heading = current_heading;
cittecla 62:c2fcf3b349e9 82 }
cittecla 47:728502df3cb6 83 }
cittecla 47:728502df3cb6 84
cittecla 129:0f60bf9640bb 85 /**
cittecla 129:0f60bf9640bb 86 * draws something on the map, object defines what to draw
cittecla 129:0f60bf9640bb 87 * 0 = obstacle
cittecla 129:0f60bf9640bb 88 * 1 = target
cittecla 129:0f60bf9640bb 89 * 2 =
cittecla 129:0f60bf9640bb 90 * by Claudio Citterio
cittecla 129:0f60bf9640bb 91 **/
cittecla 49:652438112348 92 void draw_to_map(position pos, int object)
cittecla 47:728502df3cb6 93 {
cittecla 47:728502df3cb6 94 uint8_t x_negative_offset = 0;
cittecla 47:728502df3cb6 95 uint8_t x_positive_offset = 0;
cittecla 47:728502df3cb6 96 uint8_t y_negative_offset = 0;
cittecla 47:728502df3cb6 97 uint8_t y_positive_offset = 0;
cittecla 47:728502df3cb6 98
cittecla 48:4078877669e4 99 if (pos.x < 5) {
cittecla 48:4078877669e4 100 x_negative_offset = 3 - pos.x;
cittecla 48:4078877669e4 101 }
cittecla 48:4078877669e4 102 if (pos.x > (col-5)) {
cittecla 48:4078877669e4 103 x_positive_offset = pos.x - (col - 4);
cittecla 47:728502df3cb6 104 }
cittecla 48:4078877669e4 105
cittecla 48:4078877669e4 106 if (pos.y < 5) {
cittecla 49:652438112348 107 y_negative_offset = 3 - pos.y;
cittecla 48:4078877669e4 108 }
cittecla 48:4078877669e4 109 if (pos.y > (row-5)) {
cittecla 48:4078877669e4 110 y_positive_offset = pos.y - (row - 4);
cittecla 47:728502df3cb6 111 }
cittecla 47:728502df3cb6 112
cittecla 48:4078877669e4 113 for (int y = 0 + y_negative_offset; y < 7 - y_positive_offset; y++) {
cittecla 48:4078877669e4 114 for (int x = 0 + x_negative_offset; x < 7 - x_positive_offset; x++) {
cittecla 49:652438112348 115 switch(object) {
cittecla 49:652438112348 116 case 0:
cittecla 49:652438112348 117 a = list_time_value;
cittecla 49:652438112348 118 obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
cittecla 49:652438112348 119 break;
cittecla 49:652438112348 120 case 1:
cittecla 49:652438112348 121 a = list_target_value;
cittecla 49:652438112348 122 target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
cittecla 49:652438112348 123 break;
cittecla 49:652438112348 124 case 2:
cittecla 49:652438112348 125 a = list_boundry_value;
cittecla 49:652438112348 126 target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
cittecla 49:652438112348 127 break;
cittecla 49:652438112348 128 default:
cittecla 49:652438112348 129 break;
cittecla 49:652438112348 130 }
cittecla 47:728502df3cb6 131 }
cittecla 47:728502df3cb6 132 }
cittecla 28:274e1d4ecac5 133 }
cittecla 28:274e1d4ecac5 134
cittecla 49:652438112348 135 //******************************************************************************
cittecla 129:0f60bf9640bb 136 /**
cittecla 129:0f60bf9640bb 137 * prints map defined by list
cittecla 129:0f60bf9640bb 138 * 0 = obstacle_list
cittecla 129:0f60bf9640bb 139 * 1 = open_list
cittecla 129:0f60bf9640bb 140 * 2 = target_list
cittecla 129:0f60bf9640bb 141 * very slow, shouldn't be called in final code
cittecla 129:0f60bf9640bb 142 * by Claudio Citterio
cittecla 129:0f60bf9640bb 143 **/
cittecla 129:0f60bf9640bb 144 void print_map(int list)
cittecla 49:652438112348 145 {
cittecla 52:56399c2f13cd 146 // Debug function for printing the obstacle matrix on putty.
cittecla 48:4078877669e4 147 for (int y = 0; y < col; y++) {
cittecla 48:4078877669e4 148 for (int x = 0; x < row; x++) {
cittecla 129:0f60bf9640bb 149 if(list == 0)printf("%d ", obstacle_list[y][x]);
cittecla 129:0f60bf9640bb 150 if(list == 1)printf("%d ", open_list[y][x]);
cittecla 129:0f60bf9640bb 151 if(list == 2)printf("%d ", target_list[y][x]);
cittecla 48:4078877669e4 152 }
cittecla 52:56399c2f13cd 153 printf("\r\n");
cittecla 48:4078877669e4 154 }
cittecla 52:56399c2f13cd 155 printf("\r\n");
cittecla 48:4078877669e4 156 }
cittecla 48:4078877669e4 157
cittecla 49:652438112348 158 //******************************************************************************
cittecla 129:0f60bf9640bb 159 /**
cittecla 129:0f60bf9640bb 160 * calculates a position given by numerouse distances and angels
cittecla 129:0f60bf9640bb 161 * is called by mapping to define where to object is
cittecla 129:0f60bf9640bb 162 * by Claudio Citterio
cittecla 129:0f60bf9640bb 163 **/
cittecla 50:2e2bf0815fd9 164 position position_calculation(float distance, float degree, float offsetx, float offsety, float heading, position current_pos)
cittecla 47:728502df3cb6 165 {
cittecla 50:2e2bf0815fd9 166 distance *= 100;
cittecla 50:2e2bf0815fd9 167 offsetx *= 100;
cittecla 50:2e2bf0815fd9 168 offsety *= 100;
cittecla 50:2e2bf0815fd9 169
cittecla 50:2e2bf0815fd9 170 position pos = { 0 };
cittecla 50:2e2bf0815fd9 171 float direction = 0;
cittecla 50:2e2bf0815fd9 172
cittecla 50:2e2bf0815fd9 173 float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4;
cittecla 50:2e2bf0815fd9 174 float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4;
cittecla 50:2e2bf0815fd9 175 float hyp = sqrt(x*x+y*y);
cittecla 50:2e2bf0815fd9 176 direction = asin(x/hyp)/(float)M_PI*180;
cittecla 50:2e2bf0815fd9 177 direction += heading;
cittecla 47:728502df3cb6 178
cittecla 50:2e2bf0815fd9 179 while (direction >= 360) direction -= 360;
cittecla 50:2e2bf0815fd9 180 while (direction < 0) direction += 360;
cittecla 47:728502df3cb6 181
cittecla 71:ddf4eb5c3081 182 printf("%f || %f || %f || %f\r\n", x, y, hyp, degree);
cittecla 50:2e2bf0815fd9 183
cittecla 50:2e2bf0815fd9 184 if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
cittecla 50:2e2bf0815fd9 185 pos.x = current_pos.x + rint(sin(direction / 180 * (float)M_PI)*hyp);
cittecla 50:2e2bf0815fd9 186 pos.y = current_pos.y - rint(cos(direction / 180 * (float)M_PI)*hyp);
cittecla 71:ddf4eb5c3081 187 printf("%d || %d\r\n", pos.x,pos.y);
cittecla 50:2e2bf0815fd9 188 }
cittecla 50:2e2bf0815fd9 189 if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
cittecla 50:2e2bf0815fd9 190 pos.x = current_pos.x + rint(sin((180-direction) / 180 * (float)M_PI)*hyp);
cittecla 50:2e2bf0815fd9 191 pos.y = current_pos.y + rint(cos((180-direction) / 180 * (float)M_PI)*hyp);
cittecla 71:ddf4eb5c3081 192 printf("%d || %d\r\n", pos.x, pos.y);
cittecla 50:2e2bf0815fd9 193 }
cittecla 50:2e2bf0815fd9 194
cittecla 46:8b52c7b34d34 195 return pos;
cittecla 46:8b52c7b34d34 196 }
cittecla 38:3526c36e4c73 197
cittecla 62:c2fcf3b349e9 198
cittecla 62:c2fcf3b349e9 199 coordinates coordinates_calculation(float distance, float degree, float offsetx, float offsety, float heading, coordinates current_coord)
cittecla 62:c2fcf3b349e9 200 {
cittecla 62:c2fcf3b349e9 201 distance *= 100;
cittecla 62:c2fcf3b349e9 202 offsetx *= 100;
cittecla 62:c2fcf3b349e9 203 offsety *= 100;
cittecla 62:c2fcf3b349e9 204
cittecla 62:c2fcf3b349e9 205 coordinates coord = { 0 };
cittecla 62:c2fcf3b349e9 206 float direction = 0;
cittecla 62:c2fcf3b349e9 207
cittecla 62:c2fcf3b349e9 208 float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4;
cittecla 62:c2fcf3b349e9 209 float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4;
cittecla 62:c2fcf3b349e9 210 float hyp = sqrt(x*x+y*y);
cittecla 62:c2fcf3b349e9 211 direction = asin(x/hyp)/(float)M_PI*180;
cittecla 62:c2fcf3b349e9 212 direction += heading;
cittecla 62:c2fcf3b349e9 213
cittecla 62:c2fcf3b349e9 214 while (direction >= 360) direction -= 360;
cittecla 62:c2fcf3b349e9 215 while (direction < 0) direction += 360;
cittecla 62:c2fcf3b349e9 216
cittecla 62:c2fcf3b349e9 217 printf("%f || %f || %f || %f\n", x, y, hyp, degree);
cittecla 62:c2fcf3b349e9 218
cittecla 62:c2fcf3b349e9 219 if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
cittecla 62:c2fcf3b349e9 220 coord.x = current_coord.x + sin(direction / 180 * (float)M_PI)*hyp;
cittecla 62:c2fcf3b349e9 221 coord.y = current_coord.y - cos(direction / 180 * (float)M_PI)*hyp;
cittecla 62:c2fcf3b349e9 222 printf("%f || %f\n", coord.x,coord.y);
cittecla 62:c2fcf3b349e9 223 }
cittecla 62:c2fcf3b349e9 224 if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
cittecla 62:c2fcf3b349e9 225 coord.x = current_coord.x + sin((180-direction) / 180 * (float)M_PI)*hyp;
cittecla 62:c2fcf3b349e9 226 coord.y = current_coord.y + cos((180-direction) / 180 * (float)M_PI)*hyp;
cittecla 62:c2fcf3b349e9 227 printf("%f || %f\n", coord.x, coord.y);
cittecla 62:c2fcf3b349e9 228 }
cittecla 62:c2fcf3b349e9 229
cittecla 62:c2fcf3b349e9 230 return coord;
cittecla 62:c2fcf3b349e9 231 }
cittecla 129:0f60bf9640bb 232
cittecla 129:0f60bf9640bb 233 /**
cittecla 129:0f60bf9640bb 234 * selects closest target from current position
cittecla 129:0f60bf9640bb 235 * targets ar printed as 1 in target_list
cittecla 129:0f60bf9640bb 236 * by Claudio Citterio
cittecla 129:0f60bf9640bb 237 **/
cittecla 41:462d379e85c4 238 int select_target()
cittecla 41:462d379e85c4 239 {
cittecla 41:462d379e85c4 240 position myPos = get_current_pos();
cittecla 41:462d379e85c4 241 position diff = {0};
cittecla 41:462d379e85c4 242 target.x = 0;
cittecla 41:462d379e85c4 243 int closest_dist = 10000;
cittecla 41:462d379e85c4 244 int current_dist = 0;
cittecla 41:462d379e85c4 245
cittecla 41:462d379e85c4 246 for(int i = 0; i < row; i++) {
cittecla 41:462d379e85c4 247 for(int j = 0; j < col; j++) {
cittecla 129:0f60bf9640bb 248 if(target_list[i][j] == 1) {
cittecla 41:462d379e85c4 249 diff.x = abs(myPos.x - j);
cittecla 41:462d379e85c4 250 diff.y = abs(myPos.y - i);
cittecla 41:462d379e85c4 251 current_dist = diff.x * diff.y;
cittecla 41:462d379e85c4 252 if(current_dist < closest_dist) {
cittecla 41:462d379e85c4 253 closest_dist = current_dist;
cittecla 41:462d379e85c4 254 target.x = j;
cittecla 41:462d379e85c4 255 target.y = i;
cittecla 41:462d379e85c4 256 }
cittecla 41:462d379e85c4 257 }
cittecla 41:462d379e85c4 258 }
cittecla 41:462d379e85c4 259 }
cittecla 41:462d379e85c4 260 if(target.x == 0) {
cittecla 128:6bde4483ce7b 261 return 48; // No Target found
cittecla 41:462d379e85c4 262 } else {
cittecla 128:6bde4483ce7b 263 start = get_current_pos();
cittecla 128:6bde4483ce7b 264 printf("start: %d || %d\r\n", start.x, start.y);
cittecla 129:0f60bf9640bb 265 printf("Target: %d || %d\r\n", target.x, target.y);
cittecla 128:6bde4483ce7b 266
cittecla 41:462d379e85c4 267 return 36; // Target found
cittecla 41:462d379e85c4 268 }
cittecla 41:462d379e85c4 269 }
cittecla 41:462d379e85c4 270
cittecla 86:df8c869a5a52 271 int generate_fake_target(){
cittecla 129:0f60bf9640bb 272 position pos = {50,50};
cittecla 129:0f60bf9640bb 273 draw_to_map(pos,1);
cittecla 126:d0b2057272d0 274 return 35;
cittecla 86:df8c869a5a52 275 }
cittecla 86:df8c869a5a52 276
cittecla 49:652438112348 277 //******************************************************************************
cittecla 41:462d379e85c4 278 int switch_target_red()
cittecla 41:462d379e85c4 279 {
cittecla 41:462d379e85c4 280 obstacle_list[target.y][target.x] = 0;
cittecla 47:728502df3cb6 281 return 0;
cittecla 49:652438112348 282 }