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@75:dba260cb5ae4, 2017-04-24 (annotated)

Committer:
cittecla
Date:
Mon Apr 24 15:01:23 2017 +0000
Revision:
75:dba260cb5ae4
Parent:
71:ddf4eb5c3081
Child:
76:bdbdd64cdd80
updated turn_for_deg

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 39:92723f7ea54f 7
cittecla 61:628f8a4e857c 8 bool is_turning = false;
cittecla 61:628f8a4e857c 9 bool direction = false;
cittecla 61:628f8a4e857c 10 float wanted_deg = 0;
cittecla 63:b27aa01c2cf6 11 Timer t;
cittecla 61:628f8a4e857c 12 float previous_t = 0;
cittecla 61:628f8a4e857c 13
cittecla 52:56399c2f13cd 14 int moving()
cittecla 52:56399c2f13cd 15 {
cittecla 52:56399c2f13cd 16
cittecla 39:92723f7ea54f 17 return 0;
cittecla 52:56399c2f13cd 18 }
cittecla 52:56399c2f13cd 19
cittecla 52:56399c2f13cd 20 int move_forward_for_distance(float distance)
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_backward_for_distance()
cittecla 52:56399c2f13cd 27 {
cittecla 52:56399c2f13cd 28
cittecla 39:92723f7ea54f 29 return 0;
cittecla 52:56399c2f13cd 30 }
cittecla 52:56399c2f13cd 31
cittecla 75:dba260cb5ae4 32 float turn_for_deg(float deg) //if deg not 0 equals initilisation.
cittecla 52:56399c2f13cd 33 {
cittecla 52:56399c2f13cd 34
cittecla 75:dba260cb5ae4 35 if(deg != 0) {
cittecla 61:628f8a4e857c 36
cittecla 61:628f8a4e857c 37 is_turning = true;
cittecla 61:628f8a4e857c 38 float left = 0;
cittecla 61:628f8a4e857c 39 float right = 0;
cittecla 61:628f8a4e857c 40
cittecla 75:dba260cb5ae4 41 wanted_deg = sqrt(deg*deg);
cittecla 61:628f8a4e857c 42
cittecla 61:628f8a4e857c 43 if(deg < 0) { // turn left
cittecla 61:628f8a4e857c 44 direction = 1;
cittecla 61:628f8a4e857c 45 left = -50.0f;
cittecla 61:628f8a4e857c 46 right = 50.0f;
cittecla 61:628f8a4e857c 47 } else { // turn right
cittecla 61:628f8a4e857c 48 direction = 0;
cittecla 61:628f8a4e857c 49 left = 50.0f;
cittecla 61:628f8a4e857c 50 right = -50.0f;
cittecla 61:628f8a4e857c 51 }
cittecla 61:628f8a4e857c 52 set_speed(left, right);
cittecla 71:ddf4eb5c3081 53 t.reset();
cittecla 61:628f8a4e857c 54 t.start();
cittecla 61:628f8a4e857c 55
cittecla 61:628f8a4e857c 56 } else {
cittecla 61:628f8a4e857c 57
cittecla 61:628f8a4e857c 58 float speed_left = get_speed_left();
cittecla 75:dba260cb5ae4 59 wanted_deg -= 360.0f / (2*radius*M_PI) * t.read() * fabsf(speed_left);
cittecla 75:dba260cb5ae4 60 t.reset();
cittecla 75:dba260cb5ae4 61 if(wanted_deg <= 0) {
cittecla 61:628f8a4e857c 62 set_speed(0,0);
cittecla 61:628f8a4e857c 63 is_turning = false;
cittecla 61:628f8a4e857c 64 t.stop();
cittecla 61:628f8a4e857c 65 }
cittecla 61:628f8a4e857c 66 }
cittecla 75:dba260cb5ae4 67 return (wanted_deg);
cittecla 52:56399c2f13cd 68 }
cittecla 52:56399c2f13cd 69
cittecla 52:56399c2f13cd 70
cittecla 52:56399c2f13cd 71 int move_to_next_coord()
cittecla 52:56399c2f13cd 72 {
cittecla 52:56399c2f13cd 73
cittecla 52:56399c2f13cd 74 float current_heading = get_current_heading();
cittecla 53:453f24775644 75 position current_pos = get_current_pos();
cittecla 52:56399c2f13cd 76 position next_pos = get_next_pos();
cittecla 61:628f8a4e857c 77
cittecla 52:56399c2f13cd 78 float needed_heading = 0;
cittecla 52:56399c2f13cd 79 float distance = 0;
cittecla 61:628f8a4e857c 80
cittecla 52:56399c2f13cd 81 // nord(-y) = 0 grad
cittecla 61:628f8a4e857c 82 if(current_pos.y > next_pos.y) {
cittecla 61:628f8a4e857c 83 if(current_pos.x > next_pos.x) needed_heading = 315;
cittecla 61:628f8a4e857c 84 distance = sqrt2;
cittecla 61:628f8a4e857c 85 if(current_pos.x == next_pos.x) needed_heading = 0;
cittecla 61:628f8a4e857c 86 distance = 1;
cittecla 61:628f8a4e857c 87 if(current_pos.x < next_pos.x) needed_heading = 45;
cittecla 61:628f8a4e857c 88 distance = sqrt2;
cittecla 61:628f8a4e857c 89 }
cittecla 61:628f8a4e857c 90 if(current_pos.y == next_pos.y) {
cittecla 61:628f8a4e857c 91 if(current_pos.x > next_pos.x) needed_heading = 270;
cittecla 61:628f8a4e857c 92 distance = 1;
cittecla 53:453f24775644 93 if(current_pos.x == next_pos.x) //error same position;
cittecla 61:628f8a4e857c 94 if(current_pos.x < next_pos.x) needed_heading = 90;
cittecla 61:628f8a4e857c 95 distance = 1;
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 = 225;
cittecla 61:628f8a4e857c 99 distance = sqrt2;
cittecla 61:628f8a4e857c 100 if(current_pos.x == next_pos.x) needed_heading = 180;
cittecla 61:628f8a4e857c 101 distance = 1;
cittecla 61:628f8a4e857c 102 if(current_pos.x < next_pos.x) needed_heading = 135;
cittecla 61:628f8a4e857c 103 distance = sqrt2;
cittecla 61:628f8a4e857c 104 }
cittecla 52:56399c2f13cd 105
cittecla 52:56399c2f13cd 106 if(needed_heading != current_heading) {
cittecla 53:453f24775644 107 turn_for_deg((needed_heading-current_heading));
cittecla 61:628f8a4e857c 108 } else {
cittecla 53:453f24775644 109 move_forward_for_distance(distance);
cittecla 39:92723f7ea54f 110 }
cittecla 39:92723f7ea54f 111 return 0;
cittecla 52:56399c2f13cd 112 }
cittecla 52:56399c2f13cd 113
cittecla 52:56399c2f13cd 114 int move_in_search_for_brick()
cittecla 52:56399c2f13cd 115 {
cittecla 52:56399c2f13cd 116
cittecla 52:56399c2f13cd 117 return 0;
cittecla 52:56399c2f13cd 118 }
cittecla 52:56399c2f13cd 119
cittecla 39:92723f7ea54f 120
cittecla 39:92723f7ea54f 121
cittecla 39:92723f7ea54f 122
cittecla 39:92723f7ea54f 123
cittecla 39:92723f7ea54f 124
cittecla 39:92723f7ea54f 125
cittecla 39:92723f7ea54f 126