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@51:4a18b47fd659, 2017-04-15 (annotated)

Committer:
cittecla
Date:
Sat Apr 15 00:18:50 2017 +0000
Revision:
51:4a18b47fd659
Parent:
50:2e2bf0815fd9
Child:
52:56399c2f13cd
bug fixes and generalized servo angel input (#define servo_angel)

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 50:2e2bf0815fd9 33 position current_pos = get_current_pos();
cittecla 50:2e2bf0815fd9 34 float current_heading = get_current_heading();
cittecla 50:2e2bf0815fd9 35 if(old_heading != current_heading || old_pos.x != current_pos.x || old_pos.y != current_pos.y) {
cittecla 47:728502df3cb6 36 //check_sensor(left);
cittecla 49:652438112348 37 float distance0 = getDistanceIR(0);
cittecla 49:652438112348 38 if(distance0 < 0.75f && distance0 > 0.1f) {
cittecla 51:4a18b47fd659 39 set_servo_position(0, servo_angle);
cittecla 49:652438112348 40 float distance = getDistanceIR(1);
cittecla 49:652438112348 41 int object = 0;
cittecla 49:652438112348 42 if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance0 && distance + 0.05f > distance0) {
cittecla 49:652438112348 43 object = 0;
cittecla 49:652438112348 44 } else {
cittecla 49:652438112348 45 object = 1;
cittecla 49:652438112348 46 }
cittecla 51:4a18b47fd659 47 position mapping_pos = position_calculation(distance0, servo_angle, 0.12, 0.12, current_heading, current_pos);
cittecla 49:652438112348 48 draw_to_map(mapping_pos, object);
cittecla 47:728502df3cb6 49 }
cittecla 47:728502df3cb6 50 //check_sensor(right);
cittecla 51:4a18b47fd659 51 float distance4 = getDistanceIR(4);
cittecla 49:652438112348 52 if(distance4 < 0.75f && distance4 > 0.1f) {
cittecla 51:4a18b47fd659 53 set_servo_position(1, -servo_angle);
cittecla 51:4a18b47fd659 54 float distance = getDistanceIR(5);
cittecla 49:652438112348 55 int object = 0;
cittecla 49:652438112348 56 if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance4 && distance + 0.05f > distance4) {
cittecla 49:652438112348 57 object = 0;
cittecla 49:652438112348 58 } else {
cittecla 49:652438112348 59 object = 1;
cittecla 49:652438112348 60 }
cittecla 51:4a18b47fd659 61 position mapping_pos = position_calculation(distance4, -servo_angle, -0.12, 0.12, current_heading, current_pos);
cittecla 49:652438112348 62 draw_to_map(mapping_pos, object);
cittecla 49:652438112348 63 }
cittecla 49:652438112348 64 //check_sensor(center);
cittecla 51:4a18b47fd659 65 float distance2 = getDistanceIR(2);
cittecla 49:652438112348 66 if(distance2 < 0.75f && distance2 > 0.1f) {
cittecla 51:4a18b47fd659 67 float distance = getDistanceIR(3);
cittecla 49:652438112348 68 int object = 0;
cittecla 49:652438112348 69 if(distance < 0.75f && distance > 0.1f && distance -0.05f < distance2 && distance + 0.05f > distance2) {
cittecla 49:652438112348 70 object = 0;
cittecla 49:652438112348 71 } else {
cittecla 49:652438112348 72 object = 1;
cittecla 49:652438112348 73 }
cittecla 50:2e2bf0815fd9 74 position mapping_pos = position_calculation(distance2, 0, -0.12, 0.12, current_heading, current_pos);
cittecla 49:652438112348 75 draw_to_map(mapping_pos, object);
cittecla 47:728502df3cb6 76 }
cittecla 47:728502df3cb6 77
cittecla 50:2e2bf0815fd9 78 old_pos = current_pos;
cittecla 50:2e2bf0815fd9 79 old_heading = current_heading;
cittecla 46:8b52c7b34d34 80 }
cittecla 47:728502df3cb6 81 }
cittecla 47:728502df3cb6 82
cittecla 49:652438112348 83 //******************************************************************************
cittecla 49:652438112348 84 void draw_to_map(position pos, int object)
cittecla 47:728502df3cb6 85 {
cittecla 47:728502df3cb6 86 uint8_t x_negative_offset = 0;
cittecla 47:728502df3cb6 87 uint8_t x_positive_offset = 0;
cittecla 47:728502df3cb6 88 uint8_t y_negative_offset = 0;
cittecla 47:728502df3cb6 89 uint8_t y_positive_offset = 0;
cittecla 47:728502df3cb6 90
cittecla 48:4078877669e4 91 if (pos.x < 5) {
cittecla 48:4078877669e4 92 x_negative_offset = 3 - pos.x;
cittecla 48:4078877669e4 93 }
cittecla 48:4078877669e4 94 if (pos.x > (col-5)) {
cittecla 48:4078877669e4 95 x_positive_offset = pos.x - (col - 4);
cittecla 47:728502df3cb6 96 }
cittecla 48:4078877669e4 97
cittecla 48:4078877669e4 98 if (pos.y < 5) {
cittecla 49:652438112348 99 y_negative_offset = 3 - pos.y;
cittecla 48:4078877669e4 100 }
cittecla 48:4078877669e4 101 if (pos.y > (row-5)) {
cittecla 48:4078877669e4 102 y_positive_offset = pos.y - (row - 4);
cittecla 47:728502df3cb6 103 }
cittecla 47:728502df3cb6 104
cittecla 48:4078877669e4 105 for (int y = 0 + y_negative_offset; y < 7 - y_positive_offset; y++) {
cittecla 48:4078877669e4 106 for (int x = 0 + x_negative_offset; x < 7 - x_positive_offset; x++) {
cittecla 49:652438112348 107 switch(object) {
cittecla 49:652438112348 108 case 0:
cittecla 49:652438112348 109 a = list_time_value;
cittecla 49:652438112348 110 obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
cittecla 49:652438112348 111 break;
cittecla 49:652438112348 112 case 1:
cittecla 49:652438112348 113 a = list_target_value;
cittecla 49:652438112348 114 target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
cittecla 49:652438112348 115 break;
cittecla 49:652438112348 116 case 2:
cittecla 49:652438112348 117 a = list_boundry_value;
cittecla 49:652438112348 118 target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
cittecla 49:652438112348 119 break;
cittecla 49:652438112348 120 default:
cittecla 49:652438112348 121 break;
cittecla 49:652438112348 122 }
cittecla 47:728502df3cb6 123 }
cittecla 47:728502df3cb6 124 }
cittecla 28:274e1d4ecac5 125 }
cittecla 28:274e1d4ecac5 126
cittecla 49:652438112348 127 //******************************************************************************
cittecla 49:652438112348 128 void print_map()
cittecla 49:652438112348 129 {
cittecla 48:4078877669e4 130 for (int y = 0; y < col; y++) {
cittecla 48:4078877669e4 131 for (int x = 0; x < row; x++) {
cittecla 48:4078877669e4 132 printf("%d ", obstacle_list[y][x]);
cittecla 48:4078877669e4 133 }
cittecla 48:4078877669e4 134 printf("\n");
cittecla 48:4078877669e4 135 }
cittecla 48:4078877669e4 136 printf("\n");
cittecla 48:4078877669e4 137 }
cittecla 48:4078877669e4 138
cittecla 49:652438112348 139 //******************************************************************************
cittecla 50:2e2bf0815fd9 140 position position_calculation(float distance, float degree, float offsetx, float offsety, float heading, position current_pos)
cittecla 47:728502df3cb6 141 {
cittecla 50:2e2bf0815fd9 142 distance *= 100;
cittecla 50:2e2bf0815fd9 143 offsetx *= 100;
cittecla 50:2e2bf0815fd9 144 offsety *= 100;
cittecla 50:2e2bf0815fd9 145
cittecla 50:2e2bf0815fd9 146 position pos = { 0 };
cittecla 50:2e2bf0815fd9 147 float direction = 0;
cittecla 50:2e2bf0815fd9 148
cittecla 50:2e2bf0815fd9 149 float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4;
cittecla 50:2e2bf0815fd9 150 float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4;
cittecla 50:2e2bf0815fd9 151 float hyp = sqrt(x*x+y*y);
cittecla 50:2e2bf0815fd9 152 direction = asin(x/hyp)/(float)M_PI*180;
cittecla 50:2e2bf0815fd9 153 direction += heading;
cittecla 47:728502df3cb6 154
cittecla 50:2e2bf0815fd9 155 while (direction >= 360) direction -= 360;
cittecla 50:2e2bf0815fd9 156 while (direction < 0) direction += 360;
cittecla 47:728502df3cb6 157
cittecla 50:2e2bf0815fd9 158 printf("%f || %f || %f || %f\n", x, y, hyp, degree);
cittecla 50:2e2bf0815fd9 159
cittecla 50:2e2bf0815fd9 160 if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
cittecla 50:2e2bf0815fd9 161 pos.x = current_pos.x + rint(sin(direction / 180 * (float)M_PI)*hyp);
cittecla 50:2e2bf0815fd9 162 pos.y = current_pos.y - rint(cos(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 if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
cittecla 50:2e2bf0815fd9 166 pos.x = current_pos.x + rint(sin((180-direction) / 180 * (float)M_PI)*hyp);
cittecla 50:2e2bf0815fd9 167 pos.y = current_pos.y + rint(cos((180-direction) / 180 * (float)M_PI)*hyp);
cittecla 50:2e2bf0815fd9 168 printf("%d || %d\n", pos.x, pos.y);
cittecla 50:2e2bf0815fd9 169 }
cittecla 50:2e2bf0815fd9 170
cittecla 46:8b52c7b34d34 171 return pos;
cittecla 46:8b52c7b34d34 172 }
cittecla 38:3526c36e4c73 173
cittecla 49:652438112348 174 //******************************************************************************
cittecla 41:462d379e85c4 175 int select_target()
cittecla 41:462d379e85c4 176 {
cittecla 41:462d379e85c4 177 position myPos = get_current_pos();
cittecla 41:462d379e85c4 178 position diff = {0};
cittecla 41:462d379e85c4 179 target.x = 0;
cittecla 41:462d379e85c4 180 int closest_dist = 10000;
cittecla 41:462d379e85c4 181 int current_dist = 0;
cittecla 41:462d379e85c4 182
cittecla 41:462d379e85c4 183 for(int i = 0; i < row; i++) {
cittecla 41:462d379e85c4 184 for(int j = 0; j < col; j++) {
cittecla 41:462d379e85c4 185 if(obstacle_list[i][j] == 2) {
cittecla 41:462d379e85c4 186 diff.x = abs(myPos.x - j);
cittecla 41:462d379e85c4 187 diff.y = abs(myPos.y - i);
cittecla 41:462d379e85c4 188 current_dist = diff.x * diff.y;
cittecla 41:462d379e85c4 189 if(current_dist < closest_dist) {
cittecla 41:462d379e85c4 190 closest_dist = current_dist;
cittecla 41:462d379e85c4 191 target.x = j;
cittecla 41:462d379e85c4 192 target.y = i;
cittecla 41:462d379e85c4 193 }
cittecla 41:462d379e85c4 194 }
cittecla 41:462d379e85c4 195 }
cittecla 41:462d379e85c4 196 }
cittecla 41:462d379e85c4 197 if(target.x == 0) {
cittecla 41:462d379e85c4 198 return 47; // No Target found
cittecla 41:462d379e85c4 199 } else {
cittecla 41:462d379e85c4 200 return 36; // Target found
cittecla 41:462d379e85c4 201 }
cittecla 41:462d379e85c4 202 }
cittecla 41:462d379e85c4 203
cittecla 49:652438112348 204 //******************************************************************************
cittecla 41:462d379e85c4 205 int switch_target_red()
cittecla 41:462d379e85c4 206 {
cittecla 41:462d379e85c4 207 obstacle_list[target.y][target.x] = 0;
cittecla 47:728502df3cb6 208 return 0;
cittecla 49:652438112348 209 }