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@136:906ac19fb850, 2017-05-22 (annotated)

Committer:
PESGruppe1
Date:
Mon May 22 13:18:13 2017 +0000
Revision:
136:906ac19fb850
Parent:
134:5c29654ce301 
New finepostitioning mit vor und zur?ckdrehen 1 grad;

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 134:5c29654ce301 8 coordinates current_coord; // in cm from 0 point // Updated
cittecla 134:5c29654ce301 9 position current_pos; // in grid (coord / 4) // 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 134:5c29654ce301 32 coord.x = pos.x*4.0f + 2.0f;
cittecla 134:5c29654ce301 33 coord.y = pos.y*4.0f + 2.0f;
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};
cittecla 134:5c29654ce301 40 pos.x = rint(coord.x/4.0f - 2.0f); //round values
cittecla 134:5c29654ce301 41 pos.y = rint(coord.y/4.0f - 2.0f);
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 112:b3270806629f 64 while (current_heading >= 360) current_heading -= 360;
cittecla 112:b3270806629f 65 while (current_heading < 0) current_heading += 360;
cittecla 70:922cbbfebf02 66
cittecla 70:922cbbfebf02 67 }
cittecla 38:3526c36e4c73 68
cittecla 60:b57577b0072f 69 void positioning()
cittecla 39:92723f7ea54f 70 {
cittecla 112:b3270806629f 71 //printf("positioning...\r\n");
cittecla 112:b3270806629f 72
cittecla 112:b3270806629f 73 // get wheel_speed
cittecla 134:5c29654ce301 74 float speed_left = fabsf(get_speed_left());
cittecla 134:5c29654ce301 75 float speed_right = fabsf(get_speed_right());
cittecla 134:5c29654ce301 76
cittecla 134:5c29654ce301 77 //printf("\r\nspeeds: %f || %f\r\n", speed_left, speed_right);
cittecla 112:b3270806629f 78
cittecla 112:b3270806629f 79 // calc movement and turning speed
cittecla 134:5c29654ce301 80 float movement_speed = (speed_left + speed_right) / 2.0f ;
cittecla 134:5c29654ce301 81 float turning_speed = (speed_left - speed_right) / 2.0f ;
cittecla 134:5c29654ce301 82
cittecla 134:5c29654ce301 83 printf("\nmovement_speed %f\r\n", movement_speed);
cittecla 134:5c29654ce301 84 printf("turning_speed %f\r\n",turning_speed);
cittecla 134:5c29654ce301 85 printf("time: %f\r\n\n",t2.read());
cittecla 112:b3270806629f 86
cittecla 112:b3270806629f 87 // calc heading and coverd distance
cittecla 112:b3270806629f 88 current_heading += 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t2.read() * turning_speed*0.1f);
cittecla 134:5c29654ce301 89
cittecla 112:b3270806629f 90 float distance = (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t2.read() * movement_speed * 0.1f;
cittecla 112:b3270806629f 91 clamp_heading();
cittecla 112:b3270806629f 92 t2.reset();
cittecla 112:b3270806629f 93 t2.start();
cittecla 134:5c29654ce301 94
cittecla 112:b3270806629f 95 // calc new position
cittecla 112:b3270806629f 96 current_coord.x += sin(current_heading/180.0f*(float)M_PI)* distance;
cittecla 112:b3270806629f 97 current_coord.y -= cos(current_heading/180.0f*(float)M_PI) * distance;
cittecla 71:ddf4eb5c3081 98
cittecla 134:5c29654ce301 99 current_pos = coord_to_pos(current_coord);
cittecla 39:92723f7ea54f 100 }
cittecla 38:3526c36e4c73 101
cittecla 38:3526c36e4c73 102
cittecla 62:c2fcf3b349e9 103
cittecla 62:c2fcf3b349e9 104 int initial_positioning()
cittecla 33:8a98f8b9d859 105 {
cittecla 62:c2fcf3b349e9 106 if(init == false) {
cittecla 71:ddf4eb5c3081 107 printf("initial_positioning init\r\n");
cittecla 62:c2fcf3b349e9 108 last_dist_r = 100;
cittecla 62:c2fcf3b349e9 109 last_dist_l = 100;
cittecla 33:8a98f8b9d859 110
cittecla 112:b3270806629f 111 // deg_r = -50;
cittecla 112:b3270806629f 112 // deg_l = 50;
cittecla 112:b3270806629f 113
cittecla 72:4e8a151d804e 114 deg_r = 0;
cittecla 72:4e8a151d804e 115 deg_l = 0;
cittecla 62:c2fcf3b349e9 116
cittecla 120:cdf7a6751f9e 117 set_servo_position(0,deg_l); //servo sensor left
cittecla 120:cdf7a6751f9e 118 set_servo_position(2,deg_r); //servo sensor right
cittecla 70:922cbbfebf02 119 current_coord.x = 0;
cittecla 70:922cbbfebf02 120 current_coord.y = 0;
cittecla 62:c2fcf3b349e9 121 t2.start();
cittecla 62:c2fcf3b349e9 122 init = true;
cittecla 62:c2fcf3b349e9 123 } else {
cittecla 33:8a98f8b9d859 124
cittecla 62:c2fcf3b349e9 125 if(t2 > 0.2f) {
cittecla 112:b3270806629f 126
cittecla 71:ddf4eb5c3081 127 t2.reset();
cittecla 72:4e8a151d804e 128 float dist_r = getDistanceIR(0);
cittecla 72:4e8a151d804e 129 float dist_l = getDistanceIR(4);
cittecla 72:4e8a151d804e 130 printf("distances %f || %f\r\n",dist_l, dist_r);
cittecla 12:91c2e07d2b5b 131
cittecla 62:c2fcf3b349e9 132 //right
cittecla 62:c2fcf3b349e9 133 if(dist_r < last_dist_r) {
cittecla 62:c2fcf3b349e9 134 last_dist_r = dist_r;
cittecla 112:b3270806629f 135
cittecla 112:b3270806629f 136
cittecla 120:cdf7a6751f9e 137 deg_r += 3;
cittecla 120:cdf7a6751f9e 138 set_servo_position(2,deg_r);
cittecla 134:5c29654ce301 139
cittecla 120:cdf7a6751f9e 140 } else if(current_coord.y == 0) {
cittecla 120:cdf7a6751f9e 141 deg_r -= 5; //compensate overturning
cittecla 120:cdf7a6751f9e 142 //deg_r -= 3; // compensate not straight looking
cittecla 70:922cbbfebf02 143 dist_r *= 100;
cittecla 120:cdf7a6751f9e 144 float offsetx = 11.5;
cittecla 120:cdf7a6751f9e 145 float offsety = 11;
aeschsim 125:573a18c2155f 146 float arctan_a_b = 46.273f;
aeschsim 125:573a18c2155f 147 float c = sqrt(offsetx*offsetx+offsety*offsety);
cittecla 62:c2fcf3b349e9 148
aeschsim 125:573a18c2155f 149 current_coord.y = dist_r + c * cos( ( (90-fabsf(deg_r)-arctan_a_b) / 180.0f)*(float)M_PI);
cittecla 70:922cbbfebf02 150
cittecla 70:922cbbfebf02 151
cittecla 62:c2fcf3b349e9 152 }
cittecla 80:92b9d083322d 153
cittecla 62:c2fcf3b349e9 154 //left
cittecla 70:922cbbfebf02 155 if(dist_l < last_dist_l) {
cittecla 70:922cbbfebf02 156 last_dist_l = dist_l;
cittecla 70:922cbbfebf02 157 deg_l -= 5;
cittecla 120:cdf7a6751f9e 158 set_servo_position(0,deg_l);
cittecla 134:5c29654ce301 159
cittecla 120:cdf7a6751f9e 160 } else if(current_coord.x == 0) {
cittecla 120:cdf7a6751f9e 161 deg_l += 3; //compensate overturning
cittecla 120:cdf7a6751f9e 162 deg_l -= 3; //compensate not straight looking
aeschsim 125:573a18c2155f 163 dist_l *= 100;
cittecla 120:cdf7a6751f9e 164 float offsetx = 11.5;
cittecla 120:cdf7a6751f9e 165 float offsety = 11;
aeschsim 125:573a18c2155f 166 float arctan_a_b = 46.273f;
aeschsim 125:573a18c2155f 167 float c = sqrt(offsetx*offsetx+offsety*offsety);
cittecla 70:922cbbfebf02 168
aeschsim 125:573a18c2155f 169 current_coord.x = dist_l + c * cos( ( (90-fabsf(deg_l)-arctan_a_b) / 180.0f)*(float)M_PI);
cittecla 70:922cbbfebf02 170
cittecla 70:922cbbfebf02 171 }
cittecla 70:922cbbfebf02 172
cittecla 70:922cbbfebf02 173 if(current_coord.x != 0 && current_coord.y != 0) {
cittecla 134:5c29654ce301 174
cittecla 120:cdf7a6751f9e 175 printf("degs: %d || %d \r\n", deg_l , deg_r);
cittecla 120:cdf7a6751f9e 176 printf("coords : %f || %f \r\n", current_coord.x, current_coord.y);
cittecla 120:cdf7a6751f9e 177 current_heading = (360-deg_r + 270-deg_l)/2;
cittecla 70:922cbbfebf02 178
cittecla 70:922cbbfebf02 179 clamp_heading();
cittecla 70:922cbbfebf02 180 printf("heading: %f \r\n", current_heading);
cittecla 134:5c29654ce301 181
cittecla 120:cdf7a6751f9e 182 current_coord.x = 20;
cittecla 120:cdf7a6751f9e 183 current_coord.y = 20;
cittecla 70:922cbbfebf02 184
cittecla 70:922cbbfebf02 185 //finished
cittecla 120:cdf7a6751f9e 186 return 11;
cittecla 70:922cbbfebf02 187
cittecla 80:92b9d083322d 188 }
cittecla 62:c2fcf3b349e9 189 }
cittecla 33:8a98f8b9d859 190 }
cittecla 70:922cbbfebf02 191 // not finished
cittecla 120:cdf7a6751f9e 192 return 17;
cittecla 62:c2fcf3b349e9 193 }
cittecla 62:c2fcf3b349e9 194
cittecla 62:c2fcf3b349e9 195