Tobis Programm forked to not destroy your golden files
Fork of Robocode by
source/Mapping.cpp@50:2e2bf0815fd9, 2017-04-15 (annotated)
- Committer:
- cittecla
- Date:
- Sat Apr 15 00:11:16 2017 +0000
- Revision:
- 50:2e2bf0815fd9
- Parent:
- 49:652438112348
- Child:
- 51:4a18b47fd659
position_calculation completely implemented and tested!; Critical mapping functions now complete.
Who changed what in which revision?
User | Revision | Line number | New 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 | 47:728502df3cb6 | 8 | uint8_t obstacle_list[row][col] = { 0 }; |
cittecla | 49:652438112348 | 9 | uint8_t target_list[row][col] = {0}; |
cittecla | 47:728502df3cb6 | 10 | uint8_t list_time_value = 60; |
cittecla | 49:652438112348 | 11 | uint8_t list_target_value = 1; |
cittecla | 49:652438112348 | 12 | uint8_t list_boundry_value = 255; |
cittecla | 47:728502df3cb6 | 13 | uint8_t a = list_time_value; //Substitution um Matrix übersichtlich zu halten |
cittecla | 49:652438112348 | 14 | uint8_t superpos[7][7] = {{0,0,a,a,a,0,0}, |
cittecla | 47:728502df3cb6 | 15 | {0,a,a,a,a,a,0}, |
cittecla | 47:728502df3cb6 | 16 | {a,a,a,a,a,a,a}, |
cittecla | 47:728502df3cb6 | 17 | {a,a,a,a,a,a,a}, |
cittecla | 47:728502df3cb6 | 18 | {a,a,a,a,a,a,a}, |
cittecla | 47:728502df3cb6 | 19 | {0,a,a,a,a,a,0}, |
cittecla | 47:728502df3cb6 | 20 | {0,0,a,a,a,0,0} |
cittecla | 47:728502df3cb6 | 21 | }; |
cittecla | 47:728502df3cb6 | 22 | |
cittecla | 46:8b52c7b34d34 | 23 | position old_pos = {0}; |
cittecla | 49:652438112348 | 24 | float old_heading = 0; |
cittecla | 46:8b52c7b34d34 | 25 | |
cittecla | 49:652438112348 | 26 | //****************************************************************************** |
cittecla | 13:57f5a7876d2f | 27 | void mapping() |
cittecla | 13:57f5a7876d2f | 28 | { |
cittecla | 50:2e2bf0815fd9 | 29 | position current_pos = get_current_pos(); |
cittecla | 50:2e2bf0815fd9 | 30 | float current_heading = get_current_heading(); |
cittecla | 50:2e2bf0815fd9 | 31 | if(old_heading != current_heading || old_pos.x != current_pos.x || old_pos.y != current_pos.y) { |
cittecla | 47:728502df3cb6 | 32 | //check_sensor(left); |
cittecla | 49:652438112348 | 33 | float distance0 = getDistanceIR(0); |
cittecla | 49:652438112348 | 34 | if(distance0 < 0.75f && distance0 > 0.1f) { |
cittecla | 49:652438112348 | 35 | set_servo_position(0, 50); |
cittecla | 49:652438112348 | 36 | float distance = getDistanceIR(1); |
cittecla | 49:652438112348 | 37 | int object = 0; |
cittecla | 49:652438112348 | 38 | if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance0 && distance + 0.05f > distance0) { |
cittecla | 49:652438112348 | 39 | object = 0; |
cittecla | 49:652438112348 | 40 | } else { |
cittecla | 49:652438112348 | 41 | object = 1; |
cittecla | 49:652438112348 | 42 | } |
cittecla | 50:2e2bf0815fd9 | 43 | position mapping_pos = position_calculation(distance0, 50, 0.12, 0.12, current_heading, current_pos); |
cittecla | 49:652438112348 | 44 | draw_to_map(mapping_pos, object); |
cittecla | 47:728502df3cb6 | 45 | } |
cittecla | 47:728502df3cb6 | 46 | //check_sensor(right); |
cittecla | 49:652438112348 | 47 | float distance4 = getDistanceIR(0); |
cittecla | 49:652438112348 | 48 | if(distance4 < 0.75f && distance4 > 0.1f) { |
cittecla | 49:652438112348 | 49 | float distance = getDistanceIR(1); |
cittecla | 49:652438112348 | 50 | int object = 0; |
cittecla | 49:652438112348 | 51 | if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance4 && distance + 0.05f > distance4) { |
cittecla | 49:652438112348 | 52 | object = 0; |
cittecla | 49:652438112348 | 53 | } else { |
cittecla | 49:652438112348 | 54 | object = 1; |
cittecla | 49:652438112348 | 55 | } |
cittecla | 50:2e2bf0815fd9 | 56 | position mapping_pos = position_calculation(distance4, -50, -0.12, 0.12, current_heading, current_pos); |
cittecla | 49:652438112348 | 57 | draw_to_map(mapping_pos, object); |
cittecla | 49:652438112348 | 58 | } |
cittecla | 49:652438112348 | 59 | //check_sensor(center); |
cittecla | 49:652438112348 | 60 | float distance2 = getDistanceIR(0); |
cittecla | 49:652438112348 | 61 | if(distance2 < 0.75f && distance2 > 0.1f) { |
cittecla | 49:652438112348 | 62 | float distance = getDistanceIR(1); |
cittecla | 49:652438112348 | 63 | int object = 0; |
cittecla | 49:652438112348 | 64 | if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance2 && distance + 0.05f > distance2) { |
cittecla | 49:652438112348 | 65 | object = 0; |
cittecla | 49:652438112348 | 66 | } else { |
cittecla | 49:652438112348 | 67 | object = 1; |
cittecla | 49:652438112348 | 68 | } |
cittecla | 50:2e2bf0815fd9 | 69 | position mapping_pos = position_calculation(distance2, 0, -0.12, 0.12, current_heading, current_pos); |
cittecla | 49:652438112348 | 70 | draw_to_map(mapping_pos, object); |
cittecla | 47:728502df3cb6 | 71 | } |
cittecla | 47:728502df3cb6 | 72 | |
cittecla | 50:2e2bf0815fd9 | 73 | old_pos = current_pos; |
cittecla | 50:2e2bf0815fd9 | 74 | old_heading = current_heading; |
cittecla | 46:8b52c7b34d34 | 75 | } |
cittecla | 47:728502df3cb6 | 76 | } |
cittecla | 47:728502df3cb6 | 77 | |
cittecla | 49:652438112348 | 78 | //****************************************************************************** |
cittecla | 49:652438112348 | 79 | void draw_to_map(position pos, int object) |
cittecla | 47:728502df3cb6 | 80 | { |
cittecla | 47:728502df3cb6 | 81 | uint8_t x_negative_offset = 0; |
cittecla | 47:728502df3cb6 | 82 | uint8_t x_positive_offset = 0; |
cittecla | 47:728502df3cb6 | 83 | uint8_t y_negative_offset = 0; |
cittecla | 47:728502df3cb6 | 84 | uint8_t y_positive_offset = 0; |
cittecla | 47:728502df3cb6 | 85 | |
cittecla | 48:4078877669e4 | 86 | if (pos.x < 5) { |
cittecla | 48:4078877669e4 | 87 | x_negative_offset = 3 - pos.x; |
cittecla | 48:4078877669e4 | 88 | } |
cittecla | 48:4078877669e4 | 89 | if (pos.x > (col-5)) { |
cittecla | 48:4078877669e4 | 90 | x_positive_offset = pos.x - (col - 4); |
cittecla | 47:728502df3cb6 | 91 | } |
cittecla | 48:4078877669e4 | 92 | |
cittecla | 48:4078877669e4 | 93 | if (pos.y < 5) { |
cittecla | 49:652438112348 | 94 | y_negative_offset = 3 - pos.y; |
cittecla | 48:4078877669e4 | 95 | } |
cittecla | 48:4078877669e4 | 96 | if (pos.y > (row-5)) { |
cittecla | 48:4078877669e4 | 97 | y_positive_offset = pos.y - (row - 4); |
cittecla | 47:728502df3cb6 | 98 | } |
cittecla | 47:728502df3cb6 | 99 | |
cittecla | 48:4078877669e4 | 100 | for (int y = 0 + y_negative_offset; y < 7 - y_positive_offset; y++) { |
cittecla | 48:4078877669e4 | 101 | for (int x = 0 + x_negative_offset; x < 7 - x_positive_offset; x++) { |
cittecla | 49:652438112348 | 102 | switch(object) { |
cittecla | 49:652438112348 | 103 | case 0: |
cittecla | 49:652438112348 | 104 | a = list_time_value; |
cittecla | 49:652438112348 | 105 | obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x]; |
cittecla | 49:652438112348 | 106 | break; |
cittecla | 49:652438112348 | 107 | case 1: |
cittecla | 49:652438112348 | 108 | a = list_target_value; |
cittecla | 49:652438112348 | 109 | target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x]; |
cittecla | 49:652438112348 | 110 | break; |
cittecla | 49:652438112348 | 111 | case 2: |
cittecla | 49:652438112348 | 112 | a = list_boundry_value; |
cittecla | 49:652438112348 | 113 | target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x]; |
cittecla | 49:652438112348 | 114 | break; |
cittecla | 49:652438112348 | 115 | default: |
cittecla | 49:652438112348 | 116 | break; |
cittecla | 49:652438112348 | 117 | } |
cittecla | 47:728502df3cb6 | 118 | } |
cittecla | 47:728502df3cb6 | 119 | } |
cittecla | 28:274e1d4ecac5 | 120 | } |
cittecla | 28:274e1d4ecac5 | 121 | |
cittecla | 49:652438112348 | 122 | //****************************************************************************** |
cittecla | 49:652438112348 | 123 | void print_map() |
cittecla | 49:652438112348 | 124 | { |
cittecla | 48:4078877669e4 | 125 | for (int y = 0; y < col; y++) { |
cittecla | 48:4078877669e4 | 126 | for (int x = 0; x < row; x++) { |
cittecla | 48:4078877669e4 | 127 | printf("%d ", obstacle_list[y][x]); |
cittecla | 48:4078877669e4 | 128 | } |
cittecla | 48:4078877669e4 | 129 | printf("\n"); |
cittecla | 48:4078877669e4 | 130 | } |
cittecla | 48:4078877669e4 | 131 | printf("\n"); |
cittecla | 48:4078877669e4 | 132 | } |
cittecla | 48:4078877669e4 | 133 | |
cittecla | 49:652438112348 | 134 | //****************************************************************************** |
cittecla | 50:2e2bf0815fd9 | 135 | position position_calculation(float distance, float degree, float offsetx, float offsety, float heading, position current_pos) |
cittecla | 47:728502df3cb6 | 136 | { |
cittecla | 50:2e2bf0815fd9 | 137 | distance *= 100; |
cittecla | 50:2e2bf0815fd9 | 138 | offsetx *= 100; |
cittecla | 50:2e2bf0815fd9 | 139 | offsety *= 100; |
cittecla | 50:2e2bf0815fd9 | 140 | |
cittecla | 50:2e2bf0815fd9 | 141 | position pos = { 0 }; |
cittecla | 50:2e2bf0815fd9 | 142 | float direction = 0; |
cittecla | 50:2e2bf0815fd9 | 143 | |
cittecla | 50:2e2bf0815fd9 | 144 | float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4; |
cittecla | 50:2e2bf0815fd9 | 145 | float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4; |
cittecla | 50:2e2bf0815fd9 | 146 | float hyp = sqrt(x*x+y*y); |
cittecla | 50:2e2bf0815fd9 | 147 | direction = asin(x/hyp)/(float)M_PI*180; |
cittecla | 50:2e2bf0815fd9 | 148 | direction += heading; |
cittecla | 47:728502df3cb6 | 149 | |
cittecla | 50:2e2bf0815fd9 | 150 | while (direction >= 360) direction -= 360; |
cittecla | 50:2e2bf0815fd9 | 151 | while (direction < 0) direction += 360; |
cittecla | 47:728502df3cb6 | 152 | |
cittecla | 50:2e2bf0815fd9 | 153 | printf("%f || %f || %f || %f\n", x, y, hyp, degree); |
cittecla | 50:2e2bf0815fd9 | 154 | |
cittecla | 50:2e2bf0815fd9 | 155 | if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) { |
cittecla | 50:2e2bf0815fd9 | 156 | pos.x = current_pos.x + rint(sin(direction / 180 * (float)M_PI)*hyp); |
cittecla | 50:2e2bf0815fd9 | 157 | pos.y = current_pos.y - rint(cos(direction / 180 * (float)M_PI)*hyp); |
cittecla | 50:2e2bf0815fd9 | 158 | printf("%d || %d\n", pos.x,pos.y); |
cittecla | 50:2e2bf0815fd9 | 159 | } |
cittecla | 50:2e2bf0815fd9 | 160 | if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) { |
cittecla | 50:2e2bf0815fd9 | 161 | pos.x = current_pos.x + rint(sin((180-direction) / 180 * (float)M_PI)*hyp); |
cittecla | 50:2e2bf0815fd9 | 162 | pos.y = current_pos.y + rint(cos((180-direction) / 180 * (float)M_PI)*hyp); |
cittecla | 50:2e2bf0815fd9 | 163 | printf("%d || %d\n", pos.x, pos.y); |
cittecla | 50:2e2bf0815fd9 | 164 | } |
cittecla | 50:2e2bf0815fd9 | 165 | |
cittecla | 46:8b52c7b34d34 | 166 | return pos; |
cittecla | 46:8b52c7b34d34 | 167 | } |
cittecla | 38:3526c36e4c73 | 168 | |
cittecla | 49:652438112348 | 169 | //****************************************************************************** |
cittecla | 41:462d379e85c4 | 170 | int select_target() |
cittecla | 41:462d379e85c4 | 171 | { |
cittecla | 41:462d379e85c4 | 172 | position myPos = get_current_pos(); |
cittecla | 41:462d379e85c4 | 173 | position diff = {0}; |
cittecla | 41:462d379e85c4 | 174 | target.x = 0; |
cittecla | 41:462d379e85c4 | 175 | int closest_dist = 10000; |
cittecla | 41:462d379e85c4 | 176 | int current_dist = 0; |
cittecla | 41:462d379e85c4 | 177 | |
cittecla | 41:462d379e85c4 | 178 | for(int i = 0; i < row; i++) { |
cittecla | 41:462d379e85c4 | 179 | for(int j = 0; j < col; j++) { |
cittecla | 41:462d379e85c4 | 180 | if(obstacle_list[i][j] == 2) { |
cittecla | 41:462d379e85c4 | 181 | diff.x = abs(myPos.x - j); |
cittecla | 41:462d379e85c4 | 182 | diff.y = abs(myPos.y - i); |
cittecla | 41:462d379e85c4 | 183 | current_dist = diff.x * diff.y; |
cittecla | 41:462d379e85c4 | 184 | if(current_dist < closest_dist) { |
cittecla | 41:462d379e85c4 | 185 | closest_dist = current_dist; |
cittecla | 41:462d379e85c4 | 186 | target.x = j; |
cittecla | 41:462d379e85c4 | 187 | target.y = i; |
cittecla | 41:462d379e85c4 | 188 | } |
cittecla | 41:462d379e85c4 | 189 | } |
cittecla | 41:462d379e85c4 | 190 | } |
cittecla | 41:462d379e85c4 | 191 | } |
cittecla | 41:462d379e85c4 | 192 | if(target.x == 0) { |
cittecla | 41:462d379e85c4 | 193 | return 47; // No Target found |
cittecla | 41:462d379e85c4 | 194 | } else { |
cittecla | 41:462d379e85c4 | 195 | return 36; // Target found |
cittecla | 41:462d379e85c4 | 196 | } |
cittecla | 41:462d379e85c4 | 197 | } |
cittecla | 41:462d379e85c4 | 198 | |
cittecla | 49:652438112348 | 199 | //****************************************************************************** |
cittecla | 41:462d379e85c4 | 200 | int switch_target_red() |
cittecla | 41:462d379e85c4 | 201 | { |
cittecla | 41:462d379e85c4 | 202 | obstacle_list[target.y][target.x] = 0; |
cittecla | 47:728502df3cb6 | 203 | return 0; |
cittecla | 49:652438112348 | 204 | } |