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@113:c7afe49752b9, 2017-05-09 (annotated)

Committer:
cittecla
Date:
Tue May 09 13:58:57 2017 +0000
Revision:
113:c7afe49752b9
Parent:
111:cd7b7b551616
Child:
114:720dc5df42a5
a few changes in movement have been done

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