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

Committer:
cittecla
Date:
Mon May 01 10:07:35 2017 +0000
Revision:
85:d8ea8a99fa3a
Parent:
81:956f65714207
Child:
86:df8c869a5a52
Implemented move_for_distance

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"
PESGruppe1 74:d9c387b83196 7 #define OFFSET_GREIFER_TO_IRSENSOR 100 // 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 85:d8ea8a99fa3a 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 85:d8ea8a99fa3a 17 bool first_search_cycle = true; // flag for state first time in function "move in search for brick"
PESGruppe1 74:d9c387b83196 18 bool brick_found = false; // flag for saving whether a brick was found or not
PESGruppe1 74:d9c387b83196 19 bool movement_to_brick_finished = false; // flag for saving whether movement to brick is finished or not
cittecla 77:ff87a10c4baf 20 float restdeg = 0;
cittecla 77:ff87a10c4baf 21
cittecla 61:628f8a4e857c 22
cittecla 52:56399c2f13cd 23 int moving()
cittecla 52:56399c2f13cd 24 {
cittecla 52:56399c2f13cd 25
cittecla 39:92723f7ea54f 26 return 0;
cittecla 52:56399c2f13cd 27 }
cittecla 52:56399c2f13cd 28
cittecla 85:d8ea8a99fa3a 29 float move_for_distance(float distance)
cittecla 52:56399c2f13cd 30 {
cittecla 85:d8ea8a99fa3a 31 if(distance != 0) {
cittecla 52:56399c2f13cd 32
cittecla 85:d8ea8a99fa3a 33 is_moving = true;
cittecla 85:d8ea8a99fa3a 34 float left = 0;
cittecla 85:d8ea8a99fa3a 35 float right = 0;
cittecla 85:d8ea8a99fa3a 36
cittecla 85:d8ea8a99fa3a 37 wanted_dist = sqrt(distance*distance);
cittecla 85:d8ea8a99fa3a 38
cittecla 85:d8ea8a99fa3a 39 if(distance > 0) { //move forward
cittecla 85:d8ea8a99fa3a 40 direction = 1;
cittecla 85:d8ea8a99fa3a 41 left = 50.0f;
cittecla 85:d8ea8a99fa3a 42 right = 50.0f;
cittecla 85:d8ea8a99fa3a 43 } else { //move backward
cittecla 85:d8ea8a99fa3a 44 direction = 0;
cittecla 85:d8ea8a99fa3a 45 left = -50.0f;
cittecla 85:d8ea8a99fa3a 46 right = -50.0f;
cittecla 85:d8ea8a99fa3a 47 }
cittecla 85:d8ea8a99fa3a 48 set_speed(left, right);
cittecla 85:d8ea8a99fa3a 49 t.reset();
cittecla 85:d8ea8a99fa3a 50 t.start();
cittecla 85:d8ea8a99fa3a 51
cittecla 85:d8ea8a99fa3a 52 } else {
cittecla 85:d8ea8a99fa3a 53
cittecla 85:d8ea8a99fa3a 54 float speed_left = get_speed_left();
cittecla 85:d8ea8a99fa3a 55 wanted_dist -= (2*(float)wheel_r*(float)M_PI) * t.read() * fabsf(speed_left);
cittecla 85:d8ea8a99fa3a 56 t.reset();
cittecla 85:d8ea8a99fa3a 57
cittecla 85:d8ea8a99fa3a 58 if(wanted_dist <= 0) { //distance covered, Stop function
cittecla 85:d8ea8a99fa3a 59 set_speed(0,0);
cittecla 85:d8ea8a99fa3a 60 is_moving = false;
cittecla 85:d8ea8a99fa3a 61 t.stop();
cittecla 85:d8ea8a99fa3a 62 }
cittecla 85:d8ea8a99fa3a 63 }
cittecla 85:d8ea8a99fa3a 64 return wanted_dist;
cittecla 52:56399c2f13cd 65 }
cittecla 52:56399c2f13cd 66
cittecla 52:56399c2f13cd 67 int move_backward_for_distance()
cittecla 52:56399c2f13cd 68 {
cittecla 52:56399c2f13cd 69
cittecla 39:92723f7ea54f 70 return 0;
cittecla 52:56399c2f13cd 71 }
cittecla 52:56399c2f13cd 72
cittecla 85:d8ea8a99fa3a 73 float turn_for_deg(float deg) //if deg not 0 equals initilisation.
cittecla 52:56399c2f13cd 74 {
cittecla 52:56399c2f13cd 75
cittecla 75:dba260cb5ae4 76 if(deg != 0) {
cittecla 61:628f8a4e857c 77
cittecla 61:628f8a4e857c 78 is_turning = true;
cittecla 61:628f8a4e857c 79 float left = 0;
cittecla 61:628f8a4e857c 80 float right = 0;
cittecla 61:628f8a4e857c 81
cittecla 75:dba260cb5ae4 82 wanted_deg = sqrt(deg*deg);
cittecla 61:628f8a4e857c 83
cittecla 61:628f8a4e857c 84 if(deg < 0) { // turn left
cittecla 61:628f8a4e857c 85 direction = 1;
cittecla 61:628f8a4e857c 86 left = -50.0f;
cittecla 61:628f8a4e857c 87 right = 50.0f;
cittecla 61:628f8a4e857c 88 } else { // turn right
cittecla 61:628f8a4e857c 89 direction = 0;
cittecla 61:628f8a4e857c 90 left = 50.0f;
cittecla 61:628f8a4e857c 91 right = -50.0f;
cittecla 61:628f8a4e857c 92 }
cittecla 61:628f8a4e857c 93 set_speed(left, right);
cittecla 71:ddf4eb5c3081 94 t.reset();
cittecla 61:628f8a4e857c 95 t.start();
cittecla 61:628f8a4e857c 96
cittecla 61:628f8a4e857c 97 } else {
cittecla 61:628f8a4e857c 98
cittecla 61:628f8a4e857c 99 float speed_left = get_speed_left();
cittecla 85:d8ea8a99fa3a 100 wanted_deg -= 360.0f / (2*(float)wheel_r*(float)M_PI) * t.read() * fabsf(speed_left);
cittecla 75:dba260cb5ae4 101 t.reset();
cittecla 75:dba260cb5ae4 102 if(wanted_deg <= 0) {
cittecla 61:628f8a4e857c 103 set_speed(0,0);
cittecla 61:628f8a4e857c 104 is_turning = false;
cittecla 61:628f8a4e857c 105 t.stop();
cittecla 61:628f8a4e857c 106 }
cittecla 61:628f8a4e857c 107 }
cittecla 75:dba260cb5ae4 108 return (wanted_deg);
cittecla 52:56399c2f13cd 109 }
cittecla 52:56399c2f13cd 110
cittecla 52:56399c2f13cd 111
cittecla 52:56399c2f13cd 112 int move_to_next_coord()
cittecla 52:56399c2f13cd 113 {
cittecla 52:56399c2f13cd 114
cittecla 52:56399c2f13cd 115 float current_heading = get_current_heading();
cittecla 53:453f24775644 116 position current_pos = get_current_pos();
cittecla 52:56399c2f13cd 117 position next_pos = get_next_pos();
cittecla 61:628f8a4e857c 118
cittecla 52:56399c2f13cd 119 float needed_heading = 0;
cittecla 52:56399c2f13cd 120 float distance = 0;
cittecla 61:628f8a4e857c 121
cittecla 52:56399c2f13cd 122 // nord(-y) = 0 grad
cittecla 61:628f8a4e857c 123 if(current_pos.y > next_pos.y) {
cittecla 61:628f8a4e857c 124 if(current_pos.x > next_pos.x) needed_heading = 315;
cittecla 61:628f8a4e857c 125 distance = sqrt2;
cittecla 61:628f8a4e857c 126 if(current_pos.x == next_pos.x) needed_heading = 0;
cittecla 61:628f8a4e857c 127 distance = 1;
cittecla 61:628f8a4e857c 128 if(current_pos.x < next_pos.x) needed_heading = 45;
cittecla 61:628f8a4e857c 129 distance = sqrt2;
cittecla 61:628f8a4e857c 130 }
cittecla 61:628f8a4e857c 131 if(current_pos.y == next_pos.y) {
cittecla 61:628f8a4e857c 132 if(current_pos.x > next_pos.x) needed_heading = 270;
cittecla 61:628f8a4e857c 133 distance = 1;
cittecla 53:453f24775644 134 if(current_pos.x == next_pos.x) //error same position;
cittecla 61:628f8a4e857c 135 if(current_pos.x < next_pos.x) needed_heading = 90;
cittecla 61:628f8a4e857c 136 distance = 1;
cittecla 61:628f8a4e857c 137 }
cittecla 61:628f8a4e857c 138 if(current_pos.y < next_pos.y) {
cittecla 61:628f8a4e857c 139 if(current_pos.x > next_pos.x) needed_heading = 225;
cittecla 61:628f8a4e857c 140 distance = sqrt2;
cittecla 61:628f8a4e857c 141 if(current_pos.x == next_pos.x) needed_heading = 180;
cittecla 61:628f8a4e857c 142 distance = 1;
cittecla 61:628f8a4e857c 143 if(current_pos.x < next_pos.x) needed_heading = 135;
cittecla 61:628f8a4e857c 144 distance = sqrt2;
cittecla 61:628f8a4e857c 145 }
cittecla 52:56399c2f13cd 146
cittecla 52:56399c2f13cd 147 if(needed_heading != current_heading) {
cittecla 53:453f24775644 148 turn_for_deg((needed_heading-current_heading));
cittecla 61:628f8a4e857c 149 } else {
cittecla 85:d8ea8a99fa3a 150 move_for_distance(distance);
cittecla 39:92723f7ea54f 151 }
cittecla 39:92723f7ea54f 152 return 0;
cittecla 52:56399c2f13cd 153 }
cittecla 52:56399c2f13cd 154
PESGruppe1 74:d9c387b83196 155 // Tobias Berger
cittecla 52:56399c2f13cd 156 int move_in_search_for_brick()
cittecla 52:56399c2f13cd 157 {
cittecla 85:d8ea8a99fa3a 158
PESGruppe1 74:d9c387b83196 159 float distance_to_Brick; // variable how far away the brick is
PESGruppe1 74:d9c387b83196 160 // Init State turn for 60 degrees CW
cittecla 85:d8ea8a99fa3a 161 if(first_search_cycle==true) {
PESGruppe1 74:d9c387b83196 162 first_search_cycle=false; // delet flag for initial condition
cittecla 77:ff87a10c4baf 163 restdeg=turn_for_deg(60); // call function and start turning
PESGruppe1 74:d9c387b83196 164 }
cittecla 85:d8ea8a99fa3a 165
PESGruppe1 74:d9c387b83196 166 // Search for Brick and evaluation
PESGruppe1 74:d9c387b83196 167 float upper = getDistanceIR(4); // get distance from upper Center Sensor CHECK SENSORNUMBERS NOT SURE
PESGruppe1 74:d9c387b83196 168 float lower = getDistanceIR(6); // get distance from Lower Center Sensor
cittecla 85:d8ea8a99fa3a 169
cittecla 85:d8ea8a99fa3a 170 if((lower<800.0f)&&(lower>100.0f)) { // if something is in the range of 10 to 80cm at the lower Sensor
cittecla 85:d8ea8a99fa3a 171 if((upper>800.0f)&&(upper<100.0f)) { // and nothing is detected with the upper Sensor
cittecla 85:d8ea8a99fa3a 172 brick_found = true;
PESGruppe1 74:d9c387b83196 173 }
cittecla 85:d8ea8a99fa3a 174 } else {
PESGruppe1 74:d9c387b83196 175 brick_found = false;
cittecla 85:d8ea8a99fa3a 176
cittecla 85:d8ea8a99fa3a 177 if((restdeg>1)||(restdeg<-1)) { // continue turning until restdegree nearly 0
PESGruppe1 74:d9c387b83196 178 turn_for_deg(restdeg);
cittecla 85:d8ea8a99fa3a 179 } else { // if restdegree nearly 0 and nothing found => turn in other direction
cittecla 85:d8ea8a99fa3a 180 restdeg=-60; // 60 DEGREES FROM YET WILL BE THE SAME AREA AS PREVIOUSLY
PESGruppe1 74:d9c387b83196 181 }
cittecla 85:d8ea8a99fa3a 182
PESGruppe1 74:d9c387b83196 183 }
cittecla 52:56399c2f13cd 184
cittecla 85:d8ea8a99fa3a 185 if(brick_found==true) {
PESGruppe1 74:d9c387b83196 186 turn_for_deg(0); // stop turning
cittecla 77:ff87a10c4baf 187 first_search_cycle=true; // set flag to start turning once again respectivly to get in Initialstate
cittecla 77:ff87a10c4baf 188 lower=getDistanceIR(6); // Measure distance to Brick for Movement
PESGruppe1 74:d9c387b83196 189 distance_to_Brick = lower-OFFSET_GREIFER_TO_IRSENSOR; // calculate
cittecla 85:d8ea8a99fa3a 190 move_for_distance(distance_to_Brick); //not whole distance, rest in next function // Move to Brick ATTENTION FUNCTION NOT IMPLEMENTED YET
PESGruppe1 74:d9c387b83196 191 arm_position_grabbing(); // Call Aeschlimans function MOVE A LITTLE AFTER GREIFER ON FLOOR IN AESCHLIMANS FUNCTION?
PESGruppe1 74:d9c387b83196 192 //movement_to_brick_finished=true;
PESGruppe1 74:d9c387b83196 193 }
PESGruppe1 74:d9c387b83196 194
PESGruppe1 74:d9c387b83196 195
cittecla 85:d8ea8a99fa3a 196
cittecla 52:56399c2f13cd 197 return 0;
cittecla 52:56399c2f13cd 198 }
cittecla 52:56399c2f13cd 199
cittecla 39:92723f7ea54f 200
cittecla 39:92723f7ea54f 201
cittecla 39:92723f7ea54f 202
cittecla 39:92723f7ea54f 203
cittecla 39:92723f7ea54f 204
cittecla 39:92723f7ea54f 205
cittecla 39:92723f7ea54f 206