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@105:d489c2e8de35, 2017-05-04 (annotated)

Committer:
cittecla
Date:
Thu May 04 09:19:26 2017 +0000
Revision:
105:d489c2e8de35
Parent:
104:7bc5eb2b4199
Child:
106:02d3327bf76a
added move_for_distance_with_radius(float distance, float r) function.; when overloaded with r = 0 equals in move_for_distance.

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 86:df8c869a5a52 32 void stop_move()
cittecla 86:df8c869a5a52 33 {
cittecla 86:df8c869a5a52 34 set_speed(0,0);
cittecla 86:df8c869a5a52 35 wanted_dist = 0;
cittecla 86:df8c869a5a52 36 is_moving = false;
cittecla 86:df8c869a5a52 37 }
cittecla 86:df8c869a5a52 38
cittecla 86:df8c869a5a52 39 void stop_turn()
cittecla 86:df8c869a5a52 40 {
cittecla 86:df8c869a5a52 41 set_speed(0,0);
cittecla 86:df8c869a5a52 42 wanted_deg = 0;
cittecla 86:df8c869a5a52 43 is_turning = false;
cittecla 86:df8c869a5a52 44 }
cittecla 86:df8c869a5a52 45
cittecla 105:d489c2e8de35 46 float move_for_distance_with_radius(float distance, float r)
cittecla 105:d489c2e8de35 47 {
cittecla 105:d489c2e8de35 48
cittecla 105:d489c2e8de35 49 if(distance != 0) {
cittecla 105:d489c2e8de35 50
cittecla 105:d489c2e8de35 51 is_moving = true;
cittecla 105:d489c2e8de35 52 wanted_dist = fabsf(distance);
cittecla 105:d489c2e8de35 53
cittecla 105:d489c2e8de35 54 float circumference = r*2*(float)M_PI;
cittecla 105:d489c2e8de35 55 float circumference_inner = ((r-(float)OFFSET_WHEELS)*2*(float)M_PI);
cittecla 105:d489c2e8de35 56 float circumference_outer = ((r+(float)OFFSET_WHEELS)*2*(float)M_PI);
cittecla 105:d489c2e8de35 57
cittecla 105:d489c2e8de35 58 float center_speed = 50;
cittecla 105:d489c2e8de35 59 float inner_speed = center_speed/circumference*circumference_inner;
cittecla 105:d489c2e8de35 60 float outer_speed = center_speed/circumference*circumference_outer;
cittecla 105:d489c2e8de35 61
cittecla 105:d489c2e8de35 62 //wanted_deg = 360 / (2*radius*(float)M_PI) * wanted_dist;
cittecla 105:d489c2e8de35 63 //float time = (10*wanted_dist)/(wheel_r * center_speed);
cittecla 105:d489c2e8de35 64
cittecla 105:d489c2e8de35 65
cittecla 105:d489c2e8de35 66 if(r > 0) {
cittecla 105:d489c2e8de35 67
cittecla 105:d489c2e8de35 68 //turn right
cittecla 105:d489c2e8de35 69 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 70 direction = 1;
cittecla 105:d489c2e8de35 71 left = outer_speed;
cittecla 105:d489c2e8de35 72 right = inner_speed;
cittecla 105:d489c2e8de35 73 } else { //move backward
cittecla 105:d489c2e8de35 74 direction = 0;
cittecla 105:d489c2e8de35 75 left = -outer_speed;
cittecla 105:d489c2e8de35 76 right = -inner_speed;
cittecla 105:d489c2e8de35 77 }
cittecla 105:d489c2e8de35 78 } else if(r < 0) {
cittecla 105:d489c2e8de35 79
cittecla 105:d489c2e8de35 80 // turn left
cittecla 105:d489c2e8de35 81 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 82 direction = 1;
cittecla 105:d489c2e8de35 83 left = inner_speed;
cittecla 105:d489c2e8de35 84 right = outer_speed;
cittecla 105:d489c2e8de35 85 } else { //move backward
cittecla 105:d489c2e8de35 86 direction = 0;
cittecla 105:d489c2e8de35 87 left = -inner_speed;
cittecla 105:d489c2e8de35 88 right = -outer_speed;
cittecla 105:d489c2e8de35 89 }
cittecla 105:d489c2e8de35 90 } else {
cittecla 105:d489c2e8de35 91 //normal straight movement
cittecla 105:d489c2e8de35 92
cittecla 105:d489c2e8de35 93 if(distance > 0) { //move forward
cittecla 105:d489c2e8de35 94 direction = 1;
cittecla 105:d489c2e8de35 95 left = center_speed;
cittecla 105:d489c2e8de35 96 right = center_speed;
cittecla 105:d489c2e8de35 97 } else { //move backward
cittecla 105:d489c2e8de35 98 direction = 0;
cittecla 105:d489c2e8de35 99 left = -center_speed;
cittecla 105:d489c2e8de35 100 right = -center_speed;
cittecla 105:d489c2e8de35 101 }
cittecla 105:d489c2e8de35 102 }
cittecla 105:d489c2e8de35 103
cittecla 105:d489c2e8de35 104 set_speed(left, right);
cittecla 105:d489c2e8de35 105 devider = true;
cittecla 105:d489c2e8de35 106 t.reset();
cittecla 105:d489c2e8de35 107 t.start();
cittecla 105:d489c2e8de35 108 } else {
cittecla 105:d489c2e8de35 109 float speed_multiplier = 0.6f;
cittecla 105:d489c2e8de35 110 if(wanted_dist < 0.10f && devider == true) {
cittecla 105:d489c2e8de35 111 //printf("devided\r\n");
cittecla 105:d489c2e8de35 112 devider = false;
cittecla 105:d489c2e8de35 113 left = left * speed_multiplier;
cittecla 105:d489c2e8de35 114 right = right * speed_multiplier;
cittecla 105:d489c2e8de35 115 //printf("left: %f || right: %f\r\n", left, right);
cittecla 105:d489c2e8de35 116 set_speed(left, right);
cittecla 105:d489c2e8de35 117 }
cittecla 105:d489c2e8de35 118
cittecla 105:d489c2e8de35 119 float speed_left = get_speed_left();
cittecla 105:d489c2e8de35 120 float speed_right = get_speed_right();
cittecla 105:d489c2e8de35 121 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 122 t.reset();
cittecla 105:d489c2e8de35 123
cittecla 105:d489c2e8de35 124 if(wanted_dist <= 0) { //distance covered, Stop function
cittecla 105:d489c2e8de35 125 set_speed(0,0);
cittecla 105:d489c2e8de35 126 is_moving = false;
cittecla 105:d489c2e8de35 127 t.stop();
cittecla 105:d489c2e8de35 128 }
cittecla 105:d489c2e8de35 129 }
cittecla 105:d489c2e8de35 130 printf("remaining distance to cover: %f\r\n", wanted_dist);
cittecla 105:d489c2e8de35 131 return wanted_dist;
cittecla 105:d489c2e8de35 132 }
cittecla 105:d489c2e8de35 133
cittecla 105:d489c2e8de35 134
cittecla 85:d8ea8a99fa3a 135 float move_for_distance(float distance)
cittecla 52:56399c2f13cd 136 {
cittecla 88:b89cace9329b 137 printf("move for distance\r\n");
cittecla 85:d8ea8a99fa3a 138 if(distance != 0) {
cittecla 52:56399c2f13cd 139
cittecla 85:d8ea8a99fa3a 140 is_moving = true;
cittecla 85:d8ea8a99fa3a 141
cittecla 105:d489c2e8de35 142 wanted_dist = fabsf(distance);
cittecla 85:d8ea8a99fa3a 143
cittecla 85:d8ea8a99fa3a 144 if(distance > 0) { //move forward
cittecla 85:d8ea8a99fa3a 145 direction = 1;
cittecla 85:d8ea8a99fa3a 146 left = 50.0f;
cittecla 85:d8ea8a99fa3a 147 right = 50.0f;
cittecla 85:d8ea8a99fa3a 148 } else { //move backward
cittecla 85:d8ea8a99fa3a 149 direction = 0;
cittecla 85:d8ea8a99fa3a 150 left = -50.0f;
cittecla 85:d8ea8a99fa3a 151 right = -50.0f;
cittecla 85:d8ea8a99fa3a 152 }
cittecla 88:b89cace9329b 153 printf("set speed %f\r\n", left);
cittecla 85:d8ea8a99fa3a 154 set_speed(left, right);
cittecla 94:0381e8b1beda 155 devider = true;
cittecla 85:d8ea8a99fa3a 156 t.reset();
cittecla 85:d8ea8a99fa3a 157 t.start();
cittecla 85:d8ea8a99fa3a 158
cittecla 85:d8ea8a99fa3a 159 } else {
cittecla 94:0381e8b1beda 160 float speed_multiplier = 0.6f;
cittecla 94:0381e8b1beda 161 if(wanted_dist < 0.10f && devider == true) {
cittecla 94:0381e8b1beda 162 //printf("devided\r\n");
cittecla 94:0381e8b1beda 163 devider = false;
aeschsim 99:78d87027c85b 164 left = left * speed_multiplier;
aeschsim 99:78d87027c85b 165 right = right * speed_multiplier;
aeschsim 99:78d87027c85b 166 //printf("left: %f || right: %f\r\n", left, right);
cittecla 94:0381e8b1beda 167 set_speed(left, right);
cittecla 94:0381e8b1beda 168 }
cittecla 85:d8ea8a99fa3a 169
cittecla 85:d8ea8a99fa3a 170 float speed_left = get_speed_left();
cittecla 94:0381e8b1beda 171 printf("speed left: %f\r\n", speed_left);
cittecla 89:7f9d6e641a01 172 wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
cittecla 85:d8ea8a99fa3a 173 t.reset();
cittecla 85:d8ea8a99fa3a 174
cittecla 85:d8ea8a99fa3a 175 if(wanted_dist <= 0) { //distance covered, Stop function
cittecla 85:d8ea8a99fa3a 176 set_speed(0,0);
cittecla 85:d8ea8a99fa3a 177 is_moving = false;
cittecla 85:d8ea8a99fa3a 178 t.stop();
cittecla 85:d8ea8a99fa3a 179 }
cittecla 85:d8ea8a99fa3a 180 }
cittecla 88:b89cace9329b 181 printf("remaining distance to cover: %f\r\n", wanted_dist);
cittecla 85:d8ea8a99fa3a 182 return wanted_dist;
cittecla 52:56399c2f13cd 183 }
cittecla 52:56399c2f13cd 184
cittecla 85:d8ea8a99fa3a 185 float turn_for_deg(float deg) //if deg not 0 equals initilisation.
cittecla 52:56399c2f13cd 186 {
cittecla 52:56399c2f13cd 187
cittecla 75:dba260cb5ae4 188 if(deg != 0) {
cittecla 61:628f8a4e857c 189
cittecla 61:628f8a4e857c 190 is_turning = true;
cittecla 94:0381e8b1beda 191 wanted_deg = fabsf(deg);
cittecla 61:628f8a4e857c 192
cittecla 61:628f8a4e857c 193 if(deg < 0) { // turn left
cittecla 61:628f8a4e857c 194 direction = 1;
cittecla 94:0381e8b1beda 195 left = -30.0f;
cittecla 94:0381e8b1beda 196 right = 30.0f;
cittecla 61:628f8a4e857c 197 } else { // turn right
cittecla 61:628f8a4e857c 198 direction = 0;
cittecla 94:0381e8b1beda 199 left = 30.0f;
cittecla 94:0381e8b1beda 200 right = -30.0f;
cittecla 61:628f8a4e857c 201 }
cittecla 61:628f8a4e857c 202 set_speed(left, right);
cittecla 94:0381e8b1beda 203 devider = true;
cittecla 71:ddf4eb5c3081 204 t.reset();
cittecla 61:628f8a4e857c 205 t.start();
cittecla 61:628f8a4e857c 206
cittecla 61:628f8a4e857c 207 } else {
cittecla 94:0381e8b1beda 208 float speed_multiplier = 0.6f;
cittecla 94:0381e8b1beda 209 if(wanted_deg < 10.0f && devider == true) {
cittecla 94:0381e8b1beda 210 devider = false;
aeschsim 99:78d87027c85b 211 left = left * speed_multiplier;
aeschsim 99:78d87027c85b 212 right = right * speed_multiplier;
cittecla 94:0381e8b1beda 213 set_speed(left, right);
cittecla 94:0381e8b1beda 214 }
cittecla 101:6b10685aa34d 215
cittecla 61:628f8a4e857c 216 float speed_left = get_speed_left();
cittecla 94:0381e8b1beda 217 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 218 t.reset();
cittecla 75:dba260cb5ae4 219 if(wanted_deg <= 0) {
cittecla 61:628f8a4e857c 220 set_speed(0,0);
cittecla 61:628f8a4e857c 221 is_turning = false;
cittecla 61:628f8a4e857c 222 t.stop();
cittecla 61:628f8a4e857c 223 }
cittecla 61:628f8a4e857c 224 }
cittecla 94:0381e8b1beda 225 printf("remaining deg %f\r\n", wanted_deg);
cittecla 75:dba260cb5ae4 226 return (wanted_deg);
cittecla 52:56399c2f13cd 227 }
cittecla 52:56399c2f13cd 228
cittecla 52:56399c2f13cd 229
cittecla 52:56399c2f13cd 230 int move_to_next_coord()
cittecla 52:56399c2f13cd 231 {
cittecla 52:56399c2f13cd 232
cittecla 52:56399c2f13cd 233 float current_heading = get_current_heading();
cittecla 53:453f24775644 234 position current_pos = get_current_pos();
cittecla 52:56399c2f13cd 235 position next_pos = get_next_pos();
cittecla 61:628f8a4e857c 236
cittecla 52:56399c2f13cd 237 float needed_heading = 0;
cittecla 52:56399c2f13cd 238 float distance = 0;
cittecla 61:628f8a4e857c 239
cittecla 52:56399c2f13cd 240 // nord(-y) = 0 grad
cittecla 61:628f8a4e857c 241 if(current_pos.y > next_pos.y) {
cittecla 61:628f8a4e857c 242 if(current_pos.x > next_pos.x) needed_heading = 315;
cittecla 61:628f8a4e857c 243 distance = sqrt2;
cittecla 61:628f8a4e857c 244 if(current_pos.x == next_pos.x) needed_heading = 0;
cittecla 61:628f8a4e857c 245 distance = 1;
cittecla 61:628f8a4e857c 246 if(current_pos.x < next_pos.x) needed_heading = 45;
cittecla 61:628f8a4e857c 247 distance = sqrt2;
cittecla 61:628f8a4e857c 248 }
cittecla 61:628f8a4e857c 249 if(current_pos.y == next_pos.y) {
cittecla 61:628f8a4e857c 250 if(current_pos.x > next_pos.x) needed_heading = 270;
cittecla 61:628f8a4e857c 251 distance = 1;
cittecla 53:453f24775644 252 if(current_pos.x == next_pos.x) //error same position;
cittecla 61:628f8a4e857c 253 if(current_pos.x < next_pos.x) needed_heading = 90;
cittecla 61:628f8a4e857c 254 distance = 1;
cittecla 61:628f8a4e857c 255 }
cittecla 61:628f8a4e857c 256 if(current_pos.y < next_pos.y) {
cittecla 61:628f8a4e857c 257 if(current_pos.x > next_pos.x) needed_heading = 225;
cittecla 61:628f8a4e857c 258 distance = sqrt2;
cittecla 61:628f8a4e857c 259 if(current_pos.x == next_pos.x) needed_heading = 180;
cittecla 61:628f8a4e857c 260 distance = 1;
cittecla 61:628f8a4e857c 261 if(current_pos.x < next_pos.x) needed_heading = 135;
cittecla 61:628f8a4e857c 262 distance = sqrt2;
cittecla 61:628f8a4e857c 263 }
cittecla 52:56399c2f13cd 264
cittecla 52:56399c2f13cd 265 if(needed_heading != current_heading) {
cittecla 53:453f24775644 266 turn_for_deg((needed_heading-current_heading));
cittecla 61:628f8a4e857c 267 } else {
cittecla 85:d8ea8a99fa3a 268 move_for_distance(distance);
cittecla 39:92723f7ea54f 269 }
cittecla 39:92723f7ea54f 270 return 0;
cittecla 52:56399c2f13cd 271 }
cittecla 52:56399c2f13cd 272
PESGruppe1 74:d9c387b83196 273 // Tobias Berger
cittecla 52:56399c2f13cd 274 int move_in_search_for_brick()
cittecla 52:56399c2f13cd 275 {
PESGruppe1 79:50f8d58f79ab 276 float upper = getDistanceIR(2); // get distance from upper Center Sensor
PESGruppe1 79:50f8d58f79ab 277 float lower = getDistanceIR(3); // get distance from Lower Center Sensor
aeschsim 96:1c6867536350 278 printf("Current Search State: >%d<\r\n",search_state);
cittecla 86:df8c869a5a52 279 switch (search_state) {
cittecla 86:df8c869a5a52 280 case 0: //first cycle right
cittecla 86:df8c869a5a52 281 turn_for_deg(60.0f); // call function and start turning
cittecla 86:df8c869a5a52 282 search_state = 1;
cittecla 86:df8c869a5a52 283 break;
cittecla 85:d8ea8a99fa3a 284
cittecla 86:df8c869a5a52 285 case 1: // turn right 60 deg
cittecla 101:6b10685aa34d 286 if((lower<0.75f)&&(lower>0.05f)) { // if something is in the range of 10 to 80cm at the lower Sensor
cittecla 86:df8c869a5a52 287 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 86:df8c869a5a52 288 stop_turn();
cittecla 101:6b10685aa34d 289 search_state = 5; //brick found
aeschsim 99:78d87027c85b 290 printf("Brick found lower: %f upper:%f",lower,upper);
cittecla 86:df8c869a5a52 291 }
cittecla 86:df8c869a5a52 292 } else {
cittecla 86:df8c869a5a52 293 search_state = 1; // go to same state
cittecla 86:df8c869a5a52 294 if(turn_for_deg(0) < 0) {
cittecla 86:df8c869a5a52 295 stop_turn();
cittecla 86:df8c869a5a52 296 search_state = 2;
cittecla 86:df8c869a5a52 297 }
cittecla 86:df8c869a5a52 298 }
cittecla 86:df8c869a5a52 299 break;
cittecla 85:d8ea8a99fa3a 300
cittecla 86:df8c869a5a52 301 case 2: // first cycle left
cittecla 86:df8c869a5a52 302 turn_for_deg(-120.0f);
cittecla 86:df8c869a5a52 303 search_state = 3;
cittecla 86:df8c869a5a52 304 break;
cittecla 85:d8ea8a99fa3a 305
cittecla 86:df8c869a5a52 306 case 3: // turn left 120 deg
cittecla 101:6b10685aa34d 307 if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
cittecla 86:df8c869a5a52 308 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 86:df8c869a5a52 309 stop_turn();
cittecla 104:7bc5eb2b4199 310 search_state = 10; //brick found
cittecla 101:6b10685aa34d 311 printf("Brick found lower: %f upper:%f",lower,upper);
cittecla 86:df8c869a5a52 312 }
cittecla 86:df8c869a5a52 313 } else {
cittecla 86:df8c869a5a52 314 search_state = 3; // go to same state
cittecla 86:df8c869a5a52 315 if(turn_for_deg(0) < 0) {
cittecla 86:df8c869a5a52 316 stop_turn();
cittecla 101:6b10685aa34d 317 search_state = 20; // error
cittecla 86:df8c869a5a52 318 }
cittecla 86:df8c869a5a52 319 }
cittecla 86:df8c869a5a52 320 break;
cittecla 52:56399c2f13cd 321
cittecla 101:6b10685aa34d 322 case 5: // turn back left
cittecla 101:6b10685aa34d 323 turn_for_deg(-10.0f);
cittecla 101:6b10685aa34d 324 search_state = 8;
cittecla 101:6b10685aa34d 325 break;
cittecla 105:d489c2e8de35 326
cittecla 101:6b10685aa34d 327 case 8: // turn back right continue
cittecla 101:6b10685aa34d 328 if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
cittecla 101:6b10685aa34d 329 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 101:6b10685aa34d 330 stop_turn();
cittecla 101:6b10685aa34d 331 search_state = 10; //brick found
cittecla 101:6b10685aa34d 332 }
cittecla 101:6b10685aa34d 333 } else {
cittecla 104:7bc5eb2b4199 334 search_state = 8; // go to same state
cittecla 101:6b10685aa34d 335 if(turn_for_deg(0) < 0) {
cittecla 101:6b10685aa34d 336 stop_turn();
cittecla 101:6b10685aa34d 337 search_state = 20; // error
cittecla 101:6b10685aa34d 338 }
cittecla 86:df8c869a5a52 339 }
cittecla 86:df8c869a5a52 340 break;
cittecla 86:df8c869a5a52 341
cittecla 101:6b10685aa34d 342
cittecla 101:6b10685aa34d 343 case 10: // first cycle move
cittecla 101:6b10685aa34d 344 float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
cittecla 101:6b10685aa34d 345 move_for_distance(distance_to_Brick);
cittecla 101:6b10685aa34d 346 search_state =11;
cittecla 101:6b10685aa34d 347 break;
cittecla 101:6b10685aa34d 348
cittecla 101:6b10685aa34d 349 case 11: // move forward
cittecla 101:6b10685aa34d 350 if(move_for_distance(0) < 0) {
cittecla 101:6b10685aa34d 351 stop_move();
cittecla 101:6b10685aa34d 352 search_state = 12;
cittecla 101:6b10685aa34d 353 }
cittecla 101:6b10685aa34d 354 break;
cittecla 101:6b10685aa34d 355
cittecla 101:6b10685aa34d 356 case 12: // Grabbing
cittecla 94:0381e8b1beda 357 return 50; //main state machine set as Grabbing
cittecla 94:0381e8b1beda 358
cittecla 86:df8c869a5a52 359 default:
aeschsim 96:1c6867536350 360 printf("default State - move in search for brick\r\n");
cittecla 86:df8c869a5a52 361 // error
cittecla 86:df8c869a5a52 362 break;
PESGruppe1 74:d9c387b83196 363 }
cittecla 86:df8c869a5a52 364 return 47; //called until function is done
cittecla 52:56399c2f13cd 365 }
cittecla 52:56399c2f13cd 366
cittecla 39:92723f7ea54f 367
cittecla 39:92723f7ea54f 368
cittecla 39:92723f7ea54f 369
cittecla 39:92723f7ea54f 370
cittecla 39:92723f7ea54f 371
cittecla 39:92723f7ea54f 372
cittecla 39:92723f7ea54f 373