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@48:4078877669e4, 2017-04-13 (annotated)

Committer:
cittecla
Date:
Thu Apr 13 09:18:58 2017 +0000
Revision:
48:4078877669e4
Parent:
47:728502df3cb6
Child:
49:652438112348
draw_to_map functionality chacked and improved. works now properly

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 47:728502df3cb6 9 uint8_t list_time_value = 60;
cittecla 47:728502df3cb6 10 uint8_t a = list_time_value; //Substitution um Matrix übersichtlich zu halten
cittecla 47:728502df3cb6 11 uint8_t obstacle_list_superpos[7][7] = {{0,0,a,a,a,0,0},
cittecla 47:728502df3cb6 12 {0,a,a,a,a,a,0},
cittecla 47:728502df3cb6 13 {a,a,a,a,a,a,a},
cittecla 47:728502df3cb6 14 {a,a,a,a,a,a,a},
cittecla 47:728502df3cb6 15 {a,a,a,a,a,a,a},
cittecla 47:728502df3cb6 16 {0,a,a,a,a,a,0},
cittecla 47:728502df3cb6 17 {0,0,a,a,a,0,0}
cittecla 47:728502df3cb6 18 };
cittecla 47:728502df3cb6 19
cittecla 46:8b52c7b34d34 20 position old_pos = {0};
cittecla 46:8b52c7b34d34 21 position mapping_pos_right = {0};
cittecla 46:8b52c7b34d34 22 position mapping_pos_left = {0};
cittecla 46:8b52c7b34d34 23
cittecla 46:8b52c7b34d34 24 float old_heading = {0};
cittecla 46:8b52c7b34d34 25 float distance_left = 0;
cittecla 46:8b52c7b34d34 26 float distance_right = 0;
cittecla 12:91c2e07d2b5b 27
cittecla 13:57f5a7876d2f 28 void mapping()
cittecla 13:57f5a7876d2f 29 {
cittecla 46:8b52c7b34d34 30 position new_pos = get_current_pos();
cittecla 46:8b52c7b34d34 31 float new_heading = get_current_heading();
cittecla 47:728502df3cb6 32 if(old_heading != new_heading || old_pos.x != new_pos.x || old_pos.y != new_pos.y) {
cittecla 47:728502df3cb6 33 //pseudo code
cittecla 47:728502df3cb6 34 //check_sensor(left);
cittecla 47:728502df3cb6 35 distance_left = getDistanceIR(0);
cittecla 47:728502df3cb6 36 if(distance_left < 0.75f && distance_left > 0.1f) {
cittecla 47:728502df3cb6 37 mapping_pos_left = position_calculation(distance_left, 50, 0.12, 0.12, new_heading);
cittecla 47:728502df3cb6 38 draw_to_map(mapping_pos_left);
cittecla 47:728502df3cb6 39 }
cittecla 47:728502df3cb6 40 //check_sensor(right);
cittecla 47:728502df3cb6 41 distance_right = getDistanceIR(4);
cittecla 47:728502df3cb6 42 if(distance_right < 0.75f && distance_right > 0.1f) {
cittecla 47:728502df3cb6 43 mapping_pos_right = position_calculation(distance_right, -50, -0.12, 0.12, new_heading);
cittecla 47:728502df3cb6 44 draw_to_map(mapping_pos_right);
cittecla 47:728502df3cb6 45 }
cittecla 47:728502df3cb6 46
cittecla 47:728502df3cb6 47 old_pos = new_pos;
cittecla 47:728502df3cb6 48 old_heading = new_heading;
cittecla 46:8b52c7b34d34 49 }
cittecla 47:728502df3cb6 50 }
cittecla 47:728502df3cb6 51
cittecla 47:728502df3cb6 52 void draw_to_map(position pos)
cittecla 47:728502df3cb6 53 {
cittecla 47:728502df3cb6 54 uint8_t x_negative_offset = 0;
cittecla 47:728502df3cb6 55 uint8_t x_positive_offset = 0;
cittecla 47:728502df3cb6 56 uint8_t y_negative_offset = 0;
cittecla 47:728502df3cb6 57 uint8_t y_positive_offset = 0;
cittecla 47:728502df3cb6 58
cittecla 48:4078877669e4 59 if (pos.x < 5) {
cittecla 48:4078877669e4 60 x_negative_offset = 3 - pos.x;
cittecla 48:4078877669e4 61 }
cittecla 48:4078877669e4 62 if (pos.x > (col-5)) {
cittecla 48:4078877669e4 63 x_positive_offset = pos.x - (col - 4);
cittecla 47:728502df3cb6 64 }
cittecla 48:4078877669e4 65
cittecla 48:4078877669e4 66 if (pos.y < 5) {
cittecla 48:4078877669e4 67 y_negative_offset = 3 - pos.y;
cittecla 48:4078877669e4 68 }
cittecla 48:4078877669e4 69 if (pos.y > (row-5)) {
cittecla 48:4078877669e4 70 y_positive_offset = pos.y - (row - 4);
cittecla 47:728502df3cb6 71 }
cittecla 47:728502df3cb6 72
cittecla 48:4078877669e4 73 for (int y = 0 + y_negative_offset; y < 7 - y_positive_offset; y++) {
cittecla 48:4078877669e4 74 for (int x = 0 + x_negative_offset; x < 7 - x_positive_offset; x++) {
cittecla 48:4078877669e4 75
cittecla 48:4078877669e4 76 obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = obstacle_list_superpos[y][x];
cittecla 47:728502df3cb6 77 }
cittecla 47:728502df3cb6 78 }
cittecla 28:274e1d4ecac5 79 }
cittecla 28:274e1d4ecac5 80
cittecla 48:4078877669e4 81 void draw_map() {
cittecla 48:4078877669e4 82 for (int y = 0; y < col; y++) {
cittecla 48:4078877669e4 83 for (int x = 0; x < row; x++) {
cittecla 48:4078877669e4 84 printf("%d ", obstacle_list[y][x]);
cittecla 48:4078877669e4 85 }
cittecla 48:4078877669e4 86 printf("\n");
cittecla 48:4078877669e4 87 }
cittecla 48:4078877669e4 88 printf("\n");
cittecla 48:4078877669e4 89 }
cittecla 48:4078877669e4 90
cittecla 47:728502df3cb6 91 position position_calculation(float distance,float degree,float offsetx,float offsety, float heading)
cittecla 47:728502df3cb6 92 {
cittecla 47:728502df3cb6 93
cittecla 47:728502df3cb6 94
cittecla 46:8b52c7b34d34 95 position pos = {0};
cittecla 46:8b52c7b34d34 96 return pos;
cittecla 46:8b52c7b34d34 97 }
cittecla 38:3526c36e4c73 98
cittecla 41:462d379e85c4 99 int select_target()
cittecla 41:462d379e85c4 100 {
cittecla 41:462d379e85c4 101 position myPos = get_current_pos();
cittecla 41:462d379e85c4 102 position diff = {0};
cittecla 41:462d379e85c4 103 target.x = 0;
cittecla 41:462d379e85c4 104 int closest_dist = 10000;
cittecla 41:462d379e85c4 105 int current_dist = 0;
cittecla 41:462d379e85c4 106
cittecla 41:462d379e85c4 107 for(int i = 0; i < row; i++) {
cittecla 41:462d379e85c4 108 for(int j = 0; j < col; j++) {
cittecla 41:462d379e85c4 109 if(obstacle_list[i][j] == 2) {
cittecla 41:462d379e85c4 110 diff.x = abs(myPos.x - j);
cittecla 41:462d379e85c4 111 diff.y = abs(myPos.y - i);
cittecla 41:462d379e85c4 112 current_dist = diff.x * diff.y;
cittecla 41:462d379e85c4 113 if(current_dist < closest_dist) {
cittecla 41:462d379e85c4 114 closest_dist = current_dist;
cittecla 41:462d379e85c4 115 target.x = j;
cittecla 41:462d379e85c4 116 target.y = i;
cittecla 41:462d379e85c4 117 }
cittecla 41:462d379e85c4 118 }
cittecla 41:462d379e85c4 119 }
cittecla 41:462d379e85c4 120 }
cittecla 41:462d379e85c4 121 if(target.x == 0) {
cittecla 41:462d379e85c4 122 return 47; // No Target found
cittecla 41:462d379e85c4 123 } else {
cittecla 41:462d379e85c4 124 return 36; // Target found
cittecla 41:462d379e85c4 125 }
cittecla 41:462d379e85c4 126 }
cittecla 41:462d379e85c4 127
cittecla 40:a46667b62671 128
cittecla 41:462d379e85c4 129 int switch_target_red()
cittecla 41:462d379e85c4 130 {
cittecla 41:462d379e85c4 131 obstacle_list[target.y][target.x] = 0;
cittecla 47:728502df3cb6 132 return 0;
cittecla 40:a46667b62671 133 }
cittecla 40:a46667b62671 134
cittecla 40:a46667b62671 135
cittecla 40:a46667b62671 136
cittecla 40:a46667b62671 137
cittecla 40:a46667b62671 138
cittecla 38:3526c36e4c73 139 /*
cittecla 28:274e1d4ecac5 140 void chack_sensor(sensor)
cittecla 28:274e1d4ecac5 141 {
cittecla 28:274e1d4ecac5 142 get_distance();
cittecla 28:274e1d4ecac5 143 if(sensor < 80cm || sensor > 10cm) {
cittecla 28:274e1d4ecac5 144 calculate_position(sensor, distance);
cittecla 28:274e1d4ecac5 145 check_map(position.this, obstacle);
cittecla 28:274e1d4ecac5 146 if(map.no-entry){
cittecla 28:274e1d4ecac5 147 check_if_obstacle(distance);
cittecla 28:274e1d4ecac5 148 update_map();
cittecla 28:274e1d4ecac5 149 }
cittecla 28:274e1d4ecac5 150 }
cittecla 14:9e2ce5880fb0 151 }
cittecla 28:274e1d4ecac5 152
cittecla 28:274e1d4ecac5 153 void get_distance(sensor)
cittecla 28:274e1d4ecac5 154 {
cittecla 28:274e1d4ecac5 155 sensor.value;
cittecla 28:274e1d4ecac5 156 return value;
cittecla 28:274e1d4ecac5 157 }
cittecla 28:274e1d4ecac5 158
cittecla 28:274e1d4ecac5 159 void calcualte_position(sensor, distance)
cittecla 28:274e1d4ecac5 160 {
cittecla 28:274e1d4ecac5 161 do math;
cittecla 28:274e1d4ecac5 162 return position;
cittecla 28:274e1d4ecac5 163 }
cittecla 28:274e1d4ecac5 164
cittecla 28:274e1d4ecac5 165 void check_if_obstacle(distance)
cittecla 28:274e1d4ecac5 166 {
cittecla 28:274e1d4ecac5 167 move_scanner();
cittecla 28:274e1d4ecac5 168 distance2 = get_distance(sensor);
cittecla 28:274e1d4ecac5 169 reset_scanner();
cittecla 28:274e1d4ecac5 170 if(distance -5cm < distance2 && distance +5cm > distance2) {
cittecla 28:274e1d4ecac5 171 return 1;
cittecla 28:274e1d4ecac5 172 } else {
cittecla 28:274e1d4ecac5 173 return 0;
cittecla 28:274e1d4ecac5 174 }
cittecla 29:e7d0208bf2af 175 }*/