Tobias Berger / Mbed 2 deprecated Robocode

Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/Movement.cpp@101:6b10685aa34d, 2017-05-03 (annotated)

Committer:
cittecla
Date:
Wed May 03 09:24:52 2017 +0000
Revision:
101:6b10685aa34d
Parent:
99:78d87027c85b
Child:
104:7bc5eb2b4199
move_for_brick turns back after overshooting turning for deg when searching

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 18:a82994e67297 1 /**
cittecla 18:a82994e67297 2 * Movement function library
cittecla 18:a82994e67297 3 * Handels Movement of the Robot
cittecla 18:a82994e67297 4 **/
cittecla 39:92723f7ea54f 5
cittecla 39:92723f7ea54f 6 #include "Movement.h"
cittecla 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 101:6b10685aa34d 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 101:6b10685aa34d 197 if((lower<0.75f)&&(lower>0.05f)) { // 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 101:6b10685aa34d 200 search_state = 5; //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 101:6b10685aa34d 218 if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm 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 101:6b10685aa34d 221 search_state = 6; //brick found
cittecla 101:6b10685aa34d 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 101:6b10685aa34d 228 search_state = 20; // error
cittecla 86:df8c869a5a52 229 }
cittecla 86:df8c869a5a52 230 }
cittecla 86:df8c869a5a52 231 break;
cittecla 52:56399c2f13cd 232
cittecla 101:6b10685aa34d 233 case 5: // turn back left
cittecla 101:6b10685aa34d 234 turn_for_deg(-10.0f);
cittecla 101:6b10685aa34d 235 search_state = 8;
cittecla 101:6b10685aa34d 236 break;
cittecla 101:6b10685aa34d 237 case 6: // turn back right
cittecla 101:6b10685aa34d 238 turn_for_deg(10.0f);
cittecla 101:6b10685aa34d 239 search_state = 8;
cittecla 86:df8c869a5a52 240 break;
cittecla 101:6b10685aa34d 241
cittecla 101:6b10685aa34d 242 case 8: // turn back right continue
cittecla 101:6b10685aa34d 243 if((lower<0.75f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
cittecla 101:6b10685aa34d 244 if(fabsf((upper-lower))>0.02f) { // and nothing is detected with the upper Sensor
cittecla 101:6b10685aa34d 245 stop_turn();
cittecla 101:6b10685aa34d 246 search_state = 10; //brick found
cittecla 101:6b10685aa34d 247 }
cittecla 101:6b10685aa34d 248 } else {
cittecla 101:6b10685aa34d 249 search_state = 6; // go to same state
cittecla 101:6b10685aa34d 250 if(turn_for_deg(0) < 0) {
cittecla 101:6b10685aa34d 251 stop_turn();
cittecla 101:6b10685aa34d 252 search_state = 20; // error
cittecla 101:6b10685aa34d 253 }
cittecla 86:df8c869a5a52 254 }
cittecla 86:df8c869a5a52 255 break;
cittecla 86:df8c869a5a52 256
cittecla 101:6b10685aa34d 257
cittecla 101:6b10685aa34d 258 case 10: // first cycle move
cittecla 101:6b10685aa34d 259 float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
cittecla 101:6b10685aa34d 260 move_for_distance(distance_to_Brick);
cittecla 101:6b10685aa34d 261 search_state =11;
cittecla 101:6b10685aa34d 262 break;
cittecla 101:6b10685aa34d 263
cittecla 101:6b10685aa34d 264 case 11: // move forward
cittecla 101:6b10685aa34d 265 if(move_for_distance(0) < 0) {
cittecla 101:6b10685aa34d 266 stop_move();
cittecla 101:6b10685aa34d 267 search_state = 12;
cittecla 101:6b10685aa34d 268 }
cittecla 101:6b10685aa34d 269 break;
cittecla 101:6b10685aa34d 270
cittecla 101:6b10685aa34d 271 case 12: // Grabbing
cittecla 94:0381e8b1beda 272 return 50; //main state machine set as Grabbing
cittecla 94:0381e8b1beda 273
cittecla 86:df8c869a5a52 274 default:
aeschsim 96:1c6867536350 275 printf("default State - move in search for brick\r\n");
cittecla 86:df8c869a5a52 276 // error
cittecla 86:df8c869a5a52 277 break;
PESGruppe1 74:d9c387b83196 278 }
cittecla 86:df8c869a5a52 279 return 47; //called until function is done
cittecla 52:56399c2f13cd 280 }
cittecla 52:56399c2f13cd 281
cittecla 39:92723f7ea54f 282
cittecla 39:92723f7ea54f 283
cittecla 39:92723f7ea54f 284
cittecla 39:92723f7ea54f 285
cittecla 39:92723f7ea54f 286
cittecla 39:92723f7ea54f 287
cittecla 39:92723f7ea54f 288