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/Positioning.cpp@68:29c52ea147d5, 2017-04-19 (annotated)

Committer:
aeschsim
Date:
Wed Apr 19 14:00:59 2017 +0000
Revision:
68:29c52ea147d5
Parent:
64:8cfca8fad65d
Child:
69:1fdcef6a7577
Merged verion of bugfix and testprogramm for arm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 12:91c2e07d2b5b 1 /**
cittecla 12:91c2e07d2b5b 2 * Positioning function library
cittecla 12:91c2e07d2b5b 3 * Handels position of the Robot on the map
cittecla 12:91c2e07d2b5b 4 **/
cittecla 33:8a98f8b9d859 5
cittecla 33:8a98f8b9d859 6
cittecla 38:3526c36e4c73 7 #include "Positioning.h"
cittecla 62:c2fcf3b349e9 8 coordinates current_coord; // Updated
cittecla 62:c2fcf3b349e9 9 position current_pos; // Generated from current_coord
cittecla 46:8b52c7b34d34 10 float current_heading;
cittecla 52:56399c2f13cd 11 position next_pos;
cittecla 12:91c2e07d2b5b 12
cittecla 62:c2fcf3b349e9 13 bool init = false;
aeschsim 68:29c52ea147d5 14 Timer t2;
cittecla 62:c2fcf3b349e9 15
cittecla 62:c2fcf3b349e9 16 int deg_r = 0;
cittecla 62:c2fcf3b349e9 17 int deg_l = 0;
cittecla 62:c2fcf3b349e9 18 float last_dist_r = 0;
cittecla 62:c2fcf3b349e9 19 float last_dist_l = 0;
cittecla 62:c2fcf3b349e9 20
cittecla 62:c2fcf3b349e9 21
cittecla 62:c2fcf3b349e9 22 coordinates get_current_coord()
cittecla 62:c2fcf3b349e9 23 {
cittecla 62:c2fcf3b349e9 24 return current_coord;
cittecla 62:c2fcf3b349e9 25 }
cittecla 62:c2fcf3b349e9 26
cittecla 62:c2fcf3b349e9 27 coordinates pos_to_coord(position pos)
cittecla 62:c2fcf3b349e9 28 {
cittecla 62:c2fcf3b349e9 29 coordinates coord = {0};
cittecla 62:c2fcf3b349e9 30 coord.x = pos.x + 0.5f;
cittecla 62:c2fcf3b349e9 31 coord.y = pos.y + 0.5f;
cittecla 62:c2fcf3b349e9 32 return coord;
cittecla 62:c2fcf3b349e9 33 }
cittecla 62:c2fcf3b349e9 34
cittecla 62:c2fcf3b349e9 35 position coord_to_pos(coordinates coord)
cittecla 62:c2fcf3b349e9 36 {
cittecla 62:c2fcf3b349e9 37 position pos = {0};
cittecla 62:c2fcf3b349e9 38 pos.x = rint(coord.x -0.5f);
cittecla 62:c2fcf3b349e9 39 pos.y = rint(coord.y -0.5f);
cittecla 62:c2fcf3b349e9 40 return pos;
cittecla 62:c2fcf3b349e9 41 }
cittecla 62:c2fcf3b349e9 42
cittecla 34:40d8d29b44b8 43 position get_current_pos()
cittecla 33:8a98f8b9d859 44 {
cittecla 62:c2fcf3b349e9 45 current_pos = coord_to_pos(current_coord);
cittecla 33:8a98f8b9d859 46 return current_pos;
cittecla 33:8a98f8b9d859 47 }
cittecla 38:3526c36e4c73 48
cittecla 52:56399c2f13cd 49 position get_next_pos()
cittecla 52:56399c2f13cd 50 {
cittecla 52:56399c2f13cd 51 return next_pos;
cittecla 52:56399c2f13cd 52 }
cittecla 52:56399c2f13cd 53
cittecla 46:8b52c7b34d34 54 float get_current_heading()
cittecla 46:8b52c7b34d34 55 {
cittecla 46:8b52c7b34d34 56 return current_heading;
cittecla 46:8b52c7b34d34 57 }
cittecla 46:8b52c7b34d34 58
cittecla 38:3526c36e4c73 59
cittecla 60:b57577b0072f 60 void positioning()
cittecla 39:92723f7ea54f 61 {
cittecla 60:b57577b0072f 62 printf("positioning...\r\n");
cittecla 39:92723f7ea54f 63 }
cittecla 38:3526c36e4c73 64
cittecla 38:3526c36e4c73 65
cittecla 62:c2fcf3b349e9 66
cittecla 62:c2fcf3b349e9 67 int initial_positioning()
cittecla 33:8a98f8b9d859 68 {
cittecla 62:c2fcf3b349e9 69 if(init == false) {
cittecla 62:c2fcf3b349e9 70
cittecla 62:c2fcf3b349e9 71 last_dist_r = 100;
cittecla 62:c2fcf3b349e9 72 last_dist_l = 100;
cittecla 33:8a98f8b9d859 73
cittecla 62:c2fcf3b349e9 74 deg_r = -50;
cittecla 62:c2fcf3b349e9 75 deg_l = 50;
cittecla 62:c2fcf3b349e9 76
cittecla 62:c2fcf3b349e9 77 set_servo_position(0,deg_l); //servo sensor left
cittecla 62:c2fcf3b349e9 78 set_servo_position(2,-deg_r); //servo sensor right
cittecla 62:c2fcf3b349e9 79 t2.start();
cittecla 62:c2fcf3b349e9 80 init = true;
cittecla 62:c2fcf3b349e9 81 } else {
cittecla 33:8a98f8b9d859 82
cittecla 62:c2fcf3b349e9 83 if(t2 > 0.2f) {
cittecla 62:c2fcf3b349e9 84 t2 = 0;
cittecla 62:c2fcf3b349e9 85
cittecla 62:c2fcf3b349e9 86 float dist_r = getDistanceIR(4);
cittecla 62:c2fcf3b349e9 87 float dist_l = getDistanceIR(0);
cittecla 12:91c2e07d2b5b 88
cittecla 62:c2fcf3b349e9 89 //right
cittecla 62:c2fcf3b349e9 90 if(dist_r < last_dist_r) {
cittecla 62:c2fcf3b349e9 91 last_dist_r = dist_r;
cittecla 62:c2fcf3b349e9 92 deg_r += 5;
cittecla 62:c2fcf3b349e9 93 set_servo_position(2,-deg_r);
cittecla 62:c2fcf3b349e9 94 } else {
cittecla 62:c2fcf3b349e9 95
cittecla 62:c2fcf3b349e9 96
cittecla 62:c2fcf3b349e9 97 // current_coord.y =
cittecla 62:c2fcf3b349e9 98 }
cittecla 62:c2fcf3b349e9 99
cittecla 62:c2fcf3b349e9 100
cittecla 62:c2fcf3b349e9 101 //left
cittecla 62:c2fcf3b349e9 102 }
cittecla 33:8a98f8b9d859 103
cittecla 33:8a98f8b9d859 104 }
cittecla 62:c2fcf3b349e9 105 //finished
cittecla 62:c2fcf3b349e9 106 //return 11
cittecla 33:8a98f8b9d859 107
cittecla 62:c2fcf3b349e9 108 // not finished*/
cittecla 62:c2fcf3b349e9 109 return 16;
cittecla 62:c2fcf3b349e9 110 }
cittecla 62:c2fcf3b349e9 111
cittecla 64:8cfca8fad65d 112 /*
cittecla 62:c2fcf3b349e9 113 turn_straight_right();
cittecla 62:c2fcf3b349e9 114 turn_straight_left();
cittecla 62:c2fcf3b349e9 115
cittecla 62:c2fcf3b349e9 116 while(last_dist_r > sensors[r]) {
cittecla 62:c2fcf3b349e9 117 turn_sensor_right(1); //turn sensor + 1 deg
cittecla 62:c2fcf3b349e9 118 wait(0.1f)
cittecla 62:c2fcf3b349e9 119 deg_r += 1;
cittecla 62:c2fcf3b349e9 120 last_dist_r = sensors[r];
cittecla 62:c2fcf3b349e9 121 }
cittecla 33:8a98f8b9d859 122
cittecla 62:c2fcf3b349e9 123 while(last_dist_l > sensors[l]) {
cittecla 62:c2fcf3b349e9 124 turn_sensor_left(-1); //turn sensor - 1 deg
cittecla 62:c2fcf3b349e9 125 wait(0.1f)
cittecla 62:c2fcf3b349e9 126 deg_l += 1;
cittecla 62:c2fcf3b349e9 127 last_dist_l = sensors[l];
cittecla 62:c2fcf3b349e9 128 }
cittecla 62:c2fcf3b349e9 129
cittecla 62:c2fcf3b349e9 130 int deg_l_2=0;
cittecla 62:c2fcf3b349e9 131 turn_straight_left();
cittecla 62:c2fcf3b349e9 132 last_dist_l = 0;
cittecla 39:92723f7ea54f 133
cittecla 62:c2fcf3b349e9 134 while(last_dist_l < sensors[l]) {
cittecla 62:c2fcf3b349e9 135 turn_sensor_left(1); //turn sensor +1 deg (positiv = uhrzeigersinn)
cittecla 62:c2fcf3b349e9 136 wait(0.1f);
cittecla 62:c2fcf3b349e9 137 deg_l_2 += 1;
cittecla 62:c2fcf3b349e9 138 last_dist_l = sensors[l];
cittecla 62:c2fcf3b349e9 139 }
cittecla 39:92723f7ea54f 140
cittecla 62:c2fcf3b349e9 141 turn_straight_right();
cittecla 62:c2fcf3b349e9 142 turn_straight_left();
cittecla 39:92723f7ea54f 143
cittecla 62:c2fcf3b349e9 144 wait(0.2f);
cittecla 62:c2fcf3b349e9 145 */
cittecla 62:c2fcf3b349e9 146