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@114:720dc5df42a5, 2017-05-10 (annotated)

Committer:
PESGruppe1
Date:
Wed May 10 07:45:33 2017 +0000
Revision:
114:720dc5df42a5
Parent:
113:c7afe49752b9
Child:
116:e03a3692cdf0
New Statemachine in searchforbrick with double check

Who changed what in which revision?

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