Tobis Programm forked to not destroy your golden files
Fork of Robocode by
source/Movement.cpp@134:5c29654ce301, 2017-05-16 (annotated)
- 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?
User | Revision | Line number | New 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 |