Tobias Berger
Tobis Programm forked to not destroy your golden files
Fork of
Robocode
by 
PES 2 - Gruppe 1
Diff: source/Movement.cpp
- Revision:
- 116:e03a3692cdf0
- Parent:
- 114:720dc5df42a5
- Child:
- 117:66d64dbd1b36
--- a/source/Movement.cpp Wed May 10 07:45:33 2017 +0000
+++ b/source/Movement.cpp Wed May 10 08:52:04 2017 +0000
@@ -1,4 +1,4 @@
-/*/**
+/**
* Movement function library
* Handels Movement of the Robot
**/
@@ -15,7 +15,7 @@
float restdegAfterstop = 0; // Variable for Rest degree we still have to cover after e.g. 1 brick found but its not really a brick so we turn further until e.g 60 degrees covered.
-float TOLERANCE_BRICK_OR_OBSTACLE = 0.08; // Variable for Brick detection it sets how much upper sensor and lower can differ that its still detected as an obstacle and not a brick
+float TOLERANCE_BRICK_OR_OBSTACLE = 0.08f; // Variable for Brick detection it sets how much upper sensor and lower can differ that its still detected as an obstacle and not a brick
Timer t;
Timer t8; // timer used for waiting enough distance measurements
@@ -307,14 +307,15 @@
//printf("Current Search State: >%d<\r\n",search_state);
switch (search_state) {
case 0: //first cycle right
- turn_for_deg(60.0f,0.2f); // call function and start turning
+ turn_for_deg(60.0f,0.6f); // call function and start turning
search_state = 1;
break;
case 1: // turn right and check for obstacles
- if((lower<0.45f)&&(lower>0.1f)) { // if something is in the range of 10 to 80cm at the lower Sensor
+ if((lower<0.45f)&&(lower>0.08f)) { // if something is in the range of 10 to 80cm at the lower Sensor
if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
stop_turn();
+ t8.reset();
t8.start(); // start timer for enough measurements
restdegAfterstop = turn_for_deg(0,1); // get restdegrees from turn function. if a brick is falsly detected we turn restdegAfterstop to finisch search turn
search_state = 2; // brick found
@@ -332,24 +333,25 @@
case 2: // Check if Sensor after waiting still the same value
- if(t8.read() > 0.5f) {
- float lowerAfterWait = getDistanceIR(3); // get distance from Lower Sensor after Waiting
- if(fabs(lowerAfterWait-lower)<0.05) { // Compare If Measurement after waiting is still the same
- search_state = 10; // When still the same go to move forward
- } else {
- search_state=3; // When afterwait not the same go to continue turning
+ if(t8.read() > 0.1f) {
+ if((lower<0.45f)&&(lower>0.08f)) { // if something is in the range of 10 to 80cm at the lower Sensor
+ if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
+ search_state = 10; // When still the same go to move forward
+ } else {
+ search_state=3; // When afterwait not the same go to continue turning
+ }
}
}
break;
case 3: // init continue turning for restdeg
- turn_for_deg(restdegAfterstop,0.2f); // call function and start turning for restdegrees after stop
+ turn_for_deg(restdegAfterstop,0.6f); // call function and start turning for restdegrees after stop
search_state = 1; // go back to turn and search
break;
case 4: // init turn left 120 deg
- turn_for_deg(-120.0f,0.2);
+ turn_for_deg(-120.0f,0.6);
search_state = 5;
break;
@@ -357,6 +359,7 @@
if((lower<0.45f)&&(lower>0.05f)) { // if something is in the range of 10 to 80cm at the lower Sensor
if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
stop_turn();
+ t8.reset();
t8.start(); // start timer for enough measurements
restdegAfterstop = turn_for_deg(0,1); // get restdegrees from turn function. if a brick is falsly detected we turn restdegAfterstop to finisch search turn
search_state = 6; // brick found
@@ -371,14 +374,14 @@
}
break;
-
- case 6: // Check if Sensor after waiting still the same value
- if(t8.read() > 0.5f) {
- float lowerAfterWait = getDistanceIR(3); // get distance from Lower Sensor after Waiting
- if(fabs(lowerAfterWait-lower)<0.05) { // Compare If Measurement after waiting is still the same
- search_state = 10; // When still the same go to move forward
- } else {
- search_state=7; // When afterwait not the same go to continue turning
+ case 6: // Check if Sensor after waiting still detect brick
+ if(t8.read() > 0.1f) {
+ if((lower<0.45f)&&(lower>0.08f)) { // if something is in the range of 10 to 80cm at the lower Sensor
+ if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
+ search_state = 10; // When still the same go to move forward
+ } else {
+ search_state=7; // When afterwait not the same go to continue turning
+ }
}
}
break;
@@ -388,63 +391,6 @@
search_state = 5; // go back to turn and search
break;
-
-
-
-
-
-
- /*case 3: // turn left 120 deg
- if((lower<0.45f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
- if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
- stop_turn();
- t8.reset();
- t8.start();
- search_state = 9; //brick found
- printf("Brick found lower: %f upper:%f",lower,upper);
- }
- } else {
- search_state = 3; // go to same state
- if(turn_for_deg(0,1) < 0) {
- stop_turn();
- search_state = 20; // error
- }
- }
- break;
-
- case 5: // turn back left
- turn_for_deg(-10.0f,0.5f);
- search_state = 8;
- break;
-
- case 8: // turn back left continue
- if((lower<0.45f)&&(lower>0.1f)) { // if something is in the range of 10 to 75cm at the lower Sensor
- if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
- stop_turn();
- t8.reset();
- t8.start();
- search_state = 9; //brick found
- }
- } else {
- search_state = 8; // go to same state
- if(turn_for_deg(0,1) < 0) {
- stop_turn();
- search_state = 0; // error
- }
- }
- break;
-
- case 9:// Wait for enough Measurements for Distance
- // printf("timer: %f\r\n", t8.read());
- if(t8.read() > 0.5f) {
- search_state = 10;
- t8.stop();
- if(lower > 0.45f) {
- search_state = 0;
- }
- }
- break;*/
-
case 10: // first cycle move forward
float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
move_for_distance(distance_to_Brick);