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@120:cdf7a6751f9e, 2017-05-12 (annotated)

Committer:
cittecla
Date:
Fri May 12 15:20:45 2017 +0000
Revision:
120:cdf7a6751f9e
Parent:
117:66d64dbd1b36
Child:
122:8bfb39434fb7
Initial positioning is more or less working; distance calcualtion is not correct, State machine set to run through until move with list

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