Tobias Berger
Tobis Programm forked to not destroy your golden files
Fork of
Robocode
by 
PES 2 - Gruppe 1
Diff: source/Movement.cpp
- Revision:
- 124:6ef4d9ed2e28
- Parent:
- 123:de12131ff0b9
diff -r de12131ff0b9 -r 6ef4d9ed2e28 source/Movement.cpp
--- a/source/Movement.cpp Sun May 14 09:34:37 2017 +0000
+++ b/source/Movement.cpp Tue May 16 17:30:40 2017 +0000
@@ -24,9 +24,10 @@
Timer t;
Timer t8; // timer used for waiting enough distance measurements
-int search_state = 0;
+int search_state = 0; // state for Move in search for brick statemachine
int coord_move_state = 0;
int list_step = 0;
+int sigma_brick = 0; // Count how many times a brick is detected in fine postioning in statemachine move in search for brick
float left = 0;
float right = 0;
@@ -298,7 +299,7 @@
float x = next_coord.x - current_coordinates.x;
float y = next_coord.y - current_coordinates.y;
distance_to_next_coord = sqrt(x*x + y*y);
-
+
needed_heading = 90 + (atan(-y / x)/(float)M_PI * 180.0f)*-1.0f;
if (x < 0) needed_heading += 180;
@@ -307,7 +308,7 @@
} else {
coord_move_state = 8;
}
- break;
+ break;
case 5:
// turn init with new heading
@@ -342,9 +343,11 @@
/**
* this function searchs a nearby brick, moves towards it and grabbs it
* by Tobias Berger, state machine by Claudio Citterio
+ * returns 1 if brick found or 0 if nothing found
**/
int move_in_search_for_brick()
{
+
float upper = getDistanceIR(2); // get distance from upper max Sensor
float lower = getDistanceIR(3); // get distance from Lower max Sensor
//printf("Current Search State: >%d<\r\n",search_state);
@@ -358,37 +361,21 @@
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
+ 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 = 30; // go in Fine Positioning statemachine
printf("Brick first detetection lower: %f upper:%f",lower,upper);
}
} else {
- search_state = 1; // go to same state
+ search_state = 1; // go to same state continue turning
if(turn_for_deg(0, 1) < 0) { // when first 60degree rotation finished
stop_turn();
- search_state = 4; // go to init turn other direction
+ search_state = 4; // go to init turn other direction bc nothing found
}
}
break;
-
- case 2: // Check if Sensor after waiting still the same value
- 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
+ case 2: // init continue turning for restdeg
turn_for_deg(restdegAfterstop,0.8f); // call function and start turning for restdegrees after stop
search_state = 1; // go back to turn and search
break;
@@ -402,45 +389,123 @@
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
+ restdegAfterstop = (-1*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 = 30; // goto fine postioning stateamchine
printf("Brick first detetection lower: %f upper:%f",lower,upper);
}
} else {
search_state = 5; // go to same state
if(turn_for_deg(0, 1) < 0) { // when 60degree rotation finished
stop_turn();
- search_state = 20; // error go to default state, bc nothing found
+ search_state = 100; // error go to default state, bc nothing found
}
}
break;
- 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
- }
+ case 6:// init continue turning for restdeg
+ turn_for_deg((-1*restdegAfterstop),0.8f); // call function and start turning for restdegrees after stop
+ search_state = 5; // go back to turn and search
+ break;
+
+//-finepositioning-------------------------------------------------------------------------------------------------------------------------------------------------------------
+ case 30: // Start Statemachine fine positioning Init turn right for 3 degres
+ turn_for_deg(0.5f,0.8f);
+ search_state=31;
+ break;
+
+ case 31: // Wait until rotation finished
+ if(turn_for_deg(0,0.8f)<0) {
+ stop_turn();
+ search_state=32;
+ } else {
+ search_state=31; // As long as turn not finisched call case 1 wait until finished
+ }
+ break;
+
+
+ case 32: // Measure if Brick or Obstacle
+ 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
+ sigma_brick+=1; // increment sigma brick
+
+ }
+ }
+ search_state=33;
+ break;
+
+ case 33: // Init turn left direction for 3 degres
+ turn_for_deg(-1.0f,0.8f);
+ search_state=34;
+ break;
+
+ case 34: // Wait until rotation left finished
+ if(turn_for_deg(0,0.8f)<0) {
+ stop_turn();
+ printf("turn left after STOP");
+ search_state=35;
+ } else {
+ search_state=34; // As long as turn not finisched call case 1 wait until finished
+ }
+ break;
+
+ case 35: // Measure if Brick or Obstacle
+ 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
+ sigma_brick+=1; // increment sigma brick
+
+ }
+ }
+ search_state=36;
+ break;
+
+
+ case 36: // Init Turn Back right in Middle Position
+ turn_for_deg(0.5f,0.8f);
+ search_state = 37;
+ break;
+
+ case 37: // Turn until rotation finished
+ if(turn_for_deg(0,0.8f)<0) {
+ stop_turn();
+ search_state=38;
+ } else {
+ search_state=37; // As long as turn not finisched call case 1 wait until finished
+ }
+
+ break;
+
+
+ case 38: // Evaluation
+ if(sigma_brick > 1) { // If a brick is detected more than once got to move
+ sigma_brick = 0; // reset sigma brick
+ search_state = 50; // go to move forward bc brick found
+ printf("brick found in finepositioning");
+
+ } else {
+ printf("Nothing found in fine positioning");
+ if(restdegAfterstop>0) {
+ sigma_brick = 0; // reset sigma brick
+ search_state = 2; // continue turning right bc nothing found
+ } else {
+ sigma_brick = 0; // reset sigma brick
+ search_state = 6; // continue turning left bc nothing found
}
}
break;
- case 7:// init continue turning for restdeg
- turn_for_deg(restdegAfterstop,0.8f); // call function and start turning for restdegrees after stop
- search_state = 5; // go back to turn and search
+printf("SIGMA BRICK ---------------------------%d",sigma_brick);
+
+
+
+// Move to found brick-----------------------------------------------------------------------------------------------
+
+ case 50: // first cycle move forward
+ float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
+ move_for_distance(distance_to_Brick);
+ search_state = 51;
break;
- case 10: // first cycle move forward
- float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR; // calculate
- move_for_distance(distance_to_Brick);
- search_state =11;
- break;
-
- case 11: // move forward
+ case 51: // move forward
if(move_for_distance(0) < 0) {
//Safety Function:
if (getDistanceIR(2)<0.08f) {
@@ -449,15 +514,17 @@
search_state = 0;
}
stop_move();
- search_state = 12;
+ search_state = 52;
}
break;
- case 12: // Grabbing
- return 50; //main state machine set as Grabbing
+ case 52: // Grabbing
+ return 50;
+ break; // main state machine set as Grabbing
default:
printf("default State - move in search for brick\r\n");
+ return 60; // error nothing found go to Aeschlimans state
// error
break;
}