Tobias Berger
Tobis Programm forked to not destroy your golden files
Fork of
Robocode
by 
PES 2 - Gruppe 1
Diff: source/Movement.cpp
- Revision:
- 114:720dc5df42a5
- Parent:
- 113:c7afe49752b9
- Child:
- 116:e03a3692cdf0
--- a/source/Movement.cpp Tue May 09 13:58:57 2017 +0000
+++ b/source/Movement.cpp Wed May 10 07:45:33 2017 +0000
@@ -1,10 +1,10 @@
-/**
+/*/**
* Movement function library
* Handels Movement of the Robot
**/
#include "Movement.h"
-#define OFFSET_GREIFER_TO_IRSENSOR 0.15 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
+#define OFFSET_GREIFER_TO_IRSENSOR 0.2 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
#define OFFSET_WHEELS 0.09 // Offset of the wheels from the max pos
bool is_moving = false;
@@ -12,9 +12,13 @@
bool is_turning = false;
float wanted_deg = 0;
bool direction = false;
+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
Timer t;
-Timer t8;
+Timer t8; // timer used for waiting enough distance measurements
int search_state = 0;
@@ -200,7 +204,7 @@
* if deg not 0: turn initilisation.
* Claudio Citterio
**/
-float turn_for_deg(float deg)
+float turn_for_deg(float deg, float multiplier)
{
if(deg != 0) {
@@ -210,12 +214,12 @@
if(deg < 0) { // turn left
direction = 1;
- left = -20.0f;
- right = 20.0f;
+ left = -20.0f*multiplier;
+ right = 20.0f*multiplier;
} else { // turn right
direction = 0;
- left = 20.0f;
- right = -20.0f;
+ left = 20.0f*multiplier;
+ right = -20.0f*multiplier;
}
set_speed(left, right);
devider = true;
@@ -285,7 +289,7 @@
}
if(needed_heading != current_heading) {
- turn_for_deg((needed_heading-current_heading));
+ turn_for_deg(needed_heading-current_heading,1.0f);
} else {
move_for_distance(distance);
}
@@ -303,83 +307,145 @@
//printf("Current Search State: >%d<\r\n",search_state);
switch (search_state) {
case 0: //first cycle right
- turn_for_deg(60.0f); // call function and start turning
+ turn_for_deg(60.0f,0.2f); // call function and start turning
search_state = 1;
break;
- case 1: // turn right 60 deg
- 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))>0.05f) { // and nothing is detected with the upper Sensor
+ 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(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) { // and nothing is detected with the upper Sensor
stop_turn();
- search_state = 5; //brick found
- printf("Brick found lower: %f upper:%f",lower,upper);
+ 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
+ printf("Brick first detetection lower: %f upper:%f",lower,upper);
}
} else {
search_state = 1; // go to same state
- if(turn_for_deg(0) < 0) {
+ if(turn_for_deg(0, 1) < 0) { // when first 60degree rotation finished
stop_turn();
- search_state = 2;
+ search_state = 4; // go to init turn other direction
+ }
+ }
+ break;
+
+
+
+ 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
+ }
+ }
+ break;
+
+
+ case 3: // init continue turning for restdeg
+ turn_for_deg(restdegAfterstop,0.2f); // 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);
+ search_state = 5;
+ break;
+
+ case 5: // turn and search opposite direction
+ 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.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
+ 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
}
}
break;
- case 2: // first cycle left
- turn_for_deg(-120.0f);
- search_state = 3;
- 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))>0.02f) { // 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) < 0) {
- stop_turn();
- search_state = 20; // error
+ 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
}
}
break;
- case 5: // turn back left
- turn_for_deg(-10.0f);
- search_state = 8;
+ case 7:// init continue turning for restdeg
+ turn_for_deg(restdegAfterstop,0.2f); // call function and start turning for restdegrees after stop
+ search_state = 5; // go back to turn and search
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))>0.05f) { // 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) < 0) {
- stop_turn();
- search_state = 0; // error
+
+
+
+
+
+
+ /*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;
+ break;
+
+ case 5: // turn back left
+ turn_for_deg(-10.0f,0.5f);
+ search_state = 8;
+ break;
- case 9:
- // printf("timer: %f\r\n", t8.read());
- if(t8.read() > 2.0f) {
- search_state = 10;
- t8.stop();
- if(lower > 0.45f){
- search_state = 0;
+ 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;
+ break;
- case 10: // first cycle move
+ 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);
search_state =11;