Tobis Programm forked to not destroy your golden files
Fork of Robocode by
source/Movement.cpp@99:78d87027c85b, 2017-05-03 (annotated)
- Committer:
- aeschsim
- Date:
- Wed May 03 08:47:21 2017 +0000
- Revision:
- 99:78d87027c85b
- Parent:
- 96:1c6867536350
- Child:
- 101:6b10685aa34d
changed sensor reading
Who changed what in which revision?
User | Revision | Line number | New 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 | 86:df8c869a5a52 | 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 | 39:92723f7ea54f | 8 | |
cittecla | 85:d8ea8a99fa3a | 9 | bool is_moving = false; |
cittecla | 85:d8ea8a99fa3a | 10 | float wanted_dist = 0; |
cittecla | 39:92723f7ea54f | 11 | |
cittecla | 61:628f8a4e857c | 12 | bool is_turning = false; |
cittecla | 61:628f8a4e857c | 13 | bool direction = false; |
cittecla | 61:628f8a4e857c | 14 | float wanted_deg = 0; |
cittecla | 63:b27aa01c2cf6 | 15 | Timer t; |
cittecla | 61:628f8a4e857c | 16 | float previous_t = 0; |
cittecla | 86:df8c869a5a52 | 17 | int search_state = 0; |
cittecla | 77:ff87a10c4baf | 18 | float restdeg = 0; |
cittecla | 77:ff87a10c4baf | 19 | |
cittecla | 94:0381e8b1beda | 20 | float left = 0; |
cittecla | 94:0381e8b1beda | 21 | float right = 0; |
cittecla | 94:0381e8b1beda | 22 | |
cittecla | 94:0381e8b1beda | 23 | bool devider = true; |
cittecla | 94:0381e8b1beda | 24 | |
cittecla | 61:628f8a4e857c | 25 | |
cittecla | 52:56399c2f13cd | 26 | int moving() |
cittecla | 52:56399c2f13cd | 27 | { |
cittecla | 52:56399c2f13cd | 28 | |
cittecla | 39:92723f7ea54f | 29 | return 0; |
cittecla | 52:56399c2f13cd | 30 | } |
cittecla | 52:56399c2f13cd | 31 | |
cittecla | 86:df8c869a5a52 | 32 | void stop_move() |
cittecla | 86:df8c869a5a52 | 33 | { |
cittecla | 86:df8c869a5a52 | 34 | set_speed(0,0); |
cittecla | 86:df8c869a5a52 | 35 | wanted_dist = 0; |
cittecla | 86:df8c869a5a52 | 36 | is_moving = false; |
cittecla | 86:df8c869a5a52 | 37 | } |
cittecla | 86:df8c869a5a52 | 38 | |
cittecla | 86:df8c869a5a52 | 39 | void stop_turn() |
cittecla | 86:df8c869a5a52 | 40 | { |
cittecla | 86:df8c869a5a52 | 41 | set_speed(0,0); |
cittecla | 86:df8c869a5a52 | 42 | wanted_deg = 0; |
cittecla | 86:df8c869a5a52 | 43 | is_turning = false; |
cittecla | 86:df8c869a5a52 | 44 | } |
cittecla | 86:df8c869a5a52 | 45 | |
cittecla | 85:d8ea8a99fa3a | 46 | float move_for_distance(float distance) |
cittecla | 52:56399c2f13cd | 47 | { |
cittecla | 88:b89cace9329b | 48 | printf("move for distance\r\n"); |
cittecla | 85:d8ea8a99fa3a | 49 | if(distance != 0) { |
cittecla | 52:56399c2f13cd | 50 | |
cittecla | 85:d8ea8a99fa3a | 51 | is_moving = true; |
cittecla | 85:d8ea8a99fa3a | 52 | |
cittecla | 85:d8ea8a99fa3a | 53 | wanted_dist = sqrt(distance*distance); |
cittecla | 85:d8ea8a99fa3a | 54 | |
cittecla | 85:d8ea8a99fa3a | 55 | if(distance > 0) { //move forward |
cittecla | 85:d8ea8a99fa3a | 56 | direction = 1; |
cittecla | 85:d8ea8a99fa3a | 57 | left = 50.0f; |
cittecla | 85:d8ea8a99fa3a | 58 | right = 50.0f; |
cittecla | 85:d8ea8a99fa3a | 59 | } else { //move backward |
cittecla | 85:d8ea8a99fa3a | 60 | direction = 0; |
cittecla | 85:d8ea8a99fa3a | 61 | left = -50.0f; |
cittecla | 85:d8ea8a99fa3a | 62 | right = -50.0f; |
cittecla | 85:d8ea8a99fa3a | 63 | } |
cittecla | 88:b89cace9329b | 64 | printf("set speed %f\r\n", left); |
cittecla | 85:d8ea8a99fa3a | 65 | set_speed(left, right); |
cittecla | 94:0381e8b1beda | 66 | devider = true; |
cittecla | 85:d8ea8a99fa3a | 67 | t.reset(); |
cittecla | 85:d8ea8a99fa3a | 68 | t.start(); |
cittecla | 85:d8ea8a99fa3a | 69 | |
cittecla | 85:d8ea8a99fa3a | 70 | } else { |
cittecla | 94:0381e8b1beda | 71 | float speed_multiplier = 0.6f; |
cittecla | 94:0381e8b1beda | 72 | if(wanted_dist < 0.10f && devider == true) { |
cittecla | 94:0381e8b1beda | 73 | //printf("devided\r\n"); |
cittecla | 94:0381e8b1beda | 74 | devider = false; |
aeschsim | 99:78d87027c85b | 75 | left = left * speed_multiplier; |
aeschsim | 99:78d87027c85b | 76 | right = right * speed_multiplier; |
aeschsim | 99:78d87027c85b | 77 | //printf("left: %f || right: %f\r\n", left, right); |
cittecla | 94:0381e8b1beda | 78 | set_speed(left, right); |
cittecla | 94:0381e8b1beda | 79 | } |
cittecla | 85:d8ea8a99fa3a | 80 | |
cittecla | 85:d8ea8a99fa3a | 81 | float speed_left = get_speed_left(); |
cittecla | 94:0381e8b1beda | 82 | printf("speed left: %f\r\n", speed_left); |
cittecla | 89:7f9d6e641a01 | 83 | wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f; |
cittecla | 85:d8ea8a99fa3a | 84 | t.reset(); |
cittecla | 85:d8ea8a99fa3a | 85 | |
cittecla | 85:d8ea8a99fa3a | 86 | if(wanted_dist <= 0) { //distance covered, Stop function |
cittecla | 85:d8ea8a99fa3a | 87 | set_speed(0,0); |
cittecla | 85:d8ea8a99fa3a | 88 | is_moving = false; |
cittecla | 85:d8ea8a99fa3a | 89 | t.stop(); |
cittecla | 85:d8ea8a99fa3a | 90 | } |
cittecla | 85:d8ea8a99fa3a | 91 | } |
cittecla | 88:b89cace9329b | 92 | printf("remaining distance to cover: %f\r\n", wanted_dist); |
cittecla | 85:d8ea8a99fa3a | 93 | return wanted_dist; |
cittecla | 52:56399c2f13cd | 94 | } |
cittecla | 52:56399c2f13cd | 95 | |
cittecla | 85:d8ea8a99fa3a | 96 | float turn_for_deg(float deg) //if deg not 0 equals initilisation. |
cittecla | 52:56399c2f13cd | 97 | { |
cittecla | 52:56399c2f13cd | 98 | |
cittecla | 75:dba260cb5ae4 | 99 | if(deg != 0) { |
cittecla | 61:628f8a4e857c | 100 | |
cittecla | 61:628f8a4e857c | 101 | is_turning = true; |
cittecla | 94:0381e8b1beda | 102 | wanted_deg = fabsf(deg); |
cittecla | 61:628f8a4e857c | 103 | |
cittecla | 61:628f8a4e857c | 104 | if(deg < 0) { // turn left |
cittecla | 61:628f8a4e857c | 105 | direction = 1; |
cittecla | 94:0381e8b1beda | 106 | left = -30.0f; |
cittecla | 94:0381e8b1beda | 107 | right = 30.0f; |
cittecla | 61:628f8a4e857c | 108 | } else { // turn right |
cittecla | 61:628f8a4e857c | 109 | direction = 0; |
cittecla | 94:0381e8b1beda | 110 | left = 30.0f; |
cittecla | 94:0381e8b1beda | 111 | right = -30.0f; |
cittecla | 61:628f8a4e857c | 112 | } |
cittecla | 61:628f8a4e857c | 113 | set_speed(left, right); |
cittecla | 94:0381e8b1beda | 114 | devider = true; |
cittecla | 71:ddf4eb5c3081 | 115 | t.reset(); |
cittecla | 61:628f8a4e857c | 116 | t.start(); |
cittecla | 61:628f8a4e857c | 117 | |
cittecla | 61:628f8a4e857c | 118 | } else { |
cittecla | 94:0381e8b1beda | 119 | float speed_multiplier = 0.6f; |
cittecla | 94:0381e8b1beda | 120 | if(wanted_deg < 10.0f && devider == true) { |
cittecla | 94:0381e8b1beda | 121 | devider = false; |
aeschsim | 99:78d87027c85b | 122 | left = left * speed_multiplier; |
aeschsim | 99:78d87027c85b | 123 | right = right * speed_multiplier; |
cittecla | 94:0381e8b1beda | 124 | set_speed(left, right); |
cittecla | 94:0381e8b1beda | 125 | } |
cittecla | 94:0381e8b1beda | 126 | |
cittecla | 61:628f8a4e857c | 127 | float speed_left = get_speed_left(); |
cittecla | 94:0381e8b1beda | 128 | 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 | 129 | t.reset(); |
cittecla | 75:dba260cb5ae4 | 130 | if(wanted_deg <= 0) { |
cittecla | 61:628f8a4e857c | 131 | set_speed(0,0); |
cittecla | 61:628f8a4e857c | 132 | is_turning = false; |
cittecla | 61:628f8a4e857c | 133 | t.stop(); |
cittecla | 61:628f8a4e857c | 134 | } |
cittecla | 61:628f8a4e857c | 135 | } |
cittecla | 94:0381e8b1beda | 136 | printf("remaining deg %f\r\n", wanted_deg); |
cittecla | 75:dba260cb5ae4 | 137 | return (wanted_deg); |
cittecla | 52:56399c2f13cd | 138 | } |
cittecla | 52:56399c2f13cd | 139 | |
cittecla | 52:56399c2f13cd | 140 | |
cittecla | 52:56399c2f13cd | 141 | int move_to_next_coord() |
cittecla | 52:56399c2f13cd | 142 | { |
cittecla | 52:56399c2f13cd | 143 | |
cittecla | 52:56399c2f13cd | 144 | float current_heading = get_current_heading(); |
cittecla | 53:453f24775644 | 145 | position current_pos = get_current_pos(); |
cittecla | 52:56399c2f13cd | 146 | position next_pos = get_next_pos(); |
cittecla | 61:628f8a4e857c | 147 | |
cittecla | 52:56399c2f13cd | 148 | float needed_heading = 0; |
cittecla | 52:56399c2f13cd | 149 | float distance = 0; |
cittecla | 61:628f8a4e857c | 150 | |
cittecla | 52:56399c2f13cd | 151 | // nord(-y) = 0 grad |
cittecla | 61:628f8a4e857c | 152 | if(current_pos.y > next_pos.y) { |
cittecla | 61:628f8a4e857c | 153 | if(current_pos.x > next_pos.x) needed_heading = 315; |
cittecla | 61:628f8a4e857c | 154 | distance = sqrt2; |
cittecla | 61:628f8a4e857c | 155 | if(current_pos.x == next_pos.x) needed_heading = 0; |
cittecla | 61:628f8a4e857c | 156 | distance = 1; |
cittecla | 61:628f8a4e857c | 157 | if(current_pos.x < next_pos.x) needed_heading = 45; |
cittecla | 61:628f8a4e857c | 158 | distance = sqrt2; |
cittecla | 61:628f8a4e857c | 159 | } |
cittecla | 61:628f8a4e857c | 160 | if(current_pos.y == next_pos.y) { |
cittecla | 61:628f8a4e857c | 161 | if(current_pos.x > next_pos.x) needed_heading = 270; |
cittecla | 61:628f8a4e857c | 162 | distance = 1; |
cittecla | 53:453f24775644 | 163 | if(current_pos.x == next_pos.x) //error same position; |
cittecla | 61:628f8a4e857c | 164 | if(current_pos.x < next_pos.x) needed_heading = 90; |
cittecla | 61:628f8a4e857c | 165 | distance = 1; |
cittecla | 61:628f8a4e857c | 166 | } |
cittecla | 61:628f8a4e857c | 167 | if(current_pos.y < next_pos.y) { |
cittecla | 61:628f8a4e857c | 168 | if(current_pos.x > next_pos.x) needed_heading = 225; |
cittecla | 61:628f8a4e857c | 169 | distance = sqrt2; |
cittecla | 61:628f8a4e857c | 170 | if(current_pos.x == next_pos.x) needed_heading = 180; |
cittecla | 61:628f8a4e857c | 171 | distance = 1; |
cittecla | 61:628f8a4e857c | 172 | if(current_pos.x < next_pos.x) needed_heading = 135; |
cittecla | 61:628f8a4e857c | 173 | distance = sqrt2; |
cittecla | 61:628f8a4e857c | 174 | } |
cittecla | 52:56399c2f13cd | 175 | |
cittecla | 52:56399c2f13cd | 176 | if(needed_heading != current_heading) { |
cittecla | 53:453f24775644 | 177 | turn_for_deg((needed_heading-current_heading)); |
cittecla | 61:628f8a4e857c | 178 | } else { |
cittecla | 85:d8ea8a99fa3a | 179 | move_for_distance(distance); |
cittecla | 39:92723f7ea54f | 180 | } |
cittecla | 39:92723f7ea54f | 181 | return 0; |
cittecla | 52:56399c2f13cd | 182 | } |
cittecla | 52:56399c2f13cd | 183 | |
PESGruppe1 | 74:d9c387b83196 | 184 | // Tobias Berger |
cittecla | 52:56399c2f13cd | 185 | int move_in_search_for_brick() |
cittecla | 52:56399c2f13cd | 186 | { |
PESGruppe1 | 79:50f8d58f79ab | 187 | float upper = getDistanceIR(2); // get distance from upper Center Sensor |
PESGruppe1 | 79:50f8d58f79ab | 188 | float lower = getDistanceIR(3); // get distance from Lower Center Sensor |
aeschsim | 96:1c6867536350 | 189 | printf("Current Search State: >%d<\r\n",search_state); |
cittecla | 86:df8c869a5a52 | 190 | switch (search_state) { |
cittecla | 86:df8c869a5a52 | 191 | case 0: //first cycle right |
cittecla | 86:df8c869a5a52 | 192 | turn_for_deg(60.0f); // call function and start turning |
cittecla | 86:df8c869a5a52 | 193 | search_state = 1; |
cittecla | 86:df8c869a5a52 | 194 | break; |
cittecla | 85:d8ea8a99fa3a | 195 | |
cittecla | 86:df8c869a5a52 | 196 | case 1: // turn right 60 deg |
cittecla | 86:df8c869a5a52 | 197 | if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 80cm at the lower Sensor |
cittecla | 86:df8c869a5a52 | 198 | if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor |
cittecla | 86:df8c869a5a52 | 199 | stop_turn(); |
cittecla | 86:df8c869a5a52 | 200 | search_state = 4; //brick found |
aeschsim | 99:78d87027c85b | 201 | printf("Brick found lower: %f upper:%f",lower,upper); |
cittecla | 86:df8c869a5a52 | 202 | } |
cittecla | 86:df8c869a5a52 | 203 | } else { |
cittecla | 86:df8c869a5a52 | 204 | search_state = 1; // go to same state |
cittecla | 86:df8c869a5a52 | 205 | if(turn_for_deg(0) < 0) { |
cittecla | 86:df8c869a5a52 | 206 | stop_turn(); |
cittecla | 86:df8c869a5a52 | 207 | search_state = 2; |
cittecla | 86:df8c869a5a52 | 208 | } |
cittecla | 86:df8c869a5a52 | 209 | } |
cittecla | 86:df8c869a5a52 | 210 | break; |
cittecla | 85:d8ea8a99fa3a | 211 | |
cittecla | 86:df8c869a5a52 | 212 | case 2: // first cycle left |
cittecla | 86:df8c869a5a52 | 213 | turn_for_deg(-120.0f); |
cittecla | 86:df8c869a5a52 | 214 | search_state = 3; |
cittecla | 86:df8c869a5a52 | 215 | break; |
cittecla | 85:d8ea8a99fa3a | 216 | |
cittecla | 86:df8c869a5a52 | 217 | case 3: // turn left 120 deg |
cittecla | 86:df8c869a5a52 | 218 | if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 80cm at the lower Sensor |
cittecla | 86:df8c869a5a52 | 219 | if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor |
cittecla | 86:df8c869a5a52 | 220 | stop_turn(); |
cittecla | 86:df8c869a5a52 | 221 | search_state = 4; //brick found |
aeschsim | 99:78d87027c85b | 222 | printf("Brick found lower: %f upper:%f",lower,upper); |
cittecla | 86:df8c869a5a52 | 223 | } |
cittecla | 86:df8c869a5a52 | 224 | } else { |
cittecla | 86:df8c869a5a52 | 225 | search_state = 3; // go to same state |
cittecla | 86:df8c869a5a52 | 226 | if(turn_for_deg(0) < 0) { |
cittecla | 86:df8c869a5a52 | 227 | stop_turn(); |
cittecla | 86:df8c869a5a52 | 228 | search_state = 10; // error |
cittecla | 86:df8c869a5a52 | 229 | } |
cittecla | 86:df8c869a5a52 | 230 | } |
cittecla | 86:df8c869a5a52 | 231 | break; |
cittecla | 52:56399c2f13cd | 232 | |
cittecla | 86:df8c869a5a52 | 233 | case 4: // first cycle move |
cittecla | 86:df8c869a5a52 | 234 | float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate |
cittecla | 86:df8c869a5a52 | 235 | move_for_distance(distance_to_Brick); |
cittecla | 86:df8c869a5a52 | 236 | search_state = 5; |
cittecla | 86:df8c869a5a52 | 237 | break; |
cittecla | 86:df8c869a5a52 | 238 | |
cittecla | 86:df8c869a5a52 | 239 | case 5: // move forward |
cittecla | 86:df8c869a5a52 | 240 | if(move_for_distance(0) < 0) { |
cittecla | 86:df8c869a5a52 | 241 | stop_move(); |
cittecla | 86:df8c869a5a52 | 242 | search_state = 6; |
cittecla | 86:df8c869a5a52 | 243 | } |
cittecla | 86:df8c869a5a52 | 244 | break; |
cittecla | 86:df8c869a5a52 | 245 | |
cittecla | 86:df8c869a5a52 | 246 | case 6: // Grabbing |
cittecla | 94:0381e8b1beda | 247 | return 50; //main state machine set as Grabbing |
cittecla | 94:0381e8b1beda | 248 | |
cittecla | 86:df8c869a5a52 | 249 | default: |
aeschsim | 96:1c6867536350 | 250 | printf("default State - move in search for brick\r\n"); |
cittecla | 86:df8c869a5a52 | 251 | // error |
cittecla | 86:df8c869a5a52 | 252 | break; |
PESGruppe1 | 74:d9c387b83196 | 253 | } |
cittecla | 86:df8c869a5a52 | 254 | return 47; //called until function is done |
cittecla | 52:56399c2f13cd | 255 | } |
cittecla | 52:56399c2f13cd | 256 | |
cittecla | 39:92723f7ea54f | 257 | |
cittecla | 39:92723f7ea54f | 258 | |
cittecla | 39:92723f7ea54f | 259 | |
cittecla | 39:92723f7ea54f | 260 | |
cittecla | 39:92723f7ea54f | 261 | |
cittecla | 39:92723f7ea54f | 262 | |
cittecla | 39:92723f7ea54f | 263 |