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@74:d9c387b83196, 2017-04-25 (annotated)

Committer:
PESGruppe1
Date:
Tue Apr 25 07:59:59 2017 +0000
Revision:
74:d9c387b83196
Parent:
71:ddf4eb5c3081
Child:
76:bdbdd64cdd80
Added move to brick v001 Tobias Berger

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