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/Positioning.cpp@70:922cbbfebf02, 2017-04-20 (annotated)

Committer:
cittecla
Date:
Thu Apr 20 07:36:50 2017 +0000
Revision:
70:922cbbfebf02
Parent:
69:1fdcef6a7577
Child:
71:ddf4eb5c3081
initial_positioning completed; defined values assumed, not fixed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 12:91c2e07d2b5b 1 /**
cittecla 12:91c2e07d2b5b 2 * Positioning function library
cittecla 12:91c2e07d2b5b 3 * Handels position of the Robot on the map
cittecla 12:91c2e07d2b5b 4 **/
cittecla 33:8a98f8b9d859 5
cittecla 33:8a98f8b9d859 6
cittecla 38:3526c36e4c73 7 #include "Positioning.h"
cittecla 62:c2fcf3b349e9 8 coordinates current_coord; // Updated
cittecla 62:c2fcf3b349e9 9 position current_pos; // Generated from current_coord
cittecla 46:8b52c7b34d34 10 float current_heading;
cittecla 52:56399c2f13cd 11 position next_pos;
cittecla 12:91c2e07d2b5b 12
cittecla 62:c2fcf3b349e9 13 bool init = false;
aeschsim 68:29c52ea147d5 14 Timer t2;
cittecla 62:c2fcf3b349e9 15
cittecla 62:c2fcf3b349e9 16 int deg_r = 0;
cittecla 62:c2fcf3b349e9 17 int deg_l = 0;
cittecla 62:c2fcf3b349e9 18 float last_dist_r = 0;
cittecla 62:c2fcf3b349e9 19 float last_dist_l = 0;
cittecla 70:922cbbfebf02 20 float direction_r = 0;
cittecla 70:922cbbfebf02 21 float direction_l = 0;
cittecla 62:c2fcf3b349e9 22
cittecla 62:c2fcf3b349e9 23
cittecla 62:c2fcf3b349e9 24 coordinates get_current_coord()
cittecla 62:c2fcf3b349e9 25 {
cittecla 62:c2fcf3b349e9 26 return current_coord;
cittecla 62:c2fcf3b349e9 27 }
cittecla 62:c2fcf3b349e9 28
cittecla 62:c2fcf3b349e9 29 coordinates pos_to_coord(position pos)
cittecla 62:c2fcf3b349e9 30 {
cittecla 62:c2fcf3b349e9 31 coordinates coord = {0};
cittecla 62:c2fcf3b349e9 32 coord.x = pos.x + 0.5f;
cittecla 62:c2fcf3b349e9 33 coord.y = pos.y + 0.5f;
cittecla 62:c2fcf3b349e9 34 return coord;
cittecla 62:c2fcf3b349e9 35 }
cittecla 62:c2fcf3b349e9 36
cittecla 62:c2fcf3b349e9 37 position coord_to_pos(coordinates coord)
cittecla 62:c2fcf3b349e9 38 {
cittecla 62:c2fcf3b349e9 39 position pos = {0};
aeschsim 69:1fdcef6a7577 40 pos.x = rint(coord.x -0.5f); //round values
cittecla 62:c2fcf3b349e9 41 pos.y = rint(coord.y -0.5f);
cittecla 62:c2fcf3b349e9 42 return pos;
cittecla 62:c2fcf3b349e9 43 }
cittecla 62:c2fcf3b349e9 44
cittecla 34:40d8d29b44b8 45 position get_current_pos()
cittecla 33:8a98f8b9d859 46 {
cittecla 62:c2fcf3b349e9 47 current_pos = coord_to_pos(current_coord);
cittecla 33:8a98f8b9d859 48 return current_pos;
cittecla 33:8a98f8b9d859 49 }
cittecla 38:3526c36e4c73 50
cittecla 52:56399c2f13cd 51 position get_next_pos()
cittecla 52:56399c2f13cd 52 {
cittecla 52:56399c2f13cd 53 return next_pos;
cittecla 52:56399c2f13cd 54 }
cittecla 52:56399c2f13cd 55
cittecla 46:8b52c7b34d34 56 float get_current_heading()
cittecla 46:8b52c7b34d34 57 {
cittecla 46:8b52c7b34d34 58 return current_heading;
cittecla 46:8b52c7b34d34 59 }
cittecla 46:8b52c7b34d34 60
cittecla 70:922cbbfebf02 61 void clamp_heading()
cittecla 70:922cbbfebf02 62 {
cittecla 70:922cbbfebf02 63
cittecla 70:922cbbfebf02 64 while (direction_l >= 360) direction_l -= 360;
cittecla 70:922cbbfebf02 65 while (direction_l < 0) direction_l += 360;
cittecla 70:922cbbfebf02 66
cittecla 70:922cbbfebf02 67 }
cittecla 38:3526c36e4c73 68
cittecla 60:b57577b0072f 69 void positioning()
cittecla 39:92723f7ea54f 70 {
cittecla 60:b57577b0072f 71 printf("positioning...\r\n");
cittecla 70:922cbbfebf02 72
cittecla 70:922cbbfebf02 73
cittecla 70:922cbbfebf02 74
cittecla 70:922cbbfebf02 75 clamp_heading();
cittecla 39:92723f7ea54f 76 }
cittecla 38:3526c36e4c73 77
cittecla 38:3526c36e4c73 78
cittecla 62:c2fcf3b349e9 79
cittecla 62:c2fcf3b349e9 80 int initial_positioning()
cittecla 33:8a98f8b9d859 81 {
cittecla 62:c2fcf3b349e9 82 if(init == false) {
cittecla 62:c2fcf3b349e9 83
cittecla 62:c2fcf3b349e9 84 last_dist_r = 100;
cittecla 62:c2fcf3b349e9 85 last_dist_l = 100;
cittecla 33:8a98f8b9d859 86
cittecla 62:c2fcf3b349e9 87 deg_r = -50;
cittecla 62:c2fcf3b349e9 88 deg_l = 50;
cittecla 62:c2fcf3b349e9 89
cittecla 62:c2fcf3b349e9 90 set_servo_position(0,deg_l); //servo sensor left
cittecla 62:c2fcf3b349e9 91 set_servo_position(2,-deg_r); //servo sensor right
cittecla 70:922cbbfebf02 92 current_coord.x = 0;
cittecla 70:922cbbfebf02 93 current_coord.y = 0;
cittecla 62:c2fcf3b349e9 94 t2.start();
cittecla 62:c2fcf3b349e9 95 init = true;
cittecla 62:c2fcf3b349e9 96 } else {
cittecla 33:8a98f8b9d859 97
cittecla 62:c2fcf3b349e9 98 if(t2 > 0.2f) {
cittecla 62:c2fcf3b349e9 99 t2 = 0;
cittecla 62:c2fcf3b349e9 100
cittecla 62:c2fcf3b349e9 101 float dist_r = getDistanceIR(4);
cittecla 62:c2fcf3b349e9 102 float dist_l = getDistanceIR(0);
cittecla 12:91c2e07d2b5b 103
cittecla 62:c2fcf3b349e9 104 //right
cittecla 62:c2fcf3b349e9 105 if(dist_r < last_dist_r) {
cittecla 62:c2fcf3b349e9 106 last_dist_r = dist_r;
cittecla 62:c2fcf3b349e9 107 deg_r += 5;
cittecla 62:c2fcf3b349e9 108 set_servo_position(2,-deg_r);
cittecla 70:922cbbfebf02 109 } else if(direction_r == 0) {
cittecla 70:922cbbfebf02 110
cittecla 70:922cbbfebf02 111 dist_r *= 100;
cittecla 70:922cbbfebf02 112 float offsetx = 12;
cittecla 70:922cbbfebf02 113 float offsety = 12;
cittecla 62:c2fcf3b349e9 114
cittecla 70:922cbbfebf02 115 float x = (offsetx + sin(deg_r/180*(float)M_PI)*dist_r)/4;
cittecla 70:922cbbfebf02 116 float y = (-offsety - cos(deg_r/180*(float)M_PI)*dist_r)/4;
cittecla 70:922cbbfebf02 117 float hyp = sqrt(x*x+y*y); // y pos
cittecla 70:922cbbfebf02 118 direction_r = asin(x/hyp)/(float)M_PI*180; // winkel
cittecla 70:922cbbfebf02 119
cittecla 70:922cbbfebf02 120 current_coord.y = hyp;
cittecla 70:922cbbfebf02 121
cittecla 70:922cbbfebf02 122 while (direction_r >= 360) direction_r -= 360;
cittecla 70:922cbbfebf02 123 while (direction_r < 0) direction_r += 360;
cittecla 62:c2fcf3b349e9 124 }
cittecla 62:c2fcf3b349e9 125
cittecla 62:c2fcf3b349e9 126
cittecla 62:c2fcf3b349e9 127 //left
cittecla 70:922cbbfebf02 128 if(dist_l < last_dist_l) {
cittecla 70:922cbbfebf02 129 last_dist_l = dist_l;
cittecla 70:922cbbfebf02 130 deg_l -= 5;
cittecla 70:922cbbfebf02 131 set_servo_position(0,deg_l);
cittecla 70:922cbbfebf02 132 } else if(direction_l == 0) {
cittecla 70:922cbbfebf02 133
cittecla 70:922cbbfebf02 134 dist_r *= 100;
cittecla 70:922cbbfebf02 135 float offsetx = -12;
cittecla 70:922cbbfebf02 136 float offsety = 12;
cittecla 70:922cbbfebf02 137
cittecla 70:922cbbfebf02 138 float x = (offsetx + sin(deg_l/180*(float)M_PI)*dist_l)/4;
cittecla 70:922cbbfebf02 139 float y = (-offsety - cos(deg_l/180*(float)M_PI)*dist_l)/4;
cittecla 70:922cbbfebf02 140 float hyp = sqrt(x*x+y*y); // y pos
cittecla 70:922cbbfebf02 141 direction_l = asin(x/hyp)/(float)M_PI*180; // winkel
cittecla 70:922cbbfebf02 142
cittecla 70:922cbbfebf02 143 current_coord.x = hyp;
cittecla 70:922cbbfebf02 144
cittecla 70:922cbbfebf02 145 while (direction_l >= 360) direction_l -= 360;
cittecla 70:922cbbfebf02 146 while (direction_l < 0) direction_l += 360;
cittecla 70:922cbbfebf02 147 }
cittecla 70:922cbbfebf02 148
cittecla 70:922cbbfebf02 149 if(current_coord.x != 0 && current_coord.y != 0) {
cittecla 70:922cbbfebf02 150 printf("directions: %f || %f \r\n", direction_l, direction_r);
cittecla 70:922cbbfebf02 151 current_heading = (360-direction_r + 270-direction_l)/2;
cittecla 70:922cbbfebf02 152
cittecla 70:922cbbfebf02 153 clamp_heading();
cittecla 70:922cbbfebf02 154 printf("heading: %f \r\n", current_heading);
cittecla 70:922cbbfebf02 155
cittecla 70:922cbbfebf02 156 //finished
cittecla 70:922cbbfebf02 157 return 11;
cittecla 70:922cbbfebf02 158
cittecla 70:922cbbfebf02 159 }
cittecla 62:c2fcf3b349e9 160 }
cittecla 33:8a98f8b9d859 161 }
cittecla 70:922cbbfebf02 162 // not finished
cittecla 62:c2fcf3b349e9 163 return 16;
cittecla 62:c2fcf3b349e9 164 }
cittecla 62:c2fcf3b349e9 165
cittecla 64:8cfca8fad65d 166 /*
cittecla 62:c2fcf3b349e9 167 turn_straight_right();
cittecla 62:c2fcf3b349e9 168 turn_straight_left();
cittecla 62:c2fcf3b349e9 169
cittecla 62:c2fcf3b349e9 170 while(last_dist_r > sensors[r]) {
cittecla 62:c2fcf3b349e9 171 turn_sensor_right(1); //turn sensor + 1 deg
cittecla 62:c2fcf3b349e9 172 wait(0.1f)
cittecla 62:c2fcf3b349e9 173 deg_r += 1;
cittecla 62:c2fcf3b349e9 174 last_dist_r = sensors[r];
cittecla 62:c2fcf3b349e9 175 }
cittecla 33:8a98f8b9d859 176
cittecla 62:c2fcf3b349e9 177 while(last_dist_l > sensors[l]) {
cittecla 62:c2fcf3b349e9 178 turn_sensor_left(-1); //turn sensor - 1 deg
cittecla 62:c2fcf3b349e9 179 wait(0.1f)
cittecla 62:c2fcf3b349e9 180 deg_l += 1;
cittecla 62:c2fcf3b349e9 181 last_dist_l = sensors[l];
cittecla 62:c2fcf3b349e9 182 }
cittecla 62:c2fcf3b349e9 183
cittecla 62:c2fcf3b349e9 184 int deg_l_2=0;
cittecla 62:c2fcf3b349e9 185 turn_straight_left();
cittecla 62:c2fcf3b349e9 186 last_dist_l = 0;
cittecla 39:92723f7ea54f 187
cittecla 62:c2fcf3b349e9 188 while(last_dist_l < sensors[l]) {
cittecla 62:c2fcf3b349e9 189 turn_sensor_left(1); //turn sensor +1 deg (positiv = uhrzeigersinn)
cittecla 62:c2fcf3b349e9 190 wait(0.1f);
cittecla 62:c2fcf3b349e9 191 deg_l_2 += 1;
cittecla 62:c2fcf3b349e9 192 last_dist_l = sensors[l];
cittecla 62:c2fcf3b349e9 193 }
cittecla 39:92723f7ea54f 194
cittecla 62:c2fcf3b349e9 195 turn_straight_right();
cittecla 62:c2fcf3b349e9 196 turn_straight_left();
cittecla 39:92723f7ea54f 197
cittecla 62:c2fcf3b349e9 198 wait(0.2f);
cittecla 62:c2fcf3b349e9 199 */
cittecla 62:c2fcf3b349e9 200