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

Committer:
PESGruppe1
Date:
Tue Apr 25 08:02:22 2017 +0000
Revision:
76:bdbdd64cdd80
Parent:
74:d9c387b83196
Parent:
75:dba260cb5ae4
Child:
77:ff87a10c4baf
Move to brick v001 Tobias Berger merged;

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