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@47:728502df3cb6, 2017-04-12 (annotated)

Committer:
cittecla
Date:
Wed Apr 12 13:30:44 2017 +0000
Revision:
47:728502df3cb6
Parent:
46:8b52c7b34d34
Child:
48:4078877669e4
Implemented draw_to_map() function

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 47:728502df3cb6 59 if(pos.x < 5) {
cittecla 47:728502df3cb6 60 x_negative_offset = 5-pos.x;
cittecla 47:728502df3cb6 61 }
cittecla 47:728502df3cb6 62 if(pos.x > 195) {
cittecla 47:728502df3cb6 63 x_positive_offset = pos.x-195;
cittecla 47:728502df3cb6 64 }
cittecla 47:728502df3cb6 65
cittecla 47:728502df3cb6 66 if(pos.y < 5) {
cittecla 47:728502df3cb6 67 y_negative_offset = 5-pos.y;
cittecla 47:728502df3cb6 68 }
cittecla 47:728502df3cb6 69 if(pos.y > 195) {
cittecla 47:728502df3cb6 70 y_positive_offset = pos.y-195;
cittecla 46:8b52c7b34d34 71 }
cittecla 46:8b52c7b34d34 72
cittecla 47:728502df3cb6 73 for(int y = 0 + y_negative_offset; y < 7-y_positive_offset; y++) {
cittecla 47:728502df3cb6 74 for(int x = 0 + x_negative_offset; x < 7-x_positive_offset; x++) {
cittecla 47:728502df3cb6 75
cittecla 47:728502df3cb6 76 obstacle_list[pos.x-3+x][pos.y-3+y] = obstacle_list_superpos[x][y];
cittecla 47:728502df3cb6 77 }
cittecla 47:728502df3cb6 78 }
cittecla 28:274e1d4ecac5 79 }
cittecla 28:274e1d4ecac5 80
cittecla 47:728502df3cb6 81 position position_calculation(float distance,float degree,float offsetx,float offsety, float heading)
cittecla 47:728502df3cb6 82 {
cittecla 47:728502df3cb6 83
cittecla 47:728502df3cb6 84
cittecla 46:8b52c7b34d34 85 position pos = {0};
cittecla 46:8b52c7b34d34 86 return pos;
cittecla 46:8b52c7b34d34 87 }
cittecla 38:3526c36e4c73 88
cittecla 41:462d379e85c4 89 int select_target()
cittecla 41:462d379e85c4 90 {
cittecla 41:462d379e85c4 91 position myPos = get_current_pos();
cittecla 41:462d379e85c4 92 position diff = {0};
cittecla 41:462d379e85c4 93 target.x = 0;
cittecla 41:462d379e85c4 94 int closest_dist = 10000;
cittecla 41:462d379e85c4 95 int current_dist = 0;
cittecla 41:462d379e85c4 96
cittecla 41:462d379e85c4 97 for(int i = 0; i < row; i++) {
cittecla 41:462d379e85c4 98 for(int j = 0; j < col; j++) {
cittecla 41:462d379e85c4 99 if(obstacle_list[i][j] == 2) {
cittecla 41:462d379e85c4 100 diff.x = abs(myPos.x - j);
cittecla 41:462d379e85c4 101 diff.y = abs(myPos.y - i);
cittecla 41:462d379e85c4 102 current_dist = diff.x * diff.y;
cittecla 41:462d379e85c4 103 if(current_dist < closest_dist) {
cittecla 41:462d379e85c4 104 closest_dist = current_dist;
cittecla 41:462d379e85c4 105 target.x = j;
cittecla 41:462d379e85c4 106 target.y = i;
cittecla 41:462d379e85c4 107 }
cittecla 41:462d379e85c4 108 }
cittecla 41:462d379e85c4 109 }
cittecla 41:462d379e85c4 110 }
cittecla 41:462d379e85c4 111 if(target.x == 0) {
cittecla 41:462d379e85c4 112 return 47; // No Target found
cittecla 41:462d379e85c4 113 } else {
cittecla 41:462d379e85c4 114 return 36; // Target found
cittecla 41:462d379e85c4 115 }
cittecla 41:462d379e85c4 116 }
cittecla 41:462d379e85c4 117
cittecla 40:a46667b62671 118
cittecla 41:462d379e85c4 119 int switch_target_red()
cittecla 41:462d379e85c4 120 {
cittecla 41:462d379e85c4 121 obstacle_list[target.y][target.x] = 0;
cittecla 47:728502df3cb6 122 return 0;
cittecla 40:a46667b62671 123 }
cittecla 40:a46667b62671 124
cittecla 40:a46667b62671 125
cittecla 40:a46667b62671 126
cittecla 40:a46667b62671 127
cittecla 40:a46667b62671 128
cittecla 38:3526c36e4c73 129 /*
cittecla 28:274e1d4ecac5 130 void chack_sensor(sensor)
cittecla 28:274e1d4ecac5 131 {
cittecla 28:274e1d4ecac5 132 get_distance();
cittecla 28:274e1d4ecac5 133 if(sensor < 80cm || sensor > 10cm) {
cittecla 28:274e1d4ecac5 134 calculate_position(sensor, distance);
cittecla 28:274e1d4ecac5 135 check_map(position.this, obstacle);
cittecla 28:274e1d4ecac5 136 if(map.no-entry){
cittecla 28:274e1d4ecac5 137 check_if_obstacle(distance);
cittecla 28:274e1d4ecac5 138 update_map();
cittecla 28:274e1d4ecac5 139 }
cittecla 28:274e1d4ecac5 140 }
cittecla 14:9e2ce5880fb0 141 }
cittecla 28:274e1d4ecac5 142
cittecla 28:274e1d4ecac5 143 void get_distance(sensor)
cittecla 28:274e1d4ecac5 144 {
cittecla 28:274e1d4ecac5 145 sensor.value;
cittecla 28:274e1d4ecac5 146 return value;
cittecla 28:274e1d4ecac5 147 }
cittecla 28:274e1d4ecac5 148
cittecla 28:274e1d4ecac5 149 void calcualte_position(sensor, distance)
cittecla 28:274e1d4ecac5 150 {
cittecla 28:274e1d4ecac5 151 do math;
cittecla 28:274e1d4ecac5 152 return position;
cittecla 28:274e1d4ecac5 153 }
cittecla 28:274e1d4ecac5 154
cittecla 28:274e1d4ecac5 155 void check_if_obstacle(distance)
cittecla 28:274e1d4ecac5 156 {
cittecla 28:274e1d4ecac5 157 move_scanner();
cittecla 28:274e1d4ecac5 158 distance2 = get_distance(sensor);
cittecla 28:274e1d4ecac5 159 reset_scanner();
cittecla 28:274e1d4ecac5 160 if(distance -5cm < distance2 && distance +5cm > distance2) {
cittecla 28:274e1d4ecac5 161 return 1;
cittecla 28:274e1d4ecac5 162 } else {
cittecla 28:274e1d4ecac5 163 return 0;
cittecla 28:274e1d4ecac5 164 }
cittecla 29:e7d0208bf2af 165 }*/