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@88:b89cace9329b, 2017-05-01 (annotated)

Committer:
cittecla
Date:
Mon May 01 13:38:00 2017 +0000
Revision:
88:b89cace9329b
Parent:
86:df8c869a5a52
Child:
89:7f9d6e641a01
something

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