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@49:652438112348, 2017-04-13 (annotated)

Committer:
cittecla
Date:
Thu Apr 13 22:45:22 2017 +0000
Revision:
49:652438112348
Parent:
48:4078877669e4
Child:
50:2e2bf0815fd9
a few improvements have been made.

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 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 46:8b52c7b34d34 29 position new_pos = get_current_pos();
cittecla 46:8b52c7b34d34 30 float new_heading = get_current_heading();
cittecla 47:728502df3cb6 31 if(old_heading != new_heading || old_pos.x != new_pos.x || old_pos.y != new_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 49:652438112348 43 position mapping_pos = position_calculation(distance0, 50, 0.12, 0.12, new_heading);
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 49:652438112348 56 position mapping_pos = position_calculation(distance4, -50, -0.12, 0.12, new_heading);
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 49:652438112348 69 position mapping_pos = position_calculation(distance2, 0, -0.12, 0.12, new_heading);
cittecla 49:652438112348 70 draw_to_map(mapping_pos, object);
cittecla 47:728502df3cb6 71 }
cittecla 47:728502df3cb6 72
cittecla 47:728502df3cb6 73 old_pos = new_pos;
cittecla 47:728502df3cb6 74 old_heading = new_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 47:728502df3cb6 135 position position_calculation(float distance,float degree,float offsetx,float offsety, float heading)
cittecla 47:728502df3cb6 136 {
cittecla 47:728502df3cb6 137
cittecla 47:728502df3cb6 138
cittecla 46:8b52c7b34d34 139 position pos = {0};
cittecla 46:8b52c7b34d34 140 return pos;
cittecla 46:8b52c7b34d34 141 }
cittecla 38:3526c36e4c73 142
cittecla 49:652438112348 143 //******************************************************************************
cittecla 41:462d379e85c4 144 int select_target()
cittecla 41:462d379e85c4 145 {
cittecla 41:462d379e85c4 146 position myPos = get_current_pos();
cittecla 41:462d379e85c4 147 position diff = {0};
cittecla 41:462d379e85c4 148 target.x = 0;
cittecla 41:462d379e85c4 149 int closest_dist = 10000;
cittecla 41:462d379e85c4 150 int current_dist = 0;
cittecla 41:462d379e85c4 151
cittecla 41:462d379e85c4 152 for(int i = 0; i < row; i++) {
cittecla 41:462d379e85c4 153 for(int j = 0; j < col; j++) {
cittecla 41:462d379e85c4 154 if(obstacle_list[i][j] == 2) {
cittecla 41:462d379e85c4 155 diff.x = abs(myPos.x - j);
cittecla 41:462d379e85c4 156 diff.y = abs(myPos.y - i);
cittecla 41:462d379e85c4 157 current_dist = diff.x * diff.y;
cittecla 41:462d379e85c4 158 if(current_dist < closest_dist) {
cittecla 41:462d379e85c4 159 closest_dist = current_dist;
cittecla 41:462d379e85c4 160 target.x = j;
cittecla 41:462d379e85c4 161 target.y = i;
cittecla 41:462d379e85c4 162 }
cittecla 41:462d379e85c4 163 }
cittecla 41:462d379e85c4 164 }
cittecla 41:462d379e85c4 165 }
cittecla 41:462d379e85c4 166 if(target.x == 0) {
cittecla 41:462d379e85c4 167 return 47; // No Target found
cittecla 41:462d379e85c4 168 } else {
cittecla 41:462d379e85c4 169 return 36; // Target found
cittecla 41:462d379e85c4 170 }
cittecla 41:462d379e85c4 171 }
cittecla 41:462d379e85c4 172
cittecla 49:652438112348 173 //******************************************************************************
cittecla 41:462d379e85c4 174 int switch_target_red()
cittecla 41:462d379e85c4 175 {
cittecla 41:462d379e85c4 176 obstacle_list[target.y][target.x] = 0;
cittecla 47:728502df3cb6 177 return 0;
cittecla 49:652438112348 178 }