Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

Committer:
cittecla
Date:
Tue May 16 14:24:11 2017 +0000
Revision:
134:5c29654ce301
Parent:
132:8ae08f41bb43
Child:
135:f31d24150f5e
bugfixes;

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