Tobias Berger
Tobis Programm forked to not destroy your golden files
Fork of
Robocode
by 
PES 2 - Gruppe 1
Diff: source/Movement.cpp
- Revision:
- 135:f31d24150f5e
- Parent:
- 134:5c29654ce301
--- a/source/Movement.cpp Tue May 16 14:24:11 2017 +0000
+++ b/source/Movement.cpp Wed May 17 08:16:33 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;
@@ -156,7 +157,7 @@
**/
float move_for_distance(float distance)
{
- //printf("move for distance\r\n");
+ printf("move for distance\r\n");
if(distance != 0) {
is_moving = true;
@@ -177,7 +178,6 @@
devider = true;
t.reset();
t.start();
- printf("move for %f m\r\n", distance);
} else {
float speed_multiplier = 0.6f;
@@ -191,7 +191,7 @@
}
float speed_left = get_speed_left();
- //printf("speed left: %f\r\n", speed_left);
+ printf("speed left: %f\r\n", speed_left);
wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
t.reset();
@@ -201,7 +201,7 @@
t.stop();
}
}
- //printf("remaining distance to cover: %f\r\n", wanted_dist);
+ printf("remaining distance to cover: %f\r\n", wanted_dist);
return wanted_dist;
}
@@ -232,7 +232,6 @@
devider = true;
t.reset();
t.start();
- printf("turn %f deg\r\n", deg);
} else {
float speed_multiplier = 0.6f;
@@ -252,7 +251,7 @@
t.stop();
}
}
- //printf("remaining deg %f\r\n", wanted_deg);
+ printf("remaining deg %f\r\n", wanted_deg);
return (wanted_deg);
}
@@ -270,60 +269,50 @@
coord_move_state = 1;
break;
-
-
case 1:
// get positions, coords, heading and distances
current_head = get_current_heading();
current_coordinates = get_current_coord();
position next_position = walkpath[list_step];
- position current_pos = get_current_pos();
next_coord = pos_to_coord(next_position);
- printf("\r\nlist step: %d\r\n", list_step);
- printf("current coordinates: %f || %f\r\n", current_coordinates.x, current_coordinates.y);
- printf("next coordinate: %f || %f\r\n", next_coord.x, next_coord.y);
- printf("current position: %d || %d\r\n", current_pos.x, current_pos.y);
- printf("next position: %d || %d\r\n", next_position.x, next_position.y);
-
+
+ coord_move_state = 2;
+ break;
+ case 2:
// check if path is still possible with updated map or target reached
- printf("checking\r\n");
- if(next_position.x < 1.0f && next_position.y < 1.0f) {
- printf("target reached\r\n");
+
+ if(next_position.x == 0 && next_position.y == 0) {
+ // target reached
coord_move_state = 0;
return 47;
}
if(obstacle_list[next_position.x][next_position.y] != 0) {
- printf("path obstructed\r\n");
+ // path obstructed
coord_move_state = 0;
return 35;
}
list_step += 1;
coord_move_state = 3;
break;
-
-
case 3:
- // calc new heading and distance
- float x = (next_coord.x - current_coordinates.x) / 100.0f; // distance to cover in x direction [in m]
- float y = (next_coord.y - current_coordinates.y) / 100.0f; // distance to cover in y direction [in m]
+ // calc new headings
+ 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;
- printf("current heading %f\r\n", current_head);
- printf("needed heading %f\r\n", needed_heading);
+
if(needed_heading != current_head) {
coord_move_state = 5;
} else {
coord_move_state = 8;
}
- break;
+ break;
case 5:
// turn init with new heading
- float turn_deg = needed_heading-current_head;
- if( turn_deg > 180.0f) turn_deg -= 360.0f;
- turn_for_deg(turn_deg,1);
+ turn_for_deg(needed_heading-current_head,1);
coord_move_state = 6;
break;
case 6:
@@ -354,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);
@@ -370,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;
@@ -414,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) {
@@ -461,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;
}
@@ -483,3 +538,10 @@
+
+
+
+
+
+
+