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/Movement.cpp@135:f31d24150f5e, 2017-05-17 (annotated)

Committer:
PESGruppe1
Date:
Wed May 17 08:16:33 2017 +0000
Revision:
135:f31d24150f5e
Parent:
134:5c29654ce301 
New Move in search for brick with obstacle edge detection;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 116:e03a3692cdf0 1 /**
cittecla 18:a82994e67297 2 * Movement function library
cittecla 18:a82994e67297 3 * Handels Movement of the Robot
cittecla 18:a82994e67297 4 **/
cittecla 39:92723f7ea54f 5
cittecla 39:92723f7ea54f 6 #include "Movement.h"
PESGruppe1 114:720dc5df42a5 7 #define OFFSET_GREIFER_TO_IRSENSOR 0.2 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
cittecla 109:d18a2beb9b9b 8 #define OFFSET_WHEELS 0.09 // Offset of the wheels from the max pos
cittecla 39:92723f7ea54f 9
cittecla 85:d8ea8a99fa3a 10 bool is_moving = false;
cittecla 85:d8ea8a99fa3a 11 float wanted_dist = 0;
cittecla 61:628f8a4e857c 12 bool is_turning = false;
cittecla 61:628f8a4e857c 13 float wanted_deg = 0;
cittecla 105:d489c2e8de35 14 bool direction = false;
PESGruppe1 114:720dc5df42a5 15 float restdegAfterstop = 0; // Variable for Rest degree we still have to cover after e.g. 1 brick found but its not really a brick so we turn further until e.g 60 degrees covered.
cittecla 122:8bfb39434fb7 16 float needed_heading = 0;
cittecla 122:8bfb39434fb7 17 float distance_to_next_coord = 0;
cittecla 122:8bfb39434fb7 18 float current_head = 0;
cittecla 123:de12131ff0b9 19 coordinates current_coordinates = {0};
cittecla 123:de12131ff0b9 20 coordinates next_coord = {0};
PESGruppe1 114:720dc5df42a5 21
cittecla 116:e03a3692cdf0 22 float TOLERANCE_BRICK_OR_OBSTACLE = 0.08f; // Variable for Brick detection it sets how much upper sensor and lower can differ that its still detected as an obstacle and not a brick
cittecla 105:d489c2e8de35 23
cittecla 63:b27aa01c2cf6 24 Timer t;
PESGruppe1 114:720dc5df42a5 25 Timer t8; // timer used for waiting enough distance measurements
cittecla 105:d489c2e8de35 26
PESGruppe1 135:f31d24150f5e 27 int search_state = 0; // state for Move in search for brick statemachine
cittecla 120:cdf7a6751f9e 28 int coord_move_state = 0;
cittecla 120:cdf7a6751f9e 29 int list_step = 0;
PESGruppe1 135:f31d24150f5e 30 int sigma_brick = 0; // Count how many times a brick is detected in fine postioning in statemachine move in search for brick
cittecla 77:ff87a10c4baf 31
cittecla 94:0381e8b1beda 32 float left = 0;
cittecla 94:0381e8b1beda 33 float right = 0;
cittecla 94:0381e8b1beda 34
cittecla 94:0381e8b1beda 35 bool devider = true;
cittecla 94:0381e8b1beda 36
cittecla 61:628f8a4e857c 37
cittecla 52:56399c2f13cd 38 int moving()
cittecla 52:56399c2f13cd 39 {
cittecla 52:56399c2f13cd 40
cittecla 39:92723f7ea54f 41 return 0;
cittecla 52:56399c2f13cd 42 }
cittecla 52:56399c2f13cd 43
cittecla 106:02d3327bf76a 44 /**
cittecla 106:02d3327bf76a 45 * Stops current movement immediately
cittecla 106:02d3327bf76a 46 **/
cittecla 86:df8c869a5a52 47 void stop_move()
cittecla 86:df8c869a5a52 48 {
cittecla 86:df8c869a5a52 49 set_speed(0,0);
cittecla 86:df8c869a5a52 50 wanted_dist = 0;
cittecla 86:df8c869a5a52 51 is_moving = false;
cittecla 86:df8c869a5a52 52 }
cittecla 86:df8c869a5a52 53
cittecla 106:02d3327bf76a 54 /**
cittecla 106:02d3327bf76a 55 * Stops current turn immediately
cittecla 106:02d3327bf76a 56 **/
cittecla 86:df8c869a5a52 57 void stop_turn()
cittecla 86:df8c869a5a52 58 {
cittecla 86:df8c869a5a52 59 set_speed(0,0);
cittecla 86:df8c869a5a52 60 wanted_deg = 0;
cittecla 86:df8c869a5a52 61 is_turning = false;
cittecla 86:df8c869a5a52 62 }
cittecla 86:df8c869a5a52 63
cittecla 106:02d3327bf76a 64
cittecla 106:02d3327bf76a 65 /**
cittecla 106:02d3327bf76a 66 * move for wanted distance on circle with a given radius
cittecla 106:02d3327bf76a 67 * needs to be called until return < 0
cittecla 106:02d3327bf76a 68 * if calling distance not 0: distance and radius initilisation.
cittecla 106:02d3327bf76a 69 * by Claudio Citterio
cittecla 106:02d3327bf76a 70 **/
cittecla 105:d489c2e8de35 71 float move_for_distance_with_radius(float distance, float r)
cittecla 105:d489c2e8de35 72 {
cittecla 105:d489c2e8de35 73
cittecla 105:d489c2e8de35 74 if(distance != 0) {
cittecla 105:d489c2e8de35 75
cittecla 105:d489c2e8de35 76 is_moving = true;
cittecla 105:d489c2e8de35 77 wanted_dist = fabsf(distance);
cittecla 105:d489c2e8de35 78
cittecla 105:d489c2e8de35 79 float circumference = r*2*(float)M_PI;
cittecla 105:d489c2e8de35 80 float circumference_inner = ((r-(float)OFFSET_WHEELS)*2*(float)M_PI);
cittecla 105:d489c2e8de35 81 float circumference_outer = ((r+(float)OFFSET_WHEELS)*2*(float)M_PI);
cittecla 105:d489c2e8de35 82
cittecla 109:d18a2beb9b9b 83 float max_speed = 50;
cittecla 109:d18a2beb9b9b 84 float inner_speed = max_speed/circumference*circumference_inner;
cittecla 109:d18a2beb9b9b 85 float outer_speed = max_speed/circumference*circumference_outer;
cittecla 105:d489c2e8de35 86
cittecla 109:d18a2beb9b9b 87 //reduce outer speed to max speed
cittecla 109:d18a2beb9b9b 88 float multiplier = 1.0f/inner_speed*max_speed;
cittecla 109:d18a2beb9b9b 89 inner_speed *= multiplier;
cittecla 109:d18a2beb9b9b 90 outer_speed *= multiplier;
cittecla 105:d489c2e8de35 91
cittecla 109:d18a2beb9b9b 92 if(r < 0.21f) {
cittecla 113:c7afe49752b9 93 outer_speed *= 0.8f;
cittecla 113:c7afe49752b9 94 inner_speed *= 0.8f;
cittecla 109:d18a2beb9b9b 95 }
cittecla 105:d489c2e8de35 96
cittecla 108:02bc5b4e67b7 97 if(r != 0) {
cittecla 108:02bc5b4e67b7 98 //move with turn
cittecla 108:02bc5b4e67b7 99 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 100 direction = 1;
cittecla 105:d489c2e8de35 101 left = outer_speed;
cittecla 105:d489c2e8de35 102 right = inner_speed;
cittecla 105:d489c2e8de35 103 } else { //move backward
cittecla 105:d489c2e8de35 104 direction = 0;
cittecla 105:d489c2e8de35 105 left = -outer_speed;
cittecla 105:d489c2e8de35 106 right = -inner_speed;
cittecla 105:d489c2e8de35 107 }
cittecla 105:d489c2e8de35 108 } else {
cittecla 105:d489c2e8de35 109 //normal straight movement
cittecla 108:02bc5b4e67b7 110 printf("move straight\r\n");
cittecla 105:d489c2e8de35 111 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 112 direction = 1;
cittecla 109:d18a2beb9b9b 113 left = max_speed;
cittecla 109:d18a2beb9b9b 114 right = max_speed;
cittecla 105:d489c2e8de35 115 } else { //move backward
cittecla 105:d489c2e8de35 116 direction = 0;
cittecla 109:d18a2beb9b9b 117 left = -max_speed;
cittecla 109:d18a2beb9b9b 118 right = -max_speed;
cittecla 105:d489c2e8de35 119 }
cittecla 105:d489c2e8de35 120 }
cittecla 105:d489c2e8de35 121
cittecla 105:d489c2e8de35 122 set_speed(left, right);
cittecla 105:d489c2e8de35 123 devider = true;
cittecla 105:d489c2e8de35 124 t.reset();
cittecla 105:d489c2e8de35 125 t.start();
cittecla 105:d489c2e8de35 126 } else {
cittecla 105:d489c2e8de35 127 float speed_multiplier = 0.6f;
cittecla 105:d489c2e8de35 128 if(wanted_dist < 0.10f && devider == true) {
cittecla 105:d489c2e8de35 129 //printf("devided\r\n");
cittecla 105:d489c2e8de35 130 devider = false;
cittecla 105:d489c2e8de35 131 left = left * speed_multiplier;
cittecla 105:d489c2e8de35 132 right = right * speed_multiplier;
cittecla 105:d489c2e8de35 133 //printf("left: %f || right: %f\r\n", left, right);
cittecla 105:d489c2e8de35 134 set_speed(left, right);
cittecla 105:d489c2e8de35 135 }
cittecla 105:d489c2e8de35 136
cittecla 105:d489c2e8de35 137 float speed_left = get_speed_left();
cittecla 105:d489c2e8de35 138 float speed_right = get_speed_right();
cittecla 105:d489c2e8de35 139 wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * ((fabsf(speed_left)+fabsf(speed_right))/2) * 0.1f;
cittecla 105:d489c2e8de35 140 t.reset();
cittecla 105:d489c2e8de35 141
cittecla 105:d489c2e8de35 142 if(wanted_dist <= 0) { //distance covered, Stop function
cittecla 105:d489c2e8de35 143 set_speed(0,0);
cittecla 105:d489c2e8de35 144 is_moving = false;
cittecla 105:d489c2e8de35 145 t.stop();
cittecla 105:d489c2e8de35 146 }
cittecla 105:d489c2e8de35 147 }
cittecla 105:d489c2e8de35 148 printf("remaining distance to cover: %f\r\n", wanted_dist);
cittecla 105:d489c2e8de35 149 return wanted_dist;
cittecla 105:d489c2e8de35 150 }
cittecla 105:d489c2e8de35 151
cittecla 106:02d3327bf76a 152 /**
cittecla 106:02d3327bf76a 153 * move for wanted distance
cittecla 106:02d3327bf76a 154 * needs to be called until return < 0
cittecla 106:02d3327bf76a 155 * if calling distance not 0: distance initilisation.
cittecla 106:02d3327bf76a 156 * by Claudio Citterio
cittecla 106:02d3327bf76a 157 **/
cittecla 85:d8ea8a99fa3a 158 float move_for_distance(float distance)
cittecla 52:56399c2f13cd 159 {
PESGruppe1 135:f31d24150f5e 160 printf("move for distance\r\n");
cittecla 85:d8ea8a99fa3a 161 if(distance != 0) {
cittecla 52:56399c2f13cd 162
cittecla 85:d8ea8a99fa3a 163 is_moving = true;
cittecla 85:d8ea8a99fa3a 164
cittecla 105:d489c2e8de35 165 wanted_dist = fabsf(distance);
cittecla 85:d8ea8a99fa3a 166
cittecla 85:d8ea8a99fa3a 167 if(distance > 0) { //move forward
cittecla 85:d8ea8a99fa3a 168 direction = 1;
cittecla 85:d8ea8a99fa3a 169 left = 50.0f;
cittecla 85:d8ea8a99fa3a 170 right = 50.0f;
cittecla 85:d8ea8a99fa3a 171 } else { //move backward
cittecla 85:d8ea8a99fa3a 172 direction = 0;
cittecla 85:d8ea8a99fa3a 173 left = -50.0f;
cittecla 85:d8ea8a99fa3a 174 right = -50.0f;
cittecla 85:d8ea8a99fa3a 175 }
cittecla 88:b89cace9329b 176 printf("set speed %f\r\n", left);
cittecla 85:d8ea8a99fa3a 177 set_speed(left, right);
cittecla 94:0381e8b1beda 178 devider = true;
cittecla 85:d8ea8a99fa3a 179 t.reset();
cittecla 85:d8ea8a99fa3a 180 t.start();
cittecla 85:d8ea8a99fa3a 181
cittecla 85:d8ea8a99fa3a 182 } else {
cittecla 94:0381e8b1beda 183 float speed_multiplier = 0.6f;
cittecla 94:0381e8b1beda 184 if(wanted_dist < 0.10f && devider == true) {
cittecla 94:0381e8b1beda 185 //printf("devided\r\n");
cittecla 94:0381e8b1beda 186 devider = false;
aeschsim 99:78d87027c85b 187 left = left * speed_multiplier;
aeschsim 99:78d87027c85b 188 right = right * speed_multiplier;
aeschsim 99:78d87027c85b 189 //printf("left: %f || right: %f\r\n", left, right);
cittecla 94:0381e8b1beda 190 set_speed(left, right);
cittecla 94:0381e8b1beda 191 }
cittecla 85:d8ea8a99fa3a 192
cittecla 85:d8ea8a99fa3a 193 float speed_left = get_speed_left();
PESGruppe1 135:f31d24150f5e 194 printf("speed left: %f\r\n", speed_left);
cittecla 89:7f9d6e641a01 195 wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
cittecla 85:d8ea8a99fa3a 196 t.reset();
cittecla 85:d8ea8a99fa3a 197
cittecla 85:d8ea8a99fa3a 198 if(wanted_dist <= 0) { //distance covered, Stop function
cittecla 85:d8ea8a99fa3a 199 set_speed(0,0);
cittecla 85:d8ea8a99fa3a 200 is_moving = false;
cittecla 85:d8ea8a99fa3a 201 t.stop();
cittecla 85:d8ea8a99fa3a 202 }
cittecla 85:d8ea8a99fa3a 203 }
PESGruppe1 135:f31d24150f5e 204 printf("remaining distance to cover: %f\r\n", wanted_dist);
cittecla 85:d8ea8a99fa3a 205 return wanted_dist;
cittecla 52:56399c2f13cd 206 }
cittecla 52:56399c2f13cd 207
cittecla 106:02d3327bf76a 208 /**
cittecla 106:02d3327bf76a 209 * turn for wanted degree
cittecla 106:02d3327bf76a 210 * needs to be called until return < 0
cittecla 106:02d3327bf76a 211 * if deg not 0: turn initilisation.
cittecla 106:02d3327bf76a 212 * Claudio Citterio
cittecla 106:02d3327bf76a 213 **/
PESGruppe1 114:720dc5df42a5 214 float turn_for_deg(float deg, float multiplier)
cittecla 52:56399c2f13cd 215 {
cittecla 52:56399c2f13cd 216
cittecla 75:dba260cb5ae4 217 if(deg != 0) {
cittecla 61:628f8a4e857c 218
cittecla 61:628f8a4e857c 219 is_turning = true;
cittecla 94:0381e8b1beda 220 wanted_deg = fabsf(deg);
cittecla 61:628f8a4e857c 221
cittecla 61:628f8a4e857c 222 if(deg < 0) { // turn left
cittecla 61:628f8a4e857c 223 direction = 1;
PESGruppe1 114:720dc5df42a5 224 left = -20.0f*multiplier;
PESGruppe1 114:720dc5df42a5 225 right = 20.0f*multiplier;
cittecla 61:628f8a4e857c 226 } else { // turn right
cittecla 61:628f8a4e857c 227 direction = 0;
PESGruppe1 114:720dc5df42a5 228 left = 20.0f*multiplier;
PESGruppe1 114:720dc5df42a5 229 right = -20.0f*multiplier;
cittecla 61:628f8a4e857c 230 }
cittecla 61:628f8a4e857c 231 set_speed(left, right);
cittecla 94:0381e8b1beda 232 devider = true;
cittecla 71:ddf4eb5c3081 233 t.reset();
cittecla 61:628f8a4e857c 234 t.start();
cittecla 61:628f8a4e857c 235
cittecla 61:628f8a4e857c 236 } else {
cittecla 94:0381e8b1beda 237 float speed_multiplier = 0.6f;
cittecla 94:0381e8b1beda 238 if(wanted_deg < 10.0f && devider == true) {
cittecla 94:0381e8b1beda 239 devider = false;
aeschsim 99:78d87027c85b 240 left = left * speed_multiplier;
aeschsim 99:78d87027c85b 241 right = right * speed_multiplier;
cittecla 94:0381e8b1beda 242 set_speed(left, right);
cittecla 94:0381e8b1beda 243 }
cittecla 101:6b10685aa34d 244
cittecla 61:628f8a4e857c 245 float speed_left = get_speed_left();
cittecla 94:0381e8b1beda 246 wanted_deg -= 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f);
cittecla 75:dba260cb5ae4 247 t.reset();
cittecla 75:dba260cb5ae4 248 if(wanted_deg <= 0) {
cittecla 61:628f8a4e857c 249 set_speed(0,0);
cittecla 61:628f8a4e857c 250 is_turning = false;
cittecla 61:628f8a4e857c 251 t.stop();
cittecla 61:628f8a4e857c 252 }
cittecla 61:628f8a4e857c 253 }
PESGruppe1 135:f31d24150f5e 254 printf("remaining deg %f\r\n", wanted_deg);
cittecla 75:dba260cb5ae4 255 return (wanted_deg);
cittecla 52:56399c2f13cd 256 }
cittecla 52:56399c2f13cd 257
cittecla 106:02d3327bf76a 258 /** has errors
cittecla 106:02d3327bf76a 259 * moves to next coordinate from coordinate list
cittecla 106:02d3327bf76a 260 * by Claudio Citterio
cittecla 106:02d3327bf76a 261 **/
cittecla 122:8bfb39434fb7 262
cittecla 122:8bfb39434fb7 263 int move_to_brick_by_coordlist()
cittecla 120:cdf7a6751f9e 264 {
cittecla 120:cdf7a6751f9e 265 switch(coord_move_state) {
cittecla 120:cdf7a6751f9e 266 case 0:
cittecla 120:cdf7a6751f9e 267 // init move to brick by list
cittecla 120:cdf7a6751f9e 268 list_step = 1;
cittecla 52:56399c2f13cd 269
cittecla 120:cdf7a6751f9e 270 coord_move_state = 1;
cittecla 120:cdf7a6751f9e 271 break;
cittecla 120:cdf7a6751f9e 272 case 1:
cittecla 120:cdf7a6751f9e 273 // get positions, coords, heading and distances
cittecla 122:8bfb39434fb7 274 current_head = get_current_heading();
cittecla 123:de12131ff0b9 275 current_coordinates = get_current_coord();
cittecla 123:de12131ff0b9 276 position next_position = walkpath[list_step];
cittecla 123:de12131ff0b9 277 next_coord = pos_to_coord(next_position);
PESGruppe1 135:f31d24150f5e 278
PESGruppe1 135:f31d24150f5e 279 coord_move_state = 2;
PESGruppe1 135:f31d24150f5e 280 break;
PESGruppe1 135:f31d24150f5e 281 case 2:
cittecla 120:cdf7a6751f9e 282 // check if path is still possible with updated map or target reached
PESGruppe1 135:f31d24150f5e 283
PESGruppe1 135:f31d24150f5e 284 if(next_position.x == 0 && next_position.y == 0) {
PESGruppe1 135:f31d24150f5e 285 // target reached
cittecla 120:cdf7a6751f9e 286 coord_move_state = 0;
cittecla 120:cdf7a6751f9e 287 return 47;
cittecla 120:cdf7a6751f9e 288 }
cittecla 123:de12131ff0b9 289 if(obstacle_list[next_position.x][next_position.y] != 0) {
PESGruppe1 135:f31d24150f5e 290 // path obstructed
cittecla 120:cdf7a6751f9e 291 coord_move_state = 0;
cittecla 120:cdf7a6751f9e 292 return 35;
cittecla 120:cdf7a6751f9e 293 }
cittecla 123:de12131ff0b9 294 list_step += 1;
cittecla 120:cdf7a6751f9e 295 coord_move_state = 3;
cittecla 120:cdf7a6751f9e 296 break;
cittecla 120:cdf7a6751f9e 297 case 3:
PESGruppe1 135:f31d24150f5e 298 // calc new headings
PESGruppe1 135:f31d24150f5e 299 float x = next_coord.x - current_coordinates.x;
PESGruppe1 135:f31d24150f5e 300 float y = next_coord.y - current_coordinates.y;
cittecla 123:de12131ff0b9 301 distance_to_next_coord = sqrt(x*x + y*y);
PESGruppe1 135:f31d24150f5e 302
cittecla 123:de12131ff0b9 303 needed_heading = 90 + (atan(-y / x)/(float)M_PI * 180.0f)*-1.0f;
cittecla 123:de12131ff0b9 304 if (x < 0) needed_heading += 180;
PESGruppe1 135:f31d24150f5e 305
cittecla 122:8bfb39434fb7 306 if(needed_heading != current_head) {
cittecla 120:cdf7a6751f9e 307 coord_move_state = 5;
cittecla 120:cdf7a6751f9e 308 } else {
cittecla 120:cdf7a6751f9e 309 coord_move_state = 8;
cittecla 120:cdf7a6751f9e 310 }
PESGruppe1 135:f31d24150f5e 311 break;
cittecla 120:cdf7a6751f9e 312
cittecla 120:cdf7a6751f9e 313 case 5:
cittecla 120:cdf7a6751f9e 314 // turn init with new heading
PESGruppe1 135:f31d24150f5e 315 turn_for_deg(needed_heading-current_head,1);
cittecla 122:8bfb39434fb7 316 coord_move_state = 6;
cittecla 120:cdf7a6751f9e 317 break;
cittecla 120:cdf7a6751f9e 318 case 6:
cittecla 120:cdf7a6751f9e 319 //turn until new heading == heading
cittecla 122:8bfb39434fb7 320 if(turn_for_deg(0,1)<0) {
cittecla 122:8bfb39434fb7 321 stop_turn();
cittecla 122:8bfb39434fb7 322 coord_move_state = 8;
cittecla 122:8bfb39434fb7 323 }
cittecla 120:cdf7a6751f9e 324 break;
cittecla 120:cdf7a6751f9e 325
cittecla 120:cdf7a6751f9e 326 case 8:
cittecla 120:cdf7a6751f9e 327 // move init with distance
cittecla 122:8bfb39434fb7 328 move_for_distance(distance_to_next_coord);
cittecla 122:8bfb39434fb7 329 coord_move_state = 9;
cittecla 120:cdf7a6751f9e 330 break;
cittecla 120:cdf7a6751f9e 331 case 9:
cittecla 123:de12131ff0b9 332 // move until distance is covered
cittecla 122:8bfb39434fb7 333 if(move_for_distance(0)<0) {
cittecla 122:8bfb39434fb7 334 stop_move();
cittecla 123:de12131ff0b9 335 coord_move_state = 1;
cittecla 122:8bfb39434fb7 336 }
cittecla 122:8bfb39434fb7 337 break;
cittecla 61:628f8a4e857c 338 }
cittecla 122:8bfb39434fb7 339 return 45;
cittecla 122:8bfb39434fb7 340 }
cittecla 52:56399c2f13cd 341
cittecla 52:56399c2f13cd 342
cittecla 108:02bc5b4e67b7 343 /**
cittecla 106:02d3327bf76a 344 * this function searchs a nearby brick, moves towards it and grabbs it
cittecla 106:02d3327bf76a 345 * by Tobias Berger, state machine by Claudio Citterio
PESGruppe1 135:f31d24150f5e 346 * returns 1 if brick found or 0 if nothing found
cittecla 106:02d3327bf76a 347 **/
cittecla 52:56399c2f13cd 348 int move_in_search_for_brick()
cittecla 52:56399c2f13cd 349 {
PESGruppe1 135:f31d24150f5e 350
cittecla 109:d18a2beb9b9b 351 float upper = getDistanceIR(2); // get distance from upper max Sensor
cittecla 109:d18a2beb9b9b 352 float lower = getDistanceIR(3); // get distance from Lower max Sensor
cittecla 113:c7afe49752b9 353 //printf("Current Search State: >%d<\r\n",search_state);
cittecla 86:df8c869a5a52 354 switch (search_state) {
cittecla 86:df8c869a5a52 355 case 0: //first cycle right
cittecla 117:66d64dbd1b36 356 turn_for_deg(60.0f,0.8f); // call function and start turning
cittecla 86:df8c869a5a52 357 search_state = 1;
cittecla 86:df8c869a5a52 358 break;
cittecla 85:d8ea8a99fa3a 359
PESGruppe1 114:720dc5df42a5 360 case 1: // turn right and check for obstacles
cittecla 116:e03a3692cdf0 361 if((lower<0.45f)&&(lower>0.08f)) { // if something is in the range of 10 to 80cm at the lower Sensor
PESGruppe1 114:720dc5df42a5 362 if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
cittecla 86:df8c869a5a52 363 stop_turn();
PESGruppe1 135:f31d24150f5e 364 restdegAfterstop = turn_for_deg(0,1); // get restdegrees from turn function. if a brick is falsly detected we turn restdegAfterstop to finisch search turn
PESGruppe1 135:f31d24150f5e 365 search_state = 30; // go in Fine Positioning statemachine
PESGruppe1 114:720dc5df42a5 366 printf("Brick first detetection lower: %f upper:%f",lower,upper);
cittecla 86:df8c869a5a52 367 }
cittecla 86:df8c869a5a52 368 } else {
PESGruppe1 135:f31d24150f5e 369 search_state = 1; // go to same state continue turning
PESGruppe1 114:720dc5df42a5 370 if(turn_for_deg(0, 1) < 0) { // when first 60degree rotation finished
cittecla 86:df8c869a5a52 371 stop_turn();
PESGruppe1 135:f31d24150f5e 372 search_state = 4; // go to init turn other direction bc nothing found
PESGruppe1 114:720dc5df42a5 373 }
PESGruppe1 114:720dc5df42a5 374 }
PESGruppe1 114:720dc5df42a5 375 break;
PESGruppe1 114:720dc5df42a5 376
PESGruppe1 114:720dc5df42a5 377
PESGruppe1 135:f31d24150f5e 378 case 2: // init continue turning for restdeg
cittecla 117:66d64dbd1b36 379 turn_for_deg(restdegAfterstop,0.8f); // call function and start turning for restdegrees after stop
PESGruppe1 114:720dc5df42a5 380 search_state = 1; // go back to turn and search
PESGruppe1 114:720dc5df42a5 381 break;
PESGruppe1 114:720dc5df42a5 382
PESGruppe1 114:720dc5df42a5 383 case 4: // init turn left 120 deg
cittecla 117:66d64dbd1b36 384 turn_for_deg(-120.0f,0.8f);
PESGruppe1 114:720dc5df42a5 385 search_state = 5;
PESGruppe1 114:720dc5df42a5 386 break;
PESGruppe1 114:720dc5df42a5 387
PESGruppe1 114:720dc5df42a5 388 case 5: // turn and search opposite direction
PESGruppe1 114:720dc5df42a5 389 if((lower<0.45f)&&(lower>0.05f)) { // if something is in the range of 10 to 80cm at the lower Sensor
PESGruppe1 114:720dc5df42a5 390 if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
PESGruppe1 114:720dc5df42a5 391 stop_turn();
PESGruppe1 135:f31d24150f5e 392 restdegAfterstop = (-1*turn_for_deg(0,1)); // get restdegrees from turn function. if a brick is falsly detected we turn restdegAfterstop to finisch search turn
PESGruppe1 135:f31d24150f5e 393 search_state = 30; // goto fine postioning stateamchine
PESGruppe1 114:720dc5df42a5 394 printf("Brick first detetection lower: %f upper:%f",lower,upper);
PESGruppe1 114:720dc5df42a5 395 }
PESGruppe1 114:720dc5df42a5 396 } else {
PESGruppe1 114:720dc5df42a5 397 search_state = 5; // go to same state
PESGruppe1 114:720dc5df42a5 398 if(turn_for_deg(0, 1) < 0) { // when 60degree rotation finished
PESGruppe1 114:720dc5df42a5 399 stop_turn();
PESGruppe1 135:f31d24150f5e 400 search_state = 100; // error go to default state, bc nothing found
cittecla 86:df8c869a5a52 401 }
cittecla 86:df8c869a5a52 402 }
cittecla 86:df8c869a5a52 403 break;
cittecla 85:d8ea8a99fa3a 404
PESGruppe1 135:f31d24150f5e 405 case 6:// init continue turning for restdeg
PESGruppe1 135:f31d24150f5e 406 turn_for_deg((-1*restdegAfterstop),0.8f); // call function and start turning for restdegrees after stop
PESGruppe1 135:f31d24150f5e 407 search_state = 5; // go back to turn and search
PESGruppe1 135:f31d24150f5e 408 break;
PESGruppe1 135:f31d24150f5e 409
PESGruppe1 135:f31d24150f5e 410 //-finepositioning-------------------------------------------------------------------------------------------------------------------------------------------------------------
PESGruppe1 135:f31d24150f5e 411 case 30: // Start Statemachine fine positioning Init turn right for 3 degres
PESGruppe1 135:f31d24150f5e 412 turn_for_deg(0.5f,0.8f);
PESGruppe1 135:f31d24150f5e 413 search_state=31;
PESGruppe1 135:f31d24150f5e 414 break;
PESGruppe1 135:f31d24150f5e 415
PESGruppe1 135:f31d24150f5e 416 case 31: // Wait until rotation finished
PESGruppe1 135:f31d24150f5e 417 if(turn_for_deg(0,0.8f)<0) {
PESGruppe1 135:f31d24150f5e 418 stop_turn();
PESGruppe1 135:f31d24150f5e 419 search_state=32;
PESGruppe1 135:f31d24150f5e 420 } else {
PESGruppe1 135:f31d24150f5e 421 search_state=31; // As long as turn not finisched call case 1 wait until finished
PESGruppe1 135:f31d24150f5e 422 }
PESGruppe1 135:f31d24150f5e 423 break;
PESGruppe1 135:f31d24150f5e 424
PESGruppe1 135:f31d24150f5e 425
PESGruppe1 135:f31d24150f5e 426 case 32: // Measure if Brick or Obstacle
PESGruppe1 135:f31d24150f5e 427 if((lower<0.45f)&&(lower>0.08f)) { // if something is in the range of 10 to 80cm at the lower Sensor
PESGruppe1 135:f31d24150f5e 428 if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
PESGruppe1 135:f31d24150f5e 429 sigma_brick+=1; // increment sigma brick
PESGruppe1 135:f31d24150f5e 430
PESGruppe1 135:f31d24150f5e 431 }
PESGruppe1 135:f31d24150f5e 432 }
PESGruppe1 135:f31d24150f5e 433 search_state=33;
PESGruppe1 135:f31d24150f5e 434 break;
PESGruppe1 135:f31d24150f5e 435
PESGruppe1 135:f31d24150f5e 436 case 33: // Init turn left direction for 3 degres
PESGruppe1 135:f31d24150f5e 437 turn_for_deg(-1.0f,0.8f);
PESGruppe1 135:f31d24150f5e 438 search_state=34;
PESGruppe1 135:f31d24150f5e 439 break;
PESGruppe1 135:f31d24150f5e 440
PESGruppe1 135:f31d24150f5e 441 case 34: // Wait until rotation left finished
PESGruppe1 135:f31d24150f5e 442 if(turn_for_deg(0,0.8f)<0) {
PESGruppe1 135:f31d24150f5e 443 stop_turn();
PESGruppe1 135:f31d24150f5e 444 printf("turn left after STOP");
PESGruppe1 135:f31d24150f5e 445 search_state=35;
PESGruppe1 135:f31d24150f5e 446 } else {
PESGruppe1 135:f31d24150f5e 447 search_state=34; // As long as turn not finisched call case 1 wait until finished
PESGruppe1 135:f31d24150f5e 448 }
PESGruppe1 135:f31d24150f5e 449 break;
PESGruppe1 135:f31d24150f5e 450
PESGruppe1 135:f31d24150f5e 451 case 35: // Measure if Brick or Obstacle
PESGruppe1 135:f31d24150f5e 452 if((lower<0.45f)&&(lower>0.08f)) { // if something is in the range of 10 to 80cm at the lower Sensor
PESGruppe1 135:f31d24150f5e 453 if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
PESGruppe1 135:f31d24150f5e 454 sigma_brick+=1; // increment sigma brick
PESGruppe1 135:f31d24150f5e 455
PESGruppe1 135:f31d24150f5e 456 }
PESGruppe1 135:f31d24150f5e 457 }
PESGruppe1 135:f31d24150f5e 458 search_state=36;
PESGruppe1 135:f31d24150f5e 459 break;
PESGruppe1 135:f31d24150f5e 460
PESGruppe1 135:f31d24150f5e 461
PESGruppe1 135:f31d24150f5e 462 case 36: // Init Turn Back right in Middle Position
PESGruppe1 135:f31d24150f5e 463 turn_for_deg(0.5f,0.8f);
PESGruppe1 135:f31d24150f5e 464 search_state = 37;
PESGruppe1 135:f31d24150f5e 465 break;
PESGruppe1 135:f31d24150f5e 466
PESGruppe1 135:f31d24150f5e 467 case 37: // Turn until rotation finished
PESGruppe1 135:f31d24150f5e 468 if(turn_for_deg(0,0.8f)<0) {
PESGruppe1 135:f31d24150f5e 469 stop_turn();
PESGruppe1 135:f31d24150f5e 470 search_state=38;
PESGruppe1 135:f31d24150f5e 471 } else {
PESGruppe1 135:f31d24150f5e 472 search_state=37; // As long as turn not finisched call case 1 wait until finished
PESGruppe1 135:f31d24150f5e 473 }
PESGruppe1 135:f31d24150f5e 474
PESGruppe1 135:f31d24150f5e 475 break;
PESGruppe1 135:f31d24150f5e 476
PESGruppe1 135:f31d24150f5e 477
PESGruppe1 135:f31d24150f5e 478 case 38: // Evaluation
PESGruppe1 135:f31d24150f5e 479 if(sigma_brick > 1) { // If a brick is detected more than once got to move
PESGruppe1 135:f31d24150f5e 480 sigma_brick = 0; // reset sigma brick
PESGruppe1 135:f31d24150f5e 481 search_state = 50; // go to move forward bc brick found
PESGruppe1 135:f31d24150f5e 482 printf("brick found in finepositioning");
PESGruppe1 135:f31d24150f5e 483
PESGruppe1 135:f31d24150f5e 484 } else {
PESGruppe1 135:f31d24150f5e 485 printf("Nothing found in fine positioning");
PESGruppe1 135:f31d24150f5e 486 if(restdegAfterstop>0) {
PESGruppe1 135:f31d24150f5e 487 sigma_brick = 0; // reset sigma brick
PESGruppe1 135:f31d24150f5e 488 search_state = 2; // continue turning right bc nothing found
PESGruppe1 135:f31d24150f5e 489 } else {
PESGruppe1 135:f31d24150f5e 490 sigma_brick = 0; // reset sigma brick
PESGruppe1 135:f31d24150f5e 491 search_state = 6; // continue turning left bc nothing found
cittecla 86:df8c869a5a52 492 }
cittecla 86:df8c869a5a52 493 }
cittecla 86:df8c869a5a52 494 break;
cittecla 52:56399c2f13cd 495
PESGruppe1 135:f31d24150f5e 496 printf("SIGMA BRICK ---------------------------%d",sigma_brick);
PESGruppe1 135:f31d24150f5e 497
PESGruppe1 135:f31d24150f5e 498
PESGruppe1 135:f31d24150f5e 499
PESGruppe1 135:f31d24150f5e 500 // Move to found brick-----------------------------------------------------------------------------------------------
PESGruppe1 135:f31d24150f5e 501
PESGruppe1 135:f31d24150f5e 502 case 50: // first cycle move forward
PESGruppe1 135:f31d24150f5e 503 float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
PESGruppe1 135:f31d24150f5e 504 move_for_distance(distance_to_Brick);
PESGruppe1 135:f31d24150f5e 505 search_state = 51;
cittecla 101:6b10685aa34d 506 break;
cittecla 105:d489c2e8de35 507
PESGruppe1 135:f31d24150f5e 508 case 51: // move forward
cittecla 101:6b10685aa34d 509 if(move_for_distance(0) < 0) {
aeschsim 110:510927a51be8 510 //Safety Function:
cittecla 113:c7afe49752b9 511 if (getDistanceIR(2)<0.08f) {
aeschsim 110:510927a51be8 512 stop_move();
aeschsim 111:cd7b7b551616 513 //move_for_distance(-0.10f);
aeschsim 110:510927a51be8 514 search_state = 0;
aeschsim 110:510927a51be8 515 }
cittecla 101:6b10685aa34d 516 stop_move();
PESGruppe1 135:f31d24150f5e 517 search_state = 52;
cittecla 101:6b10685aa34d 518 }
cittecla 101:6b10685aa34d 519 break;
cittecla 101:6b10685aa34d 520
PESGruppe1 135:f31d24150f5e 521 case 52: // Grabbing
PESGruppe1 135:f31d24150f5e 522 return 50;
PESGruppe1 135:f31d24150f5e 523 break; // main state machine set as Grabbing
cittecla 94:0381e8b1beda 524
cittecla 86:df8c869a5a52 525 default:
aeschsim 96:1c6867536350 526 printf("default State - move in search for brick\r\n");
PESGruppe1 135:f31d24150f5e 527 return 60; // error nothing found go to Aeschlimans state
cittecla 86:df8c869a5a52 528 // error
cittecla 86:df8c869a5a52 529 break;
PESGruppe1 74:d9c387b83196 530 }
cittecla 86:df8c869a5a52 531 return 47; //called until function is done
cittecla 52:56399c2f13cd 532 }
cittecla 52:56399c2f13cd 533
cittecla 39:92723f7ea54f 534
cittecla 39:92723f7ea54f 535
cittecla 39:92723f7ea54f 536
cittecla 39:92723f7ea54f 537
cittecla 39:92723f7ea54f 538
cittecla 39:92723f7ea54f 539
cittecla 39:92723f7ea54f 540
PESGruppe1 135:f31d24150f5e 541
PESGruppe1 135:f31d24150f5e 542
PESGruppe1 135:f31d24150f5e 543
PESGruppe1 135:f31d24150f5e 544
PESGruppe1 135:f31d24150f5e 545
PESGruppe1 135:f31d24150f5e 546
PESGruppe1 135:f31d24150f5e 547