PES 2 - Gruppe 1 / Mbed 2 deprecated Robocode_Random

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/Movement.cpp@136:b35f2d9b7402, 2017-05-16 (annotated)

Committer:
aeschsim
Date:
Tue May 16 16:18:29 2017 +0000
Revision:
136:b35f2d9b7402
Parent:
135:644346924339 
changed and added some safety functions

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