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@79:50f8d58f79ab, 2017-05-01 (annotated)

Committer:
PESGruppe1
Date:
Mon May 01 09:25:06 2017 +0000
Revision:
79:50f8d58f79ab
Parent:
77:ff87a10c4baf
Child:
83:5d3bca1ece20
Movement Move in search for brick printfs inserted

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