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@122:8bfb39434fb7, 2017-05-14 (annotated)

Committer:
cittecla
Date:
Sun May 14 08:35:35 2017 +0000
Revision:
122:8bfb39434fb7
Parent:
120:cdf7a6751f9e
Child:
123:de12131ff0b9
move to brick by coordinate list is now implemented, calculation is static -> transfer to non static should be done.

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