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@106:02d3327bf76a, 2017-05-05 (annotated)

Committer:
cittecla
Date:
Fri May 05 10:48:36 2017 +0000
Revision:
106:02d3327bf76a
Parent:
105:d489c2e8de35
Child:
107:8e8a7a9b6fd8
Child:
108:02bc5b4e67b7
added comments to movement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 18:a82994e67297 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"
cittecla 105:d489c2e8de35 7 #define OFFSET_GREIFER_TO_IRSENSOR 0.15 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
cittecla 105:d489c2e8de35 8 #define OFFSET_WHEELS 0.09 // Offset of the wheels from the center 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;
cittecla 105:d489c2e8de35 15
cittecla 63:b27aa01c2cf6 16 Timer t;
cittecla 105:d489c2e8de35 17
cittecla 86:df8c869a5a52 18 int search_state = 0;
cittecla 77:ff87a10c4baf 19
cittecla 94:0381e8b1beda 20 float left = 0;
cittecla 94:0381e8b1beda 21 float right = 0;
cittecla 94:0381e8b1beda 22
cittecla 94:0381e8b1beda 23 bool devider = true;
cittecla 94:0381e8b1beda 24
cittecla 61:628f8a4e857c 25
cittecla 52:56399c2f13cd 26 int moving()
cittecla 52:56399c2f13cd 27 {
cittecla 52:56399c2f13cd 28
cittecla 39:92723f7ea54f 29 return 0;
cittecla 52:56399c2f13cd 30 }
cittecla 52:56399c2f13cd 31
cittecla 106:02d3327bf76a 32 /**
cittecla 106:02d3327bf76a 33 * Stops current movement immediately
cittecla 106:02d3327bf76a 34 **/
cittecla 86:df8c869a5a52 35 void stop_move()
cittecla 86:df8c869a5a52 36 {
cittecla 86:df8c869a5a52 37 set_speed(0,0);
cittecla 86:df8c869a5a52 38 wanted_dist = 0;
cittecla 86:df8c869a5a52 39 is_moving = false;
cittecla 86:df8c869a5a52 40 }
cittecla 86:df8c869a5a52 41
cittecla 106:02d3327bf76a 42 /**
cittecla 106:02d3327bf76a 43 * Stops current turn immediately
cittecla 106:02d3327bf76a 44 **/
cittecla 86:df8c869a5a52 45 void stop_turn()
cittecla 86:df8c869a5a52 46 {
cittecla 86:df8c869a5a52 47 set_speed(0,0);
cittecla 86:df8c869a5a52 48 wanted_deg = 0;
cittecla 86:df8c869a5a52 49 is_turning = false;
cittecla 86:df8c869a5a52 50 }
cittecla 86:df8c869a5a52 51
cittecla 106:02d3327bf76a 52
cittecla 106:02d3327bf76a 53 /**
cittecla 106:02d3327bf76a 54 * move for wanted distance on circle with a given radius
cittecla 106:02d3327bf76a 55 * needs to be called until return < 0
cittecla 106:02d3327bf76a 56 * if calling distance not 0: distance and radius initilisation.
cittecla 106:02d3327bf76a 57 * by Claudio Citterio
cittecla 106:02d3327bf76a 58 **/
cittecla 105:d489c2e8de35 59 float move_for_distance_with_radius(float distance, float r)
cittecla 105:d489c2e8de35 60 {
cittecla 105:d489c2e8de35 61
cittecla 105:d489c2e8de35 62 if(distance != 0) {
cittecla 105:d489c2e8de35 63
cittecla 105:d489c2e8de35 64 is_moving = true;
cittecla 105:d489c2e8de35 65 wanted_dist = fabsf(distance);
cittecla 105:d489c2e8de35 66
cittecla 105:d489c2e8de35 67 float circumference = r*2*(float)M_PI;
cittecla 105:d489c2e8de35 68 float circumference_inner = ((r-(float)OFFSET_WHEELS)*2*(float)M_PI);
cittecla 105:d489c2e8de35 69 float circumference_outer = ((r+(float)OFFSET_WHEELS)*2*(float)M_PI);
cittecla 105:d489c2e8de35 70
cittecla 105:d489c2e8de35 71 float center_speed = 50;
cittecla 105:d489c2e8de35 72 float inner_speed = center_speed/circumference*circumference_inner;
cittecla 105:d489c2e8de35 73 float outer_speed = center_speed/circumference*circumference_outer;
cittecla 105:d489c2e8de35 74
cittecla 105:d489c2e8de35 75 //wanted_deg = 360 / (2*radius*(float)M_PI) * wanted_dist;
cittecla 105:d489c2e8de35 76 //float time = (10*wanted_dist)/(wheel_r * center_speed);
cittecla 105:d489c2e8de35 77
cittecla 105:d489c2e8de35 78
cittecla 105:d489c2e8de35 79 if(r > 0) {
cittecla 105:d489c2e8de35 80
cittecla 105:d489c2e8de35 81 //turn right
cittecla 105:d489c2e8de35 82 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 83 direction = 1;
cittecla 105:d489c2e8de35 84 left = outer_speed;
cittecla 105:d489c2e8de35 85 right = inner_speed;
cittecla 105:d489c2e8de35 86 } else { //move backward
cittecla 105:d489c2e8de35 87 direction = 0;
cittecla 105:d489c2e8de35 88 left = -outer_speed;
cittecla 105:d489c2e8de35 89 right = -inner_speed;
cittecla 105:d489c2e8de35 90 }
cittecla 105:d489c2e8de35 91 } else if(r < 0) {
cittecla 105:d489c2e8de35 92
cittecla 105:d489c2e8de35 93 // turn left
cittecla 105:d489c2e8de35 94 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 95 direction = 1;
cittecla 105:d489c2e8de35 96 left = inner_speed;
cittecla 105:d489c2e8de35 97 right = outer_speed;
cittecla 105:d489c2e8de35 98 } else { //move backward
cittecla 105:d489c2e8de35 99 direction = 0;
cittecla 105:d489c2e8de35 100 left = -inner_speed;
cittecla 105:d489c2e8de35 101 right = -outer_speed;
cittecla 105:d489c2e8de35 102 }
cittecla 105:d489c2e8de35 103 } else {
cittecla 105:d489c2e8de35 104 //normal straight movement
cittecla 105:d489c2e8de35 105
cittecla 105:d489c2e8de35 106 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 107 direction = 1;
cittecla 105:d489c2e8de35 108 left = center_speed;
cittecla 105:d489c2e8de35 109 right = center_speed;
cittecla 105:d489c2e8de35 110 } else { //move backward
cittecla 105:d489c2e8de35 111 direction = 0;
cittecla 105:d489c2e8de35 112 left = -center_speed;
cittecla 105:d489c2e8de35 113 right = -center_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 **/
cittecla 106:02d3327bf76a 209 float turn_for_deg(float deg)
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;
cittecla 94:0381e8b1beda 219 left = -30.0f;
cittecla 94:0381e8b1beda 220 right = 30.0f;
cittecla 61:628f8a4e857c 221 } else { // turn right
cittecla 61:628f8a4e857c 222 direction = 0;
cittecla 94:0381e8b1beda 223 left = 30.0f;
cittecla 94:0381e8b1beda 224 right = -30.0f;
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 52:56399c2f13cd 257
cittecla 52:56399c2f13cd 258 int move_to_next_coord()
cittecla 52:56399c2f13cd 259 {
cittecla 52:56399c2f13cd 260
cittecla 52:56399c2f13cd 261 float current_heading = get_current_heading();
cittecla 53:453f24775644 262 position current_pos = get_current_pos();
cittecla 52:56399c2f13cd 263 position next_pos = get_next_pos();
cittecla 61:628f8a4e857c 264
cittecla 52:56399c2f13cd 265 float needed_heading = 0;
cittecla 52:56399c2f13cd 266 float distance = 0;
cittecla 61:628f8a4e857c 267
cittecla 52:56399c2f13cd 268 // nord(-y) = 0 grad
cittecla 61:628f8a4e857c 269 if(current_pos.y > next_pos.y) {
cittecla 61:628f8a4e857c 270 if(current_pos.x > next_pos.x) needed_heading = 315;
cittecla 61:628f8a4e857c 271 distance = sqrt2;
cittecla 61:628f8a4e857c 272 if(current_pos.x == next_pos.x) needed_heading = 0;
cittecla 61:628f8a4e857c 273 distance = 1;
cittecla 61:628f8a4e857c 274 if(current_pos.x < next_pos.x) needed_heading = 45;
cittecla 61:628f8a4e857c 275 distance = sqrt2;
cittecla 61:628f8a4e857c 276 }
cittecla 61:628f8a4e857c 277 if(current_pos.y == next_pos.y) {
cittecla 61:628f8a4e857c 278 if(current_pos.x > next_pos.x) needed_heading = 270;
cittecla 61:628f8a4e857c 279 distance = 1;
cittecla 53:453f24775644 280 if(current_pos.x == next_pos.x) //error same position;
cittecla 61:628f8a4e857c 281 if(current_pos.x < next_pos.x) needed_heading = 90;
cittecla 61:628f8a4e857c 282 distance = 1;
cittecla 61:628f8a4e857c 283 }
cittecla 61:628f8a4e857c 284 if(current_pos.y < next_pos.y) {
cittecla 61:628f8a4e857c 285 if(current_pos.x > next_pos.x) needed_heading = 225;
cittecla 61:628f8a4e857c 286 distance = sqrt2;
cittecla 61:628f8a4e857c 287 if(current_pos.x == next_pos.x) needed_heading = 180;
cittecla 61:628f8a4e857c 288 distance = 1;
cittecla 61:628f8a4e857c 289 if(current_pos.x < next_pos.x) needed_heading = 135;
cittecla 61:628f8a4e857c 290 distance = sqrt2;
cittecla 61:628f8a4e857c 291 }
cittecla 52:56399c2f13cd 292
cittecla 52:56399c2f13cd 293 if(needed_heading != current_heading) {
cittecla 53:453f24775644 294 turn_for_deg((needed_heading-current_heading));
cittecla 61:628f8a4e857c 295 } else {
cittecla 85:d8ea8a99fa3a 296 move_for_distance(distance);
cittecla 39:92723f7ea54f 297 }
cittecla 39:92723f7ea54f 298 return 0;
cittecla 52:56399c2f13cd 299 }
cittecla 52:56399c2f13cd 300
cittecla 106:02d3327bf76a 301 /**
cittecla 106:02d3327bf76a 302 * this function searchs a nearby brick, moves towards it and grabbs it
cittecla 106:02d3327bf76a 303 * by Tobias Berger, state machine by Claudio Citterio
cittecla 106:02d3327bf76a 304 **/
cittecla 52:56399c2f13cd 305 int move_in_search_for_brick()
cittecla 52:56399c2f13cd 306 {
PESGruppe1 79:50f8d58f79ab 307 float upper = getDistanceIR(2); // get distance from upper Center Sensor
PESGruppe1 79:50f8d58f79ab 308 float lower = getDistanceIR(3); // get distance from Lower Center Sensor
aeschsim 96:1c6867536350 309 printf("Current Search State: >%d<\r\n",search_state);
cittecla 86:df8c869a5a52 310 switch (search_state) {
cittecla 86:df8c869a5a52 311 case 0: //first cycle right
cittecla 86:df8c869a5a52 312 turn_for_deg(60.0f); // call function and start turning
cittecla 86:df8c869a5a52 313 search_state = 1;
cittecla 86:df8c869a5a52 314 break;
cittecla 85:d8ea8a99fa3a 315
cittecla 86:df8c869a5a52 316 case 1: // turn right 60 deg
cittecla 101:6b10685aa34d 317 if((lower<0.75f)&&(lower>0.05f)) { // if something is in the range of 10 to 80cm at the lower Sensor
cittecla 86:df8c869a5a52 318 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 86:df8c869a5a52 319 stop_turn();
cittecla 101:6b10685aa34d 320 search_state = 5; //brick found
aeschsim 99:78d87027c85b 321 printf("Brick found 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
cittecla 86:df8c869a5a52 325 if(turn_for_deg(0) < 0) {
cittecla 86:df8c869a5a52 326 stop_turn();
cittecla 86:df8c869a5a52 327 search_state = 2;
cittecla 86:df8c869a5a52 328 }
cittecla 86:df8c869a5a52 329 }
cittecla 86:df8c869a5a52 330 break;
cittecla 85:d8ea8a99fa3a 331
cittecla 86:df8c869a5a52 332 case 2: // first cycle left
cittecla 86:df8c869a5a52 333 turn_for_deg(-120.0f);
cittecla 86:df8c869a5a52 334 search_state = 3;
cittecla 86:df8c869a5a52 335 break;
cittecla 85:d8ea8a99fa3a 336
cittecla 86:df8c869a5a52 337 case 3: // turn left 120 deg
cittecla 101:6b10685aa34d 338 if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
cittecla 86:df8c869a5a52 339 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 86:df8c869a5a52 340 stop_turn();
cittecla 104:7bc5eb2b4199 341 search_state = 10; //brick found
cittecla 101:6b10685aa34d 342 printf("Brick found lower: %f upper:%f",lower,upper);
cittecla 86:df8c869a5a52 343 }
cittecla 86:df8c869a5a52 344 } else {
cittecla 86:df8c869a5a52 345 search_state = 3; // go to same state
cittecla 86:df8c869a5a52 346 if(turn_for_deg(0) < 0) {
cittecla 86:df8c869a5a52 347 stop_turn();
cittecla 101:6b10685aa34d 348 search_state = 20; // error
cittecla 86:df8c869a5a52 349 }
cittecla 86:df8c869a5a52 350 }
cittecla 86:df8c869a5a52 351 break;
cittecla 52:56399c2f13cd 352
cittecla 101:6b10685aa34d 353 case 5: // turn back left
cittecla 101:6b10685aa34d 354 turn_for_deg(-10.0f);
cittecla 101:6b10685aa34d 355 search_state = 8;
cittecla 101:6b10685aa34d 356 break;
cittecla 105:d489c2e8de35 357
cittecla 101:6b10685aa34d 358 case 8: // turn back right continue
cittecla 101:6b10685aa34d 359 if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
cittecla 101:6b10685aa34d 360 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 101:6b10685aa34d 361 stop_turn();
cittecla 101:6b10685aa34d 362 search_state = 10; //brick found
cittecla 101:6b10685aa34d 363 }
cittecla 101:6b10685aa34d 364 } else {
cittecla 104:7bc5eb2b4199 365 search_state = 8; // go to same state
cittecla 101:6b10685aa34d 366 if(turn_for_deg(0) < 0) {
cittecla 101:6b10685aa34d 367 stop_turn();
cittecla 101:6b10685aa34d 368 search_state = 20; // error
cittecla 101:6b10685aa34d 369 }
cittecla 86:df8c869a5a52 370 }
cittecla 86:df8c869a5a52 371 break;
cittecla 86:df8c869a5a52 372
cittecla 101:6b10685aa34d 373
cittecla 101:6b10685aa34d 374 case 10: // first cycle move
cittecla 101:6b10685aa34d 375 float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
cittecla 101:6b10685aa34d 376 move_for_distance(distance_to_Brick);
cittecla 101:6b10685aa34d 377 search_state =11;
cittecla 101:6b10685aa34d 378 break;
cittecla 101:6b10685aa34d 379
cittecla 101:6b10685aa34d 380 case 11: // move forward
cittecla 101:6b10685aa34d 381 if(move_for_distance(0) < 0) {
cittecla 101:6b10685aa34d 382 stop_move();
cittecla 101:6b10685aa34d 383 search_state = 12;
cittecla 101:6b10685aa34d 384 }
cittecla 101:6b10685aa34d 385 break;
cittecla 101:6b10685aa34d 386
cittecla 101:6b10685aa34d 387 case 12: // Grabbing
cittecla 94:0381e8b1beda 388 return 50; //main state machine set as Grabbing
cittecla 94:0381e8b1beda 389
cittecla 86:df8c869a5a52 390 default:
aeschsim 96:1c6867536350 391 printf("default State - move in search for brick\r\n");
cittecla 86:df8c869a5a52 392 // error
cittecla 86:df8c869a5a52 393 break;
PESGruppe1 74:d9c387b83196 394 }
cittecla 86:df8c869a5a52 395 return 47; //called until function is done
cittecla 52:56399c2f13cd 396 }
cittecla 52:56399c2f13cd 397
cittecla 39:92723f7ea54f 398
cittecla 39:92723f7ea54f 399
cittecla 39:92723f7ea54f 400
cittecla 39:92723f7ea54f 401
cittecla 39:92723f7ea54f 402
cittecla 39:92723f7ea54f 403
cittecla 39:92723f7ea54f 404